fix(static_centerline_optimizer): fix several bugs and refactor the c…

…ode (autowarefoundation#6022)

* fix(static_centerline_optimizer): fix several bugs and refactor the code

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

* fix

Signed-off-by: Takayuki Murooka <[email protected]>

---------

Signed-off-by: Takayuki Murooka <[email protected]>

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/mpt_optimizer.hpp

@@ -109,8 +109,7 @@ class MPTOptimizer
  const std::shared_ptr<DebugData> debug_data_ptr,
  const std::shared_ptr<TimeKeeper> time_keeper_ptr);
 
- std::vector<TrajectoryPoint> optimizeTrajectory(
- const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points);
+ std::vector<TrajectoryPoint> optimizeTrajectory(const PlannerData & planner_data);
  std::optional<std::vector<TrajectoryPoint>> getPrevOptimizedTrajectoryPoints() const;
 
  void initialize(const bool enable_debug_info, const TrajectoryParam & traj_param);

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -37,6 +37,11 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
 public:
  explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options);
 
+ // NOTE: This is for the static_centerline_optimizer package which utilizes the following
+ // instance.
+ std::shared_ptr<MPTOptimizer> getMPTOptimizer() const { return mpt_optimizer_ptr_; }
+
+ // private:
 protected: // for the static_centerline_optimizer package
  // TODO(murooka) move this node to common
  class DrivingDirectionChecker

planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp

@@ -468,8 +468,7 @@ void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
  debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num;
 }
 
-std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
- const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points)
+std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(const PlannerData & planner_data)
 {
  time_keeper_ptr_->tic(__func__);
 
@@ -480,11 +479,11 @@ std::vector<TrajectoryPoint> MPTOptimizer::optimizeTrajectory(
  if (prev_optimized_traj_points_ptr_) {
  return *prev_optimized_traj_points_ptr_;
  }
- return smoothed_points;
+ return traj_points;
  };
 
  // 1. calculate reference points
- auto ref_points = calcReferencePoints(planner_data, smoothed_points);
+ auto ref_points = calcReferencePoints(planner_data, traj_points);
  if (ref_points.size() < 2) {
  RCLCPP_INFO_EXPRESSION(
  logger_, enable_debug_info_, "return std::nullopt since ref_points size is less than 2.");

planning/obstacle_avoidance_planner/src/node.cpp

@@ -364,7 +364,7 @@ std::vector<TrajectoryPoint> ObstacleAvoidancePlanner::optimizeTrajectory(
 
  // 2. make trajectory kinematically-feasible and collision-free (= inside the drivable area)
  // with model predictive trajectory
- const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data, p.traj_points);
+ const auto mpt_traj = mpt_optimizer_ptr_->optimizeTrajectory(planner_data);
 
  time_keeper_ptr_->toc(__func__, " ");
  return mpt_traj;

planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp

@@ -36,7 +36,11 @@ class ElasticBandSmoother : public rclcpp::Node
 public:
  explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options);
 
-protected:
+ // NOTE: This is for the static_centerline_optimizer package which utilizes the following
+ // instance.
+ std::shared_ptr<EBPathSmoother> getElasticBandSmoother() const { return eb_path_smoother_ptr_; }
+
+private:
  class DrivingDirectionChecker
  {
  public:

planning/static_centerline_optimizer/CMakeLists.txt

@@ -24,7 +24,6 @@ autoware_package()
 ament_auto_add_executable(main
  src/main.cpp
  src/static_centerline_optimizer_node.cpp
- src/successive_trajectory_optimizer_node.cpp
  src/utils.cpp
 )
 

planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp

@@ -52,6 +52,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
 
  // plan path
  std::vector<TrajectoryPoint> plan_path(const std::vector<lanelet::Id> & route_lane_ids);
+ std::vector<TrajectoryPoint> optimize_trajectory(const Path & raw_path) const;
  void on_plan_path(
  const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response);
 
@@ -62,6 +63,7 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node
  const std::string & lanelet2_output_file_path, const std::vector<lanelet::Id> & route_lane_ids,
  const std::vector<TrajectoryPoint> & optimized_traj_points);
 
+ lanelet::LaneletMapPtr original_map_ptr_{nullptr};
  HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr};
  std::shared_ptr<RouteHandler> route_handler_ptr_{nullptr};
 

planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml

@@ -17,6 +17,11 @@
  <arg name="lanelet2_map_marker_topic" default="/map/vector_map_marker"/>
  <!-- param -->
  <arg name="map_loader_param" default="$(find-pkg-share map_loader)/config/lanelet2_map_loader.param.yaml"/>
+ <arg
+ name="behavior_path_planner_param"
+ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml"
+ />
+ <arg name="path_smoother_param" default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/path_smoother/elastic_band_smoother.param.yaml"/>
  <arg
  name="obstacle_avoidance_planner_param"
  default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml"
@@ -61,6 +66,8 @@
  <param from="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/common/nearest_search.param.yaml"/>
  <!-- component param -->
  <param from="$(var map_loader_param)"/>
+ <param from="$(var behavior_path_planner_param)"/>
+ <param from="$(var path_smoother_param)"/>
  <param from="$(var obstacle_avoidance_planner_param)"/>
  <param from="$(var mission_planner_param)"/>
  <!-- node param -->

planning/static_centerline_optimizer/package.xml

@@ -28,6 +28,7 @@
  <depend>motion_utils</depend>
  <depend>obstacle_avoidance_planner</depend>
  <depend>osqp_interface</depend>
+ <depend>path_smoother</depend>
  <depend>rclcpp</depend>
  <depend>rclcpp_components</depend>
  <depend>route_handler</depend>

planning/static_centerline_optimizer/scripts/tmp/run.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+ros2 launch static_centerline_optimizer static_centerline_optimizer.launch.xml run_background:=false lanelet2_input_file_path:="$HOME/AutonomousDrivingScenarios/map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus

planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp

@@ -18,9 +18,11 @@
 #include "lanelet2_extension/utility/query.hpp"
 #include "lanelet2_extension/utility/utilities.hpp"
 #include "map_loader/lanelet2_map_loader_node.hpp"
+#include "motion_utils/resample/resample.hpp"
 #include "motion_utils/trajectory/conversion.hpp"
+#include "obstacle_avoidance_planner/node.hpp"
+#include "path_smoother/elastic_band_smoother.hpp"
 #include "static_centerline_optimizer/msg/points_with_lane_id.hpp"
-#include "static_centerline_optimizer/successive_trajectory_optimizer_node.hpp"
 #include "static_centerline_optimizer/type_alias.hpp"
 #include "static_centerline_optimizer/utils.hpp"
 #include "tier4_autoware_utils/geometry/geometry.hpp"
@@ -62,6 +64,21 @@ Path convert_to_path(const PathWithLaneId & path_with_lane_id)
  return path;
 }
 
+Trajectory convert_to_trajectory(const Path & path)
+{
+ Trajectory traj;
+ for (const auto & point : path.points) {
+ TrajectoryPoint traj_point;
+ traj_point.pose = point.pose;
+ traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps;
+ traj_point.lateral_velocity_mps = point.lateral_velocity_mps;
+ traj_point.heading_rate_rps = point.heading_rate_rps;
+
+ traj.points.push_back(traj_point);
+ }
+ return traj;
+}
+
 [[maybe_unused]] lanelet::ConstLanelets get_lanelets_from_route(
  const RouteHandler & route_handler, const LaneletRoute & route)
 {
@@ -249,14 +266,22 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_
  // load map by the map_loader package
  map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr {
  // load map
- lanelet::LaneletMapPtr map_ptr;
  tier4_map_msgs::msg::MapProjectorInfo map_projector_info;
  map_projector_info.projector_type = tier4_map_msgs::msg::MapProjectorInfo::MGRS;
- map_ptr = Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info);
+ const auto map_ptr =
+ Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info);
  if (!map_ptr) {
  return nullptr;
  }
 
+ // NOTE: The original map is stored here since the various ids in the lanelet map will change
+ // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail.
+ original_map_ptr_ =
+ Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info);
+
+ // overwrite more dense centerline
+ lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false);
+
  // create map bin msg
  const auto map_bin_msg =
  Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now());
@@ -388,7 +413,7 @@ std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::plan_path(
  const auto start_center_pose =
  utils::get_center_pose(*route_handler_ptr_, route_lane_ids.front());
 
- // ego nearest search parameters
+ // get ego nearest search parameters and resample interval in behavior_path_planner
  const double ego_nearest_dist_threshold =
  has_parameter("ego_nearest_dist_threshold")
  ? get_parameter("ego_nearest_dist_threshold").as_double()
@@ -397,31 +422,105 @@ std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::plan_path(
  has_parameter("ego_nearest_yaw_threshold")
  ? get_parameter("ego_nearest_yaw_threshold").as_double()
  : declare_parameter<double>("ego_nearest_yaw_threshold");
+ const double behavior_path_interval = has_parameter("output_path_interval")
+ ? get_parameter("output_path_interval").as_double()
+ : declare_parameter<double>("output_path_interval");
 
  // extract path with lane id from lanelets
- const auto raw_path_with_lane_id = utils::get_path_with_lane_id(
- *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold,
- ego_nearest_yaw_threshold);
-
+ const auto raw_path_with_lane_id = [&]() {
+ const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id(
+ *route_handler_ptr_, route_lanelets, start_center_pose, ego_nearest_dist_threshold,
+ ego_nearest_yaw_threshold);
+ return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval);
+ }();
  pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id);
  RCLCPP_INFO(get_logger(), "Calculated raw path with lane id and published.");
 
  // convert path with lane id to path
