diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 4b4ac123bac49..c028a0c5d9e9e 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -81,6 +81,8 @@ 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_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored - do_not_ignore_behind_pedestrians: true # [-] if true, occlusions behind pedestrians are not ignored - min_objects_velocity: 0.5 # [m/s] minimum velocity for a predicted object to be ignored + ignore_velocity_thresholds: + default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_thresholds: [0.0] # velocities of the custom labels extra_objects_size: 0.5 # [m] extra size added to the objects when masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 83d6c22e78730..015796345200e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -140,10 +141,29 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_with_traffic_light"); cp.occlusion_ignore_behind_predicted_objects = getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); - cp.occlusion_do_not_ignore_behind_pedestrians = - getOrDeclareParameter(node, ns + ".occlusion.do_not_ignore_behind_pedestrians"); - cp.occlusion_ignore_velocity_threshold = - getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_threshold"); + + cp.occlusion_ignore_velocity_thresholds.resize( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); + const auto get_label = [](const std::string & s) { + if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") + return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") + return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + }; + const auto custom_labels = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); + const auto custom_velocities = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds"); + for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) { + cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx]; + } cp.occlusion_extra_objects_size = getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); } diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 5c91a7f5fdd5d..19a5ecfa35821 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -96,16 +96,12 @@ std::vector calculate_detection_areas( std::vector select_and_inflate_objects( const std::vector & objects, - const double velocity_threshold, const bool skip_pedestrians, const double inflate_size) + const std::vector velocity_thresholds, const double inflate_size) { std::vector selected_objects; for (const auto & o : objects) { - const auto skip = - (skip_pedestrians && - o.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) || - o.kinematics.initial_twist_with_covariance.twist.linear.x >= velocity_threshold; - if (!skip) { + const auto vel_threshold = velocity_thresholds[o.classification.front().label]; + if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { auto selected_object = o; selected_object.shape.dimensions.x += inflate_size; selected_object.shape.dimensions.y += inflate_size; @@ -161,8 +157,8 @@ bool is_crosswalk_occluded( if (params.occlusion_ignore_behind_predicted_objects) { const auto objects = select_and_inflate_objects( - dynamic_objects, params.occlusion_ignore_velocity_threshold, - params.occlusion_do_not_ignore_behind_pedestrians, params.occlusion_extra_objects_size); + dynamic_objects, params.occlusion_ignore_velocity_thresholds, + params.occlusion_extra_objects_size); clear_occlusions_behind_objects(grid_map, objects); } const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index b1c3d889d73e3..34d571e44b420 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -165,8 +165,7 @@ class CrosswalkModule : public SceneModuleInterface int occlusion_occupied_min; bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; - bool occlusion_do_not_ignore_behind_pedestrians; - double occlusion_ignore_velocity_threshold; + std::vector occlusion_ignore_velocity_thresholds; double occlusion_extra_objects_size; };