diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 7d6acbfc6aa9a..b82ea7887dc98 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -209,7 +209,7 @@ This range is meant to be large when ego is far from the crosswalk and small whe In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter. -To ignore occlusions when the pedestrian light is red, `ignore_with_red_traffic_light` should be set to true. +To ignore occlusions when the crosswalk has a traffic light, `ignore_with_traffic_light` should be set to true. To ignore temporary occlusions caused by moving objects, `ignore_behind_predicted_objects` should be set to true. @@ -230,7 +230,7 @@ To inflate the masking behind objects, their footprint can be made bigger using | `min_size` | [m] | double | minimum size of an occlusion (square side size) | | `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | | `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | -| `ignore_with_red_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | | `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | | `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | | `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 09ce64593ff46..f8b9e5dbf1327 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -78,7 +78,7 @@ min_size: 0.5 # [m] minimum size of an occlusion (square side size) free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid - ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_with_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored ignore_velocity_thresholds: default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 77de7d241c05c..d287d7e705542 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -67,6 +67,7 @@ struct DebugData bool ignore_crosswalk{false}; double base_link2front; double stop_judge_range{}; + std::string virtual_wall_suffix{}; geometry_msgs::msg::Pose first_stop_pose; geometry_msgs::msg::Point nearest_collision_point; diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index d6a2a3cb185a1..4bee98db2fa56 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -190,6 +190,7 @@ motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() virtual_walls.push_back(wall); } wall.style = motion_utils::VirtualWallType::slowdown; + wall.text += debug_data_.virtual_wall_suffix; for (const auto & p : debug_data_.slow_poses) { wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 39f88d5389c2b..aeff5e27cea7b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -135,8 +135,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); - cp.occlusion_ignore_with_red_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_traffic_light"); + cp.occlusion_ignore_with_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index e4b9f346f4f6d..7caf8651d8b33 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -240,8 +240,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); }; + const auto crosswalk_has_traffic_light = + !crosswalk_.regulatoryElementsAs().empty(); const auto is_crosswalk_ignored = - (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) || + (planner_param_.occlusion_ignore_with_traffic_light && crosswalk_has_traffic_light) || crosswalk_.hasAttribute("skip_occluded_slowdown"); if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { const auto dist_ego_to_crosswalk = @@ -269,6 +271,7 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto applySafetySlowDownSpeed( *path, path_intersects, std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + debug_data_.virtual_wall_suffix = " (occluded)"; } else { most_recent_occlusion_time_.reset(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 5ce5d4020928a..d2468e7f31aa7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -162,7 +162,7 @@ class CrosswalkModule : public SceneModuleInterface double occlusion_min_size; int occlusion_free_space_max; int occlusion_occupied_min; - bool occlusion_ignore_with_red_traffic_light; + bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size;