diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 675e8b55f3a05..d88bf5127f9d7 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -79,3 +79,4 @@ 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 diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 1dccef46d92f7..d95b56564c7c2 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -131,11 +131,13 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.occlusion_occluded_object_velocity = getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); cp.occlusion_slow_down_velocity = - getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); 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"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 95dbe771b5195..e3e38e27ba82c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -51,6 +51,16 @@ 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; +} + bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 43ae747e8326b..0b3c945d35834 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -48,6 +48,13 @@ 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 b5555c9242912..3a4f63c02f69a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -235,7 +235,9 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); } // Apply safety slow down speed if the crosswalk is occluded - if (planner_param_.occlusion_enable && !path_intersects.empty()) { + if ( + planner_param_.occlusion_enable && !path_intersects.empty() && + !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) { const auto dist_ego_to_crosswalk = calcSignedArcLength(path->points, ego_pos, path_intersects.front()); const auto detection_range = calculate_detection_range( @@ -256,9 +258,12 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto is_last_occlusion_within_time_buffer = most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <= planner_param_.occlusion_time_buffer; - if (is_last_occlusion_within_time_buffer) + if (is_last_occlusion_within_time_buffer) { + const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); applySafetySlowDownSpeed( - *path, path_intersects, planner_param_.occlusion_slow_down_velocity); + *path, path_intersects, + std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + } } } recordTime(2); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 88dd3bf94035e..9f9115de0441d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -158,11 +158,12 @@ class CrosswalkModule : public SceneModuleInterface // param for occlusions bool occlusion_enable; double occlusion_occluded_object_velocity; - double occlusion_slow_down_velocity; + float occlusion_slow_down_velocity; double occlusion_time_buffer; double occlusion_min_size; int occlusion_free_space_max; int occlusion_occupied_min; + bool occlusion_ignore_with_traffic_light; }; struct ObjectInfo