perf(path_smoother): add ReplanChecker to decrease computation cost (a…

…utowarefoundation#4416)

* perf(path_smoother): add ReplanChecker to decrease computation cost

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

* Add replan checker parameters to default path_smoother config

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

---------

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/path_smoother/CMakeLists.txt

@@ -7,12 +7,7 @@ autoware_package()
 find_package(Eigen3 REQUIRED)
 
 ament_auto_add_library(path_smoother SHARED
-  # node
-  src/elastic_band_smoother.cpp
-  # algorithms
-  src/elastic_band.cpp
-  # utils
-  src/utils/trajectory_utils.cpp
+  DIRECTORY src
 )
 
 target_include_directories(path_smoother

planning/path_smoother/config/elastic_band_smoother.param.yaml

@@ -35,3 +35,13 @@
     # nearest search
     ego_nearest_dist_threshold: 3.0 # [m]
     ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]
+    # replanning & trimming trajectory param outside algorithm
+    replan:
+      enable: true  # if true, only perform smoothing when the input changes significantly
+      max_path_shape_around_ego_lat_dist: 2.0  # threshold of path shape change around ego [m]
+      max_path_shape_forward_lon_dist: 100.0   # forward point to check lateral distance difference [m]
+      max_path_shape_forward_lat_dist: 0.1     # threshold of path shape change around forward point [m]
+      max_ego_moving_dist: 5.0                 # threshold of ego's moving distance for replan [m]
+      # make max_goal_moving_dist long to keep start point fixed for pull over
+      max_goal_moving_dist: 15.0               # threshold of goal's moving distance for replan [m]
+      max_delta_time_sec: 1.0                  # threshold of delta time for replan [second]

planning/path_smoother/include/path_smoother/common_structs.hpp

@@ -30,15 +30,15 @@ struct Bounds;
 
 struct PlannerData
 {
-  // input
-  Header header;
-  std::vector<TrajectoryPoint> traj_points;  // converted from the input path
-  std::vector<geometry_msgs::msg::Point> left_bound;
-  std::vector<geometry_msgs::msg::Point> right_bound;
-
-  // ego
+  std::vector<TrajectoryPoint> traj_points;
   geometry_msgs::msg::Pose ego_pose;
   double ego_vel{};
+
+  PlannerData(
+    std::vector<TrajectoryPoint> traj_points_, geometry_msgs::msg::Pose ego_pose_, double ego_vel_)
+  : traj_points(traj_points_), ego_pose(ego_pose_), ego_vel(ego_vel_)
+  {
+  }
 };
 
 struct TimeKeeper

planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp

@@ -18,6 +18,7 @@
 #include "motion_utils/motion_utils.hpp"
 #include "path_smoother/common_structs.hpp"
 #include "path_smoother/elastic_band.hpp"
+#include "path_smoother/replan_checker.hpp"
 #include "path_smoother/type_alias.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
@@ -59,6 +60,7 @@ class ElasticBandSmoother : public rclcpp::Node
 
   // algorithms
   std::shared_ptr<EBPathSmoother> eb_path_smoother_ptr_{nullptr};
+  std::shared_ptr<ReplanChecker> replan_checker_ptr_{nullptr};
 
   // parameters
   CommonParam common_param_{};

planning/path_smoother/include/path_smoother/replan_checker.hpp

@@ -0,0 +1,71 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_
+#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_
+
+#include "path_smoother/common_structs.hpp"
+#include "path_smoother/type_alias.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+#include <vector>
+
+namespace path_smoother
+{
+class ReplanChecker
+{
+public:
+  explicit ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param);
+  void onParam(const std::vector<rclcpp::Parameter> & parameters);
+
+  bool isResetRequired(const PlannerData & planner_data) const;
+
+  bool isReplanRequired(const PlannerData & planner_data, const rclcpp::Time & current_time) const;
+
+  void updateData(
+    const PlannerData & planner_data, const bool is_replan_required,
+    const rclcpp::Time & current_time);
+
+private:
+  EgoNearestParam ego_nearest_param_;
+  rclcpp::Logger logger_;
+
+  // previous variables for isResetRequired
+  std::shared_ptr<std::vector<TrajectoryPoint>> prev_traj_points_ptr_{nullptr};
+  std::shared_ptr<geometry_msgs::msg::Pose> prev_ego_pose_ptr_{nullptr};
+
+  // previous variable for isReplanRequired
+  std::shared_ptr<rclcpp::Time> prev_replanned_time_ptr_{nullptr};
+
+  // algorithm parameters
+  bool enable_;
+  double max_path_shape_around_ego_lat_dist_;
+  double max_path_shape_forward_lat_dist_;
+  double max_path_shape_forward_lon_dist_;
+  double max_ego_moving_dist_;
+  double max_goal_moving_dist_;
+  double max_delta_time_sec_;
+
+  bool isPathAroundEgoChanged(
+    const PlannerData & planner_data, const std::vector<TrajectoryPoint> & prev_traj_points) const;
+  bool isPathForwardChanged(
+    const PlannerData & planner_data, const std::vector<TrajectoryPoint> & prev_traj_points) const;
+  bool isPathGoalChanged(
+    const PlannerData & planner_data, const std::vector<TrajectoryPoint> & prev_traj_points) const;
+};
+}  // namespace path_smoother
+
+#endif  // PATH_SMOOTHER__REPLAN_CHECKER_HPP_

