From 9708f1500caad22bffc428e8afcc8eee18d051b0 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 10 Mar 2025 14:05:49 +0900 Subject: [PATCH] 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");