From 999241ebab0758ce49587e472044b3c0f227cd2d Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Sat, 3 Feb 2024 10:48:01 +0900 Subject: [PATCH] ignore occlusions only if the traffic light is red Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 2 +- .../behavior_velocity_crosswalk_module/src/manager.cpp | 4 ++-- .../src/occluded_crosswalk.cpp | 10 ---------- .../src/occluded_crosswalk.hpp | 7 ------- .../src/scene_crosswalk.cpp | 7 ++++--- .../src/scene_crosswalk.hpp | 2 +- 6 files changed, 8 insertions(+), 24 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 324b9ad502c18..7a54a75675c94 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -79,7 +79,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_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_with_red_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/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 015796345200e..528fe7691995d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -137,8 +137,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_traffic_light = - getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); + cp.occlusion_ignore_with_red_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_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/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 5d0ecd810d845..2455b36d5883f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -51,16 +51,6 @@ lanelet::BasicPoint2d interpolate_point( return segment.second + extra_distance * direction_vector; } -bool is_crosswalk_ignored( - const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light) -{ - const auto traffic_lights_reg_elems = - crosswalk_lanelet.regulatoryElementsAs(); - const bool has_traffic_light = !traffic_lights_reg_elems.empty(); - const bool has_skip_attribute = crosswalk_lanelet.hasAttribute("skip_occluded_slowdown"); - return (ignore_with_traffic_light && has_traffic_light) || has_skip_attribute; -} - std::vector calculate_detection_areas( const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, const double detection_range) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 3005263c23f43..a76fdeb549b88 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -49,13 +49,6 @@ bool is_occluded( lanelet::BasicPoint2d interpolate_point( const lanelet::BasicSegment2d & segment, const double extra_distance); -/// @brief check if the crosswalk should be ignored -/// @param crosswalk_lanelet lanelet of the crosswalk -/// @param ignore_with_traffic_light if true, ignore the crosswalk if it has a traffic light -/// @return true if the crosswalk should be ignored -bool is_crosswalk_ignored( - const lanelet::ConstLanelet & crosswalk_lanelet, const bool ignore_with_traffic_light); - /// @brief check if the crosswalk is occluded /// @param crosswalk_lanelet lanelet of the crosswalk /// @param occupancy_grid occupancy grid with the occlusion information diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 25b2aa3eb3220..0784c174a2d93 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -239,9 +239,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); }; - if ( - planner_param_.occlusion_enable && !path_intersects.empty() && - !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { + const auto is_crosswalk_ignored = + (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) || + crosswalk_.hasAttribute("skip_occluded_slowdown"); + if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); const auto detection_range = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 34d571e44b420..3259d50e6604a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -163,7 +163,7 @@ class CrosswalkModule : public SceneModuleInterface double occlusion_min_size; int occlusion_free_space_max; int occlusion_occupied_min; - bool occlusion_ignore_with_traffic_light; + bool occlusion_ignore_with_red_traffic_light; bool occlusion_ignore_behind_predicted_objects; std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size;