From 35dce3c6ae7035f3ba0664d2d0d4cb92e8efa815 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 3 Mar 2025 19:20:07 +0900 Subject: [PATCH 1/4] ignore not specified stopping objects Signed-off-by: Kento Yabuuchi --- .../src/obstacle_cruise_module.cpp | 18 ++++++++++++++---- .../src/obstacle_cruise_module.hpp | 5 +++-- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp index 56199900ed1d8..610313ee00346 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -517,8 +517,8 @@ std::optional> 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 ObstacleCruiseModule::create_yield_cruise_obstacle( - const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, - const std::vector & traj_points) + const std::shared_ptr object, + const std::shared_ptr stopped_object, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points) { if (traj_points.empty()) return std::nullopt; // check label @@ -719,6 +720,15 @@ std::optional ObstacleCruiseModule::create_yield_cruise_obstacle return std::nullopt; } + if (!is_outside_cruise_obstacle(stopped_object->predicted_object.classification.at(0).label)) { + RCLCPP_ERROR( + logger_, + "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object is not " + "outside cruise obstacle.", + 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, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp index cfb066c1d09df..d619c51118261 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -119,8 +119,9 @@ class ObstacleCruiseModule : public PluginModuleInterface const bool is_driving_forward, const VehicleInfo & vehicle_info, const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const; std::optional create_yield_cruise_obstacle( - const std::shared_ptr object, const rclcpp::Time & predicted_objects_stamp, - const std::vector & traj_points); + const std::shared_ptr object, + const std::shared_ptr stopped_object, + const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points); bool is_inside_cruise_obstacle(const uint8_t label) const; bool is_outside_cruise_obstacle(const uint8_t label) const; bool is_cruise_obstacle(const uint8_t label) const; From c802e3681d1c5f155e6e91632c5617486a6541db Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 5 Mar 2025 08:34:30 +0900 Subject: [PATCH 2/4] change debug print level Signed-off-by: Kento Yabuuchi --- .../src/obstacle_cruise_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp index 610313ee00346..73e8370d58920 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -721,7 +721,7 @@ std::optional ObstacleCruiseModule::create_yield_cruise_obstacle } if (!is_outside_cruise_obstacle(stopped_object->predicted_object.classification.at(0).label)) { - RCLCPP_ERROR( + RCLCPP_DEBUG( logger_, "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object is not " "outside cruise obstacle.", From 9708f1500caad22bffc428e8afcc8eee18d051b0 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 10 Mar 2025 14:05:49 +0900 Subject: [PATCH 3/4] add ahead_stopped prameter Signed-off-by: Kento Yabuuchi --- .../README.md | 2 +- .../config/obstacle_cruise.param.yaml | 11 +++++++++++ .../src/obstacle_cruise_module.cpp | 12 +++++++++--- .../src/obstacle_cruise_module.hpp | 1 + .../src/parameters.hpp | 3 +++ 5 files changed, 25 insertions(+), 4 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md index 296bb6290e398..4189a64b1cf68 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md +++ b/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.ahead_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. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml index 214f7bb35a155..413b44255bee2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml +++ b/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. + ahead_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 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp index 73e8370d58920..51fb0d9f981a3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -720,11 +720,11 @@ std::optional ObstacleCruiseModule::create_yield_cruise_obstacle return std::nullopt; } - if (!is_outside_cruise_obstacle(stopped_object->predicted_object.classification.at(0).label)) { + if (!is_ahead_stopped_obstacle(stopped_object->predicted_object.classification.at(0).label)) { RCLCPP_DEBUG( logger_, - "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object is not " - "outside cruise obstacle.", + "[Cruise] Ignore yield obstacle (%s) since the corresponding stopped object type is not " + "designated as ahead_stopped.", obj_uuid_str.substr(0, 4).c_str()); return std::nullopt; } @@ -789,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_ahead_stopped_obstacle(const uint8_t label) const +{ + const auto & types = obstacle_filtering_param_.ahead_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); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp index d619c51118261..bc303ecfe5901 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -124,6 +124,7 @@ class ObstacleCruiseModule : public PluginModuleInterface const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points); bool is_inside_cruise_obstacle(const uint8_t label) const; bool is_outside_cruise_obstacle(const uint8_t label) const; + bool is_ahead_stopped_obstacle(const uint8_t label) const; bool is_cruise_obstacle(const uint8_t label) const; bool is_front_collide_obstacle( const std::vector & traj_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp index 54538343d19d1..d274eb9455564 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp @@ -64,6 +64,7 @@ struct ObstacleFilteringParam { std::vector inside_object_types{}; std::vector outside_object_types{}; + std::vector ahead_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."); + ahead_stopped_object_types = utils::get_target_object_type( + node, "obstacle_cruise.obstacle_filtering.object_type.ahead_stopped."); // use_pointcloud = get_or_declare_parameter( // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud"); From 90f6046c84d113d2b438cafd371373cc76e56c6d Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 10 Mar 2025 14:39:16 +0900 Subject: [PATCH 4/4] rename ahead_stopped -> side_stopped Signed-off-by: Kento Yabuuchi --- .../README.md | 2 +- .../config/obstacle_cruise.param.yaml | 2 +- .../src/obstacle_cruise_module.cpp | 8 ++++---- .../src/obstacle_cruise_module.hpp | 2 +- .../src/parameters.hpp | 6 +++--- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md index 4189a64b1cf68..e382ba1ae71d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/README.md +++ b/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.ahead_stopped` 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. diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml index 413b44255bee2..ad79b9a11120d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/config/obstacle_cruise.param.yaml @@ -96,7 +96,7 @@ 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. - ahead_stopped: + side_stopped: unknown: false car: true truck: true diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp index 51fb0d9f981a3..bb2928cbb2631 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.cpp @@ -720,11 +720,11 @@ std::optional ObstacleCruiseModule::create_yield_cruise_obstacle return std::nullopt; } - if (!is_ahead_stopped_obstacle(stopped_object->predicted_object.classification.at(0).label)) { + 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 ahead_stopped.", + "designated as side_stopped.", obj_uuid_str.substr(0, 4).c_str()); return std::nullopt; } @@ -789,9 +789,9 @@ bool ObstacleCruiseModule::is_outside_cruise_obstacle(const uint8_t label) const return std::find(types.begin(), types.end(), label) != types.end(); } -bool ObstacleCruiseModule::is_ahead_stopped_obstacle(const uint8_t label) const +bool ObstacleCruiseModule::is_side_stopped_obstacle(const uint8_t label) const { - const auto & types = obstacle_filtering_param_.ahead_stopped_object_types; + const auto & types = obstacle_filtering_param_.side_stopped_object_types; return std::find(types.begin(), types.end(), label) != types.end(); } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp index bc303ecfe5901..7c2877c37bd92 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/obstacle_cruise_module.hpp @@ -124,7 +124,7 @@ class ObstacleCruiseModule : public PluginModuleInterface const rclcpp::Time & predicted_objects_stamp, const std::vector & traj_points); bool is_inside_cruise_obstacle(const uint8_t label) const; bool is_outside_cruise_obstacle(const uint8_t label) const; - bool is_ahead_stopped_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 & traj_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp index d274eb9455564..e4cac0801fbfe 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_cruise_module/src/parameters.hpp @@ -64,7 +64,7 @@ struct ObstacleFilteringParam { std::vector inside_object_types{}; std::vector outside_object_types{}; - std::vector ahead_stopped_object_types{}; + std::vector side_stopped_object_types{}; // bool use_pointcloud{}; double max_lat_margin{}; @@ -94,8 +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."); - ahead_stopped_object_types = utils::get_target_object_type( - node, "obstacle_cruise.obstacle_filtering.object_type.ahead_stopped."); + side_stopped_object_types = utils::get_target_object_type( + node, "obstacle_cruise.obstacle_filtering.object_type.side_stopped."); // use_pointcloud = get_or_declare_parameter( // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud");