fix(obstacle stop/slow_down): early return without point cloud (#10289)

* fix(obstacle stop/slow_down): early return without point cloud

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update maintainer

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
   <maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
+  <maintainer email="arjun.ram@tier4.jp">Arjun Ram</maintainer>
 
   <license>Apache License 2.0</license>
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

@@ -454,6 +454,10 @@ std::vector<SlowDownObstacle> ObstacleSlowDownModule::filter_slow_down_obstacle_
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
+  if (!obstacle_filtering_param_.use_pointcloud) {
+    return std::vector<SlowDownObstacle>{};
+  }
+
   // calculate collision points with trajectory with lateral stop margin
   // NOTE: For additional margin, hysteresis is not divided by two.
   const auto & p = obstacle_filtering_param_;
@@ -480,11 +484,7 @@ std::vector<SlowDownObstacle> ObstacleSlowDownModule::filter_slow_down_obstacle_
     const auto slow_down_obstacle = create_slow_down_obstacle_for_point_cloud(
       rclcpp::Time(point_cloud.pointcloud.header.stamp), front_collision_point,
       back_collision_point, slow_down_point_data.lat_dist_to_traj);
-
-    if (slow_down_obstacle) {
-      slow_down_obstacles.push_back(*slow_down_obstacle);
-      continue;
-    }
+    slow_down_obstacles.push_back(slow_down_obstacle);
   }
 
   RCLCPP_DEBUG(
@@ -630,13 +630,10 @@ ObstacleSlowDownModule::create_slow_down_obstacle_for_predicted_object(
     back_collision_point};
 }
 
-std::optional<SlowDownObstacle> ObstacleSlowDownModule::create_slow_down_obstacle_for_point_cloud(
+SlowDownObstacle ObstacleSlowDownModule::create_slow_down_obstacle_for_point_cloud(
   const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
   const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj)
 {
-  if (!obstacle_filtering_param_.use_pointcloud) {
-    return std::nullopt;
-  }
   const unique_identifier_msgs::msg::UUID obj_uuid;
   const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid);
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp

@@ -111,7 +111,7 @@ class ObstacleSlowDownModule : public PluginModuleInterface
     const std::vector<Polygon2d> & decimated_traj_polys_with_lat_margin,
     const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
     const double dist_from_obj_poly_to_traj_poly);
-  std::optional<SlowDownObstacle> create_slow_down_obstacle_for_point_cloud(
+  SlowDownObstacle create_slow_down_obstacle_for_point_cloud(
     const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
     const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj);
   std::vector<SlowdownInterval> plan_slow_down(

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml

@@ -8,6 +8,7 @@
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
   <maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
+  <maintainer email="arjun.ram@tier4.jp">Arjun Ram</maintainer>
   <maintainer email="berkay@leodrive.ai">Berkay Karaman</maintainer>
 
   <license>Apache License 2.0</license>

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

@@ -149,8 +149,6 @@ void ObstacleStopModule::init(rclcpp::Node & node, const std::string & module_na
   common_param_ = CommonParam(node);
   stop_planning_param_ = StopPlanningParam(node, common_param_);
   obstacle_filtering_param_ = ObstacleFilteringParam(node);
-  use_pointcloud_ =
-    get_or_declare_parameter<bool>(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud");
 
   // common publisher
   processing_time_publisher_ =
@@ -332,14 +330,10 @@ std::vector<geometry_msgs::msg::Point> ObstacleStopModule::convert_point_cloud_t
   return stop_collision_points;
 }
 
-std::optional<StopObstacle> ObstacleStopModule::create_stop_obstacle_for_point_cloud(
+StopObstacle ObstacleStopModule::create_stop_obstacle_for_point_cloud(
   const std::vector<TrajectoryPoint> & traj_points, const rclcpp::Time & stamp,
   const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const
 {
-  if (!use_pointcloud_) {
-    return std::nullopt;
-  }
-
   const auto dist_to_collide_on_traj =
     autoware::motion_utils::calcSignedArcLength(traj_points, 0, stop_point) - dist_to_bumper;
 
@@ -448,6 +442,10 @@ std::vector<StopObstacle> ObstacleStopModule::filter_stop_obstacle_for_point_clo
 {
   autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
+  if (!obstacle_filtering_param_.use_pointcloud) {
+    return std::vector<StopObstacle>{};
+  }
+
   const auto & tp = trajectory_polygon_collision_check;
 
   const std::vector<geometry_msgs::msg::Point> stop_points =
@@ -468,10 +466,7 @@ std::vector<StopObstacle> ObstacleStopModule::filter_stop_obstacle_for_point_clo
     // Filter obstacles for stop
     const auto stop_obstacle = create_stop_obstacle_for_point_cloud(
       decimated_traj_points, stop_obstacle_stamp, stop_point, dist_to_bumper);
-    if (stop_obstacle) {
-      stop_obstacles.push_back(*stop_obstacle);
-      continue;
-    }
+    stop_obstacles.push_back(stop_obstacle);
   }
 
   std::vector<StopObstacle> past_stop_obstacles;

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp

@@ -68,7 +68,6 @@ class ObstacleStopModule : public PluginModuleInterface
   // ros parameters
   bool ignore_crossing_obstacle_{};
   bool suppress_sudden_stop_{};
-  bool use_pointcloud_{false};
   CommonParam common_param_{};
   StopPlanningParam stop_planning_param_{};
   ObstacleFilteringParam obstacle_filtering_param_{};
@@ -187,7 +186,7 @@ class ObstacleStopModule : public PluginModuleInterface
     const VehicleInfo & vehicle_info, const double dist_to_bumper,
     const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const;
 
-  std::optional<StopObstacle> create_stop_obstacle_for_point_cloud(
+  StopObstacle create_stop_obstacle_for_point_cloud(
     const std::vector<TrajectoryPoint> & traj_points, const rclcpp::Time & stamp,
     const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const;
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp

@@ -120,8 +120,6 @@ struct ObstacleFilteringParam
       utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.inside.");
     outside_stop_object_types =
       utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.outside.");
-    use_pointcloud = get_or_declare_parameter<bool>(
-      node, "obstacle_stop.obstacle_filtering.object_type.pointcloud");
 
     obstacle_velocity_threshold_to_stop = get_or_declare_parameter<double>(
       node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop");