feat(motion_velocity_planner): prepare for motion_velocity_<stop/slow…

…_down/cruise>_module

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

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -97,19 +97,21 @@ void DynamicObstacleStopModule::update_parameters(const std::vector<rclcpp::Para
 }
 
 VelocityPlanningResult DynamicObstacleStopModule::plan(
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+  [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
+    raw_trajectory_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
   const std::shared_ptr<const PlannerData> planner_data)
 {
   VelocityPlanningResult result;
   debug_data_.reset_data();
-  if (ego_trajectory_points.size() < 2) return result;
+  if (smoothed_trajectory_points.size() < 2) return result;
 
   autoware::universe_utils::StopWatch<std::chrono::microseconds> stopwatch;
   stopwatch.tic();
   stopwatch.tic("preprocessing");
   dynamic_obstacle_stop::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  ego_data.trajectory = ego_trajectory_points;
+  ego_data.trajectory = smoothed_trajectory_points;
   autoware::motion_utils::removeOverlapPoints(ego_data.trajectory);
   ego_data.first_trajectory_idx =
     autoware::motion_utils::findNearestSegmentIndex(ego_data.trajectory, ego_data.pose.position);
@@ -164,7 +166,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
     if (stop_pose) {
       result.stop_points.push_back(stop_pose->position);
       planning_factor_interface_->add(
-        ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
+        smoothed_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP,
         SafetyFactorArray{});
       create_virtual_walls();
     }

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp

@@ -36,7 +36,8 @@ class DynamicObstacleStopModule : public PluginModuleInterface
   void publish_planning_factor() override { planning_factor_interface_->publish(); };
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
   VelocityPlanningResult plan(
-    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) override;
   std::string get_module_name() const override { return module_name_; }
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -96,12 +96,12 @@ bool is_unavoidable(
 };
 
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis)
 {
   std::vector<autoware_perception_msgs::msg::PredictedObject> filtered_objects;
   for (const auto & object : objects) {
-    const auto & predicted_object = object.predicted_object;
+    const auto & predicted_object = object->predicted_object;
     const auto is_not_too_slow =
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >=
       params.minimum_object_velocity;

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp

@@ -76,7 +76,7 @@ bool is_unavoidable(
 /// @param hysteresis [m] extra distance threshold used for filtering
 /// @return filtered predicted objects
 std::vector<autoware_perception_msgs::msg::PredictedObject> filter_predicted_objects(
-  const std::vector<PlannerData::Object> & objects, const EgoData & ego_data,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const EgoData & ego_data,
   const PlannerParam & params, const double hysteresis);
 
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/types.hpp

@@ -53,23 +53,23 @@ struct PlannerParam
 
 struct EgoData
 {
-  TrajectoryPoints trajectory;
+  TrajectoryPoints trajectory{};
   size_t first_trajectory_idx{};
-  double longitudinal_offset_to_first_trajectory_idx;  // [m]
-  geometry_msgs::msg::Pose pose;
-  autoware::universe_utils::MultiPolygon2d trajectory_footprints;
-  Rtree rtree;
-  std::optional<geometry_msgs::msg::Pose> earliest_stop_pose;
+  double longitudinal_offset_to_first_trajectory_idx{};  // [m]
+  geometry_msgs::msg::Pose pose{};
+  autoware::universe_utils::MultiPolygon2d trajectory_footprints{};
+  Rtree rtree{};
+  std::optional<geometry_msgs::msg::Pose> earliest_stop_pose{};
 };
 
 /// @brief debug data
 struct DebugData
 {
-  autoware::universe_utils::MultiPolygon2d obstacle_footprints;
+  autoware::universe_utils::MultiPolygon2d obstacle_footprints{};
   size_t prev_dynamic_obstacles_nb{};
-  autoware::universe_utils::MultiPolygon2d ego_footprints;
+  autoware::universe_utils::MultiPolygon2d ego_footprints{};
   size_t prev_ego_footprints_nb{};
-  std::optional<geometry_msgs::msg::Pose> stop_pose;
+  std::optional<geometry_msgs::msg::Pose> stop_pose{};
   size_t prev_collisions_nb{};
   double z{};
   void reset_data()
@@ -82,8 +82,8 @@ struct DebugData
 
 struct Collision
 {
-  geometry_msgs::msg::Point point;
-  std::string object_uuid;
+  geometry_msgs::msg::Point point{};
+  std::string object_uuid{};
 };
 }  // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop
 

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp

@@ -205,7 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable)
 TEST(TestObjectFiltering, filterPredictedObjects)
 {
   using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects;
-  std::vector<autoware::motion_velocity_planner::PlannerData::Object> objects;
+  std::vector<std::shared_ptr<autoware::motion_velocity_planner::PlannerData::Object>> objects;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data;
   autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params;
   double hysteresis{};

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp

@@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter
 {
 
 multi_polygon_t createPolygonMasks(
-  const std::vector<PlannerData::Object> & dynamic_obstacles, const double buffer,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & dynamic_obstacles, const double buffer,
   const double min_vel)
 {
   return createObjectPolygons(dynamic_obstacles, buffer, min_vel);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp

@@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b
 /// @param[in] min_vel minimum velocity for an object to be masked
 /// @return polygon masks around dynamic objects
 multi_polygon_t createPolygonMasks(
-  const std::vector<PlannerData::Object> & dynamic_obstacles, const double buffer,
+  const std::vector<std::shared_ptr<PlannerData::Object>> & dynamic_obstacles, const double buffer,
   const double min_vel);
 
 /// @brief create footprint polygons from projection lines

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -135,22 +135,24 @@ void ObstacleVelocityLimiterModule::update_parameters(
 }
 
 VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+  [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
+    raw_trajectory_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
   const std::shared_ptr<const PlannerData> planner_data)
 {
   autoware::universe_utils::StopWatch<std::chrono::microseconds> stopwatch;
   stopwatch.tic();
   VelocityPlanningResult result;
   stopwatch.tic("preprocessing");
   const auto ego_idx = autoware::motion_utils::findNearestIndex(
-    ego_trajectory_points, planner_data->current_odometry.pose.pose);
+    smoothed_trajectory_points, planner_data->current_odometry.pose.pose);
   if (!ego_idx) {
     RCLCPP_WARN_THROTTLE(
       logger_, *clock_, rcutils_duration_value_t(1000),
       "Cannot calculate ego index on the trajectory");
     return result;
   }
-  auto original_traj_points = ego_trajectory_points;
+  auto original_traj_points = smoothed_trajectory_points;
   if (preprocessing_params_.calculate_steering_angles)
     obstacle_velocity_limiter::calculateSteeringAngles(
       original_traj_points, projection_params_.wheel_base);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp

@@ -49,7 +49,8 @@ class ObstacleVelocityLimiterModule : public PluginModuleInterface
   void init(rclcpp::Node & node, const std::string & module_name) override;
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
   VelocityPlanningResult plan(
-    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) override;
   std::string get_module_name() const override { return module_name_; }
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp

@@ -59,11 +59,12 @@ polygon_t createObjectPolygon(
 }
 
 multi_polygon_t createObjectPolygons(
-  const std::vector<PlannerData::Object> & objects, const double buffer, const double min_velocity)
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const double buffer,
+  const double min_velocity)
 {
   multi_polygon_t polygons;
   for (const auto & object : objects) {
-    const auto & predicted_object = object.predicted_object;
+    const auto & predicted_object = object->predicted_object;
     const double obj_vel_norm = std::hypot(
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp

@@ -164,7 +164,8 @@ polygon_t createObjectPolygon(
 /// @param [in] min_velocity objects with velocity lower will be ignored
 /// @return polygons of the objects
 multi_polygon_t createObjectPolygons(
-  const std::vector<PlannerData::Object> & objects, const double buffer, const double min_velocity);
+  const std::vector<std::shared_ptr<PlannerData::Object>> & objects, const double buffer,
+  const double min_velocity);
 
 /// @brief add obstacles obtained from sensors to the given Obstacles object
 /// @param[out] obstacles Obstacles object in which to add the sensor obstacles

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp

@@ -394,7 +394,7 @@ TEST(TestCollisionDistance, createObjPolygons)
   using autoware_perception_msgs::msg::PredictedObject;
   using autoware_perception_msgs::msg::PredictedObjects;
 
-  std::vector<PlannerData::Object> objects;
+  std::vector<std::shared_ptr<PlannerData::Object>> objects;
 
   auto polygons = createObjectPolygons(objects, 0.0, 0.0);
   EXPECT_TRUE(polygons.empty());
@@ -407,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons)
   object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0;
   object1.shape.dimensions.x = 1.0;
   object1.shape.dimensions.y = 1.0;
-  objects.push_back(PlannerData::Object(object1));
+  objects.push_back(std::make_shared<PlannerData::Object>(object1));
 
   polygons = createObjectPolygons(objects, 0.0, 1.0);
   EXPECT_TRUE(polygons.empty());
@@ -434,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons)
   object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0;
   object2.shape.dimensions.x = 2.0;
   object2.shape.dimensions.y = 1.0;
-  objects.push_back(PlannerData::Object(object2));
+  objects.push_back(std::make_shared<PlannerData::Object>(object2));
 
   polygons = createObjectPolygons(objects, 0.0, 2.0);
   ASSERT_EQ(polygons.size(), 1ul);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -108,7 +108,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
   autoware_perception_msgs::msg::PredictedObjects filtered_objects;
   filtered_objects.header = planner_data.predicted_objects_header;
   for (const auto & object : planner_data.objects) {
-    const auto & predicted_object = object.predicted_object;
+    const auto & predicted_object = object->predicted_object;
     const auto is_pedestrian =
       std::find_if(
         predicted_object.classification.begin(), predicted_object.classification.end(),

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -148,22 +148,23 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
 
 void OutOfLaneModule::limit_trajectory_size(
   out_of_lane::EgoData & ego_data,
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
   const double max_arc_length)
 {
   ego_data.first_trajectory_idx =
-    motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
+    motion_utils::findNearestSegmentIndex(smoothed_trajectory_points, ego_data.pose.position);
   ego_data.longitudinal_offset_to_first_trajectory_index =
     motion_utils::calcLongitudinalOffsetToSegment(
-      ego_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position);
+      smoothed_trajectory_points, ego_data.first_trajectory_idx, ego_data.pose.position);
   auto l = -ego_data.longitudinal_offset_to_first_trajectory_index;
-  ego_data.trajectory_points.push_back(ego_trajectory_points[ego_data.first_trajectory_idx]);
-  for (auto i = ego_data.first_trajectory_idx + 1; i < ego_trajectory_points.size(); ++i) {
-    l += universe_utils::calcDistance2d(ego_trajectory_points[i - 1], ego_trajectory_points[i]);
+  ego_data.trajectory_points.push_back(smoothed_trajectory_points[ego_data.first_trajectory_idx]);
+  for (auto i = ego_data.first_trajectory_idx + 1; i < smoothed_trajectory_points.size(); ++i) {
+    l += universe_utils::calcDistance2d(
+      smoothed_trajectory_points[i - 1], smoothed_trajectory_points[i]);
     if (l >= max_arc_length) {
       break;
     }
-    ego_data.trajectory_points.push_back(ego_trajectory_points[i]);
+    ego_data.trajectory_points.push_back(smoothed_trajectory_points[i]);
   }
 }
 
@@ -213,7 +214,9 @@ out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData &
 }
 
 VelocityPlanningResult OutOfLaneModule::plan(
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+  [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
+    raw_trajectory_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
   const std::shared_ptr<const PlannerData> planner_data)
 {
   VelocityPlanningResult result;
@@ -223,7 +226,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
   stopwatch.tic("preprocessing");
   out_of_lane::EgoData ego_data;
   ego_data.pose = planner_data->current_odometry.pose.pose;
-  limit_trajectory_size(ego_data, ego_trajectory_points, params_.max_arc_length);
+  limit_trajectory_size(ego_data, smoothed_trajectory_points, params_.max_arc_length);
   out_of_lane::calculate_min_stop_and_slowdown_distances(
     ego_data, *planner_data, previous_slowdown_pose_);
   prepare_stop_lines_rtree(ego_data, *planner_data, params_.max_arc_length);
@@ -314,7 +317,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
     }
 
     planning_factor_interface_->add(
-      ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
+      smoothed_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN,
       SafetyFactorArray{});
     virtual_wall_marker_creator.add_virtual_walls(
       out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_));

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp

@@ -41,7 +41,8 @@ class OutOfLaneModule : public PluginModuleInterface
   void publish_planning_factor() override { planning_factor_interface_->publish(); };
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
   VelocityPlanningResult plan(
-    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const std::shared_ptr<const PlannerData> planner_data) override;
   std::string get_module_name() const override { return module_name_; }
 
@@ -51,7 +52,7 @@ class OutOfLaneModule : public PluginModuleInterface
   /// given length
   static void limit_trajectory_size(
     out_of_lane::EgoData & ego_data,
-    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
+    const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
     const double max_arc_length);
 
   out_of_lane::PlannerParam params_{};