planning/path_smoother/include/path_smoother/type_alias.hpp

@@ -20,8 +20,6 @@
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/point.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-#include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_msgs/msg/header.hpp"
 #include "tier4_debug_msgs/msg/string_stamped.hpp"

planning/path_smoother/src/elastic_band_smoother.cpp

@@ -87,6 +87,7 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option
 
   eb_path_smoother_ptr_ = std::make_shared<EBPathSmoother>(
     this, enable_debug_info_, ego_nearest_param_, common_param_, time_keeper_ptr_);
+  replan_checker_ptr_ = std::make_shared<ReplanChecker>(this, ego_nearest_param_);
 
   // reset planners
   initializePlanning();
@@ -109,6 +110,7 @@ rcl_interfaces::msg::SetParametersResult ElasticBandSmoother::onParam(
 
   // parameters for core algorithms
   eb_path_smoother_ptr_->onParam(parameters);
+  replan_checker_ptr_->onParam(parameters);
 
   // reset planners
   initializePlanning();
@@ -162,11 +164,29 @@ void ElasticBandSmoother::onPath(const Path::SharedPtr path_ptr)
   const auto input_traj_points = trajectory_utils::convertToTrajectoryPoints(path_ptr->points);
 
   // 1. calculate trajectory with Elastic Band
+  // 1.a check if replan (= optimization) is required
+  PlannerData planner_data(
+    input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x);
+  const bool is_replan_required = [&]() {
+    if (replan_checker_ptr_->isResetRequired(planner_data)) {
+      // NOTE: always replan when resetting previous optimization
+      resetPreviousData();
+      return true;
+    }
+    // check replan when not resetting previous optimization
+    return !prev_optimized_traj_points_ptr_ ||
+           replan_checker_ptr_->isReplanRequired(planner_data, now());
+  }();
+  replan_checker_ptr_->updateData(planner_data, is_replan_required, now());
   time_keeper_ptr_->tic(__func__);
-  auto smoothed_traj_points =
-    eb_path_smoother_ptr_->smoothTrajectory(input_traj_points, ego_state_ptr_->pose.pose);
+  auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory(
+                                                     input_traj_points, ego_state_ptr_->pose.pose)
+                                                 : *prev_optimized_traj_points_ptr_;
   time_keeper_ptr_->toc(__func__, "    ");
 
+  prev_optimized_traj_points_ptr_ =
+    std::make_shared<std::vector<TrajectoryPoint>>(smoothed_traj_points);
+
   // 2. update velocity
   applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose);