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..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.*` 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 214f7bb35a155..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 @@ -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 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..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 @@ -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_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); 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..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 @@ -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 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_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 54538343d19d1..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,6 +64,7 @@ struct ObstacleFilteringParam { std::vector inside_object_types{}; std::vector outside_object_types{}; + std::vector 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( // node, "obstacle_cruise.obstacle_filtering.object_type.pointcloud");