fix(obstacle_cruise_planner): ignore invalid stopping objects (#10227)

* ignore not specified stopping objects

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* change debug print level

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* add ahead_stopped prameter

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* rename ahead_stopped -> side_stopped

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

---------

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md

@@ -41,7 +41,7 @@ The obstacles meeting the following condition are determined as obstacles for yi
 
 - The object type is for cruising according to `obstacle_filtering.object_type.*` and it is moving with a speed greater than `obstacle_filtering.yield.stopped_obstacle_velocity_threshold`.
 - The object is not crossing the ego's trajectory (\*1).
-- There is another object of type `obstacle_filtering.object_type.*` stopped in front of the moving obstacle.
+- There is another object of type `obstacle_filtering.object_type.side_stopped` stopped in front of the moving obstacle.
 - The lateral distance (using the ego's trajectory as reference) between both obstacles is less than `obstacle_filtering.yield.max_lat_dist_between_obstacles`
 - Both obstacles, moving and stopped, are within `obstacle_filtering.yield.lat_distance_threshold` and `obstacle_filtering.yield.lat_distance_threshold` + `obstacle_filtering.yield.max_lat_dist_between_obstacles` lateral distance from the ego's trajectory respectively.
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml

@@ -95,6 +95,17 @@
             bicycle: false
             pedestrian: false
 
+          # If an object does not have this label, the ego vehicle does not assume that the yielded obstacle will cut in due to the object.
+          side_stopped:
+            unknown: false
+            car: true
+            truck: true
+            bus: true
+            trailer: true
+            motorcycle: true
+            bicycle: false
+            pedestrian: false
+
         max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width
 
         # if crossing vehicle is determined as target obstacles or not

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp

@@ -517,8 +517,8 @@ std::optional<std::vector<CruiseObstacle>> ObstacleCruiseModule::find_yield_crui
         longitudinal_distance_between_obstacles / moving_object_speed <
         obstacle_filtering_param_.max_obstacles_collision_time;
       if (are_obstacles_aligned && obstacles_collide_within_threshold_time) {
-        const auto yield_obstacle =
-          create_yield_cruise_obstacle(moving_object, predicted_objects_stamp, traj_points);
+        const auto yield_obstacle = create_yield_cruise_obstacle(
+          moving_object, stopped_object, predicted_objects_stamp, traj_points);
         if (yield_obstacle) {
           yield_obstacles.push_back(*yield_obstacle);
           using autoware::objects_of_interest_marker_interface::ColorName;
@@ -703,8 +703,9 @@ ObstacleCruiseModule::create_collision_points_for_outside_cruise_obstacle(
 }
 
 std::optional<CruiseObstacle> ObstacleCruiseModule::create_yield_cruise_obstacle(
-  const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
-  const std::vector<TrajectoryPoint> & traj_points)
+  const std::shared_ptr<PlannerData::Object> object,
+  const std::shared_ptr<PlannerData::Object> stopped_object,
+  const rclcpp::Time & predicted_objects_stamp, const std::vector<TrajectoryPoint> & traj_points)
 {
   if (traj_points.empty()) return std::nullopt;
   // check label
@@ -719,6 +720,15 @@ std::optional<CruiseObstacle> ObstacleCruiseModule::create_yield_cruise_obstacle
     return std::nullopt;
   }
 
+  if (!is_side_stopped_obstacle(stopped_object->predicted_object.classification.at(0).label)) {
+    RCLCPP_DEBUG(
+      logger_,
+      "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object type is not "
+      "designated as side_stopped.",
+      obj_uuid_str.substr(0, 4).c_str());
+    return std::nullopt;
+  }
+
   if (
     std::hypot(
       object->predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
@@ -779,6 +789,12 @@ bool ObstacleCruiseModule::is_outside_cruise_obstacle(const uint8_t label) const
   return std::find(types.begin(), types.end(), label) != types.end();
 }
 
+bool ObstacleCruiseModule::is_side_stopped_obstacle(const uint8_t label) const
+{
+  const auto & types = obstacle_filtering_param_.side_stopped_object_types;
+  return std::find(types.begin(), types.end(), label) != types.end();
+}
+
 bool ObstacleCruiseModule::is_cruise_obstacle(const uint8_t label) const
 {
   return is_inside_cruise_obstacle(label) || is_outside_cruise_obstacle(label);

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp

@@ -119,10 +119,12 @@ class ObstacleCruiseModule : public PluginModuleInterface
     const bool is_driving_forward, const VehicleInfo & vehicle_info,
     const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const;
   std::optional<CruiseObstacle> create_yield_cruise_obstacle(
-    const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
-    const std::vector<TrajectoryPoint> & traj_points);
+    const std::shared_ptr<PlannerData::Object> object,
+    const std::shared_ptr<PlannerData::Object> stopped_object,
+    const rclcpp::Time & predicted_objects_stamp, const std::vector<TrajectoryPoint> & traj_points);
   bool is_inside_cruise_obstacle(const uint8_t label) const;
   bool is_outside_cruise_obstacle(const uint8_t label) const;
+  bool is_side_stopped_obstacle(const uint8_t label) const;
   bool is_cruise_obstacle(const uint8_t label) const;
   bool is_front_collide_obstacle(
     const std::vector<TrajectoryPoint> & traj_points,

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp

@@ -64,6 +64,7 @@ struct ObstacleFilteringParam
 {
   std::vector<uint8_t> inside_object_types{};
   std::vector<uint8_t> outside_object_types{};
+  std::vector<uint8_t> side_stopped_object_types{};
   // bool use_pointcloud{};
 
   double max_lat_margin{};
@@ -93,6 +94,8 @@ struct ObstacleFilteringParam
       utils::get_target_object_type(node, "obstacle_cruise.obstacle_filtering.object_type.inside.");
     outside_object_types = utils::get_target_object_type(
       node, "obstacle_cruise.obstacle_filtering.object_type.outside.");
+    side_stopped_object_types = utils::get_target_object_type(
+      node, "obstacle_cruise.obstacle_filtering.object_type.side_stopped.");
     // use_pointcloud = get_or_declare_parameter<bool>(
     //   node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud");