From 5a49d8bcdb30c35cba3a0ae2fba907a8c1156190 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT Date: Fri, 26 Jan 2024 09:58:53 +0900 Subject: [PATCH] Refactor and add param to NOT ignore behind pedestrians Signed-off-by: Maxime CLEMENT --- .../config/crosswalk.param.yaml | 1 + .../src/manager.cpp | 8 ++-- .../src/occluded_crosswalk.cpp | 42 +++++++++++++------ .../src/occluded_crosswalk.hpp | 18 ++++++++ .../src/scene_crosswalk.hpp | 3 +- 5 files changed, 56 insertions(+), 16 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 51cf90f22fddd..4b4ac123bac49 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -81,5 +81,6 @@ 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 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 f5d8f3148e1e0..83d6c22e78730 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -140,10 +140,12 @@ 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_min_objects_velocity = - getOrDeclareParameter(node, ns + ".occlusion.min_objects_velocity"); + 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_extra_objects_size = - getOrDeclareParameter(node, ns + ".occlusion.extra_objects_size"); + getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); } 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 867245437a2fb..5c91a7f5fdd5d 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -94,10 +94,30 @@ std::vector calculate_detection_areas( return detection_areas; } -void clear_behind_objects( - grid_map::GridMap & grid_map, +std::vector select_and_inflate_objects( const std::vector & objects, - const double min_object_velocity, const double extra_object_size) + const double velocity_threshold, const bool skip_pedestrians, 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) { + auto selected_object = o; + selected_object.shape.dimensions.x += inflate_size; + selected_object.shape.dimensions.y += inflate_size; + selected_objects.push_back(selected_object); + } + } + return selected_objects; +} + +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); @@ -107,14 +127,10 @@ void clear_behind_objects( const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition(); const auto range = grid_map.getLength().maxCoeff() / 2.0; for (auto object : objects) { - object.shape.dimensions.x += extra_object_size; - object.shape.dimensions.y += extra_object_size; const lanelet::BasicPoint2d object_position = { object.kinematics.initial_pose_with_covariance.pose.position.x, object.kinematics.initial_pose_with_covariance.pose.position.y}; - if ( - object.kinematics.initial_twist_with_covariance.twist.linear.x >= min_object_velocity && - lanelet::geometry::distance2d(grid_map_position, object_position) < range) { + if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { lanelet::BasicPoints2d edge_points; const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); @@ -143,10 +159,12 @@ bool is_crosswalk_occluded( grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); - if (params.occlusion_ignore_behind_predicted_objects) - clear_behind_objects( - grid_map, dynamic_objects, params.occlusion_min_objects_velocity, - params.occlusion_extra_objects_size); + 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); + clear_occlusions_behind_objects(grid_map, objects); + } const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); for (const auto & detection_area : calculate_detection_areas( crosswalk_lanelet, {path_intersection.x, path_intersection.y}, 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 22968fc3d9a65..337b24fae929c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -79,6 +79,24 @@ double calculate_detection_range( const double occluded_object_velocity, const double dist_ego_to_crosswalk, const double ego_velocity); +/// @brief select a subset of objects meeting the velocity threshold and inflate their shape +/// @param objects input objects +/// @param velocity_threshold minimum velocity for an object to be selected +/// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities +/// @param inflate_size [m] size by which the shape of the selected objects are inflated +/// @return selected and inflated objects +std::vector select_and_inflate_objects( + const std::vector & objects, + const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); + +/// @brief clear occlusions behind the given objects +/// @details masks behind the object assuming rays from the center of the grid map +/// @param grid_map grid map +/// @param objects objects +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects); + /// @brief update timers so that the slowdown activates if the initial time is older than the buffer /// @param initial_time initial time /// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 421281a9c47a9..b1c3d889d73e3 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -165,7 +165,8 @@ class CrosswalkModule : public SceneModuleInterface int occlusion_occupied_min; bool occlusion_ignore_with_traffic_light; bool occlusion_ignore_behind_predicted_objects; - double occlusion_min_objects_velocity; + bool occlusion_do_not_ignore_behind_pedestrians; + double occlusion_ignore_velocity_threshold; double occlusion_extra_objects_size; };