From b1aab08ad799c618e9a8a32e076c5d285a824e90 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 18 Mar 2025 01:59:13 +0900 Subject: [PATCH 1/2] fix(obstacle stop/slow_down): early return without point cloud Signed-off-by: Takayuki Murooka --- .../src/obstacle_slow_down_module.cpp | 15 ++++++--------- .../src/obstacle_slow_down_module.hpp | 2 +- .../src/obstacle_stop_module.cpp | 17 ++++++----------- .../src/obstacle_stop_module.hpp | 3 +-- .../src/parameters.hpp | 2 -- 5 files changed, 14 insertions(+), 25 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp index 45bab49d3397b..69605f45a4dde 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp @@ -454,6 +454,10 @@ std::vector ObstacleSlowDownModule::filter_slow_down_obstacle_ { autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!obstacle_filtering_param_.use_pointcloud) { + return std::vector{}; + } + // 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 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 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); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp index 4be6b0165869a..6dabcb1131219 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp +++ b/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 & decimated_traj_polys_with_lat_margin, const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, const double dist_from_obj_poly_to_traj_poly); - std::optional 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 plan_slow_down( diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp index 96f13c6f30fdc..785cb4a2fa214 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp +++ b/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(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); // common publisher processing_time_publisher_ = @@ -332,14 +330,10 @@ std::vector ObstacleStopModule::convert_point_cloud_t return stop_collision_points; } -std::optional ObstacleStopModule::create_stop_obstacle_for_point_cloud( +StopObstacle ObstacleStopModule::create_stop_obstacle_for_point_cloud( const std::vector & 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 ObstacleStopModule::filter_stop_obstacle_for_point_clo { autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (!obstacle_filtering_param_.use_pointcloud) { + return std::vector{}; + } + const auto & tp = trajectory_polygon_collision_check; const std::vector stop_points = @@ -468,10 +466,7 @@ std::vector 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 past_stop_obstacles; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp index 9eb906dc3ad0d..02890c66b2b15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp +++ b/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 create_stop_obstacle_for_point_cloud( + StopObstacle create_stop_obstacle_for_point_cloud( const std::vector & traj_points, const rclcpp::Time & stamp, const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp index ce910287bb77a..23a58e859be64 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp +++ b/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( - node, "obstacle_stop.obstacle_filtering.object_type.pointcloud"); obstacle_velocity_threshold_to_stop = get_or_declare_parameter( node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop"); From 5d79a8e8b527f2a07c2f6e8bda4e761b39fe6aa0 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 18 Mar 2025 02:16:57 +0900 Subject: [PATCH 2/2] update maintainer Signed-off-by: Takayuki Murooka --- .../package.xml | 1 + .../autoware_motion_velocity_obstacle_stop_module/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml index 2475101311ba6..477b3e6018500 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml @@ -8,6 +8,7 @@ Takayuki Murooka Yuki Takagi Maxime Clement + Arjun Ram Apache License 2.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml index 8092ce71290f5..9638c4a244e4b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml @@ -8,6 +8,7 @@ Takayuki Murooka Yuki Takagi Maxime Clement + Arjun Ram Berkay Karaman Apache License 2.0