- const auto raw_path = convert_to_path(raw_path_with_lane_id);
+ const auto raw_path = [&]() {
+ const auto non_resampled_path = convert_to_path(raw_path_with_lane_id);
+ // NOTE: The behavior_velocity_planner resamples with the interval 1.0 somewhere.
+ return motion_utils::resamplePath(non_resampled_path, 1.0);
+ }();
  pub_raw_path_->publish(raw_path);
  RCLCPP_INFO(get_logger(), "Converted to path and published.");
 
- // optimize trajectory by the obstacle_avoidance_planner package
- SuccessiveTrajectoryOptimizer successive_trajectory_optimizer(create_node_options());
- const auto optimized_traj = successive_trajectory_optimizer.on_centerline(raw_path);
- pub_optimized_centerline_->publish(optimized_traj);
- const auto optimized_traj_points = motion_utils::convertToTrajectoryPointArray(optimized_traj);
-
- RCLCPP_INFO(get_logger(), "Optimized trajectory and published.");
+ // smooth trajectory and road collision avoidance
+ const auto optimized_traj_points = optimize_trajectory(raw_path);
+ pub_optimized_centerline_->publish(
+ motion_utils::convertToTrajectory(optimized_traj_points, raw_path.header));
+ RCLCPP_INFO(
+ get_logger(), "Smoothed trajectory and made it collision free with the road and published.");
 
  return optimized_traj_points;
 }
 
+std::vector<TrajectoryPoint> StaticCenterlineOptimizerNode::optimize_trajectory(
+ const Path & raw_path) const
+{
+ // convert to trajectory points
+ const auto raw_traj_points = [&]() {
+ const auto raw_traj = convert_to_trajectory(raw_path);
+ return motion_utils::convertToTrajectoryPointArray(raw_traj);
+ }();
+
+ // create an instance of elastic band and model predictive trajectory.
+ const auto eb_path_smoother_ptr =
+ path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother();
+ const auto mpt_optimizer_ptr =
+ obstacle_avoidance_planner::ObstacleAvoidancePlanner(create_node_options()).getMPTOptimizer();
+
+ // NOTE: The optimization is executed every valid_optimized_traj_points_num points.
+ constexpr int valid_optimized_traj_points_num = 10;
+ const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num;
+
+ // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use
+ // warm start.
+ constexpr int num_initial_optimization = 2;
+
+ std::vector<TrajectoryPoint> whole_optimized_traj_points;
+ for (int virtual_ego_pose_idx = -num_initial_optimization;
+ virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) {
+ // calculate virtual ego pose for the optimization
+ constexpr int virtual_ego_pose_offset_idx = 1;
+ const auto virtual_ego_pose =
+ raw_traj_points
+ .at(
+ valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) +
+ virtual_ego_pose_offset_idx)
+ .pose;
+
+ // smooth trajectory by elastic band in the path_smoother package
+ const auto smoothed_traj_points =
+ eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose);
+
+ // road collision avoidance by model predictive trajectory in the obstacle_avoidance_planner
+ // package
+ const obstacle_avoidance_planner::PlannerData planner_data{
+ raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound,
+ virtual_ego_pose};
+ const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data);
+
+ // connect the previously and currently optimized trajectory points
+ for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) {
+ const double dist = tier4_autoware_utils::calcDistance2d(
+ whole_optimized_traj_points.at(j), optimized_traj_points.front());
+ if (dist < 0.5) {
+ const std::vector<TrajectoryPoint> extracted_whole_optimized_traj_points{
+ whole_optimized_traj_points.begin(),
+ whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1};
+ whole_optimized_traj_points = extracted_whole_optimized_traj_points;
+ break;
+ }
+ }
+ for (size_t j = 0; j < optimized_traj_points.size(); ++j) {
+ whole_optimized_traj_points.push_back(optimized_traj_points.at(j));
+ }
+ }
+
+ return whole_optimized_traj_points;
+}
+
 void StaticCenterlineOptimizerNode::on_plan_path(
  const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response)
 {
@@ -581,7 +680,7 @@ void StaticCenterlineOptimizerNode::save_map(
  RCLCPP_INFO(get_logger(), "Updated centerline in map.");
 
  // save map with modified center line
- lanelet::write(lanelet2_output_file_path, *route_handler_ptr_->getLaneletMapPtr());
+ lanelet::write(lanelet2_output_file_path, *original_map_ptr_);
  RCLCPP_INFO(get_logger(), "Saved map.");
 }
 } // namespace static_centerline_optimizer