diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml index a70d94e7e3e54..5784cce657e0d 100644 --- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -3,6 +3,7 @@ occlusion_spot: method: "predicted_object" # [-] candidate is "occupancy_grid" or "predicted_object" debug: false # [-] whether to publish debug markers. Note Default should be false for performance + use_partition_lanelet: true # [-] whether to use partition lanelet map data pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity threshold: detection_area_length: 100.0 # [m] the length of path to consider perception range diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg new file mode 100644 index 0000000000000..96bf84db9ab10 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg @@ -0,0 +1,4 @@ + + + +egoegopartition lanelet- fence- gurad_rail- wallpartition lanelet...detection area polygondetection area polygonocclusion spot inside partition is consideredocclusion spot inside p...occlusion spot outside partition is not consideredocclusion spot outside...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp index 3064cab1d23f7..65547fd0d763a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -38,6 +38,7 @@ #include #include +#include #include #include @@ -61,6 +62,7 @@ using lanelet::LaneletMapPtr; using lanelet::geometry::fromArcCoordinates; using lanelet::geometry::toArcCoordinates; using DetectionAreaIdx = boost::optional>; +using BasicPolygons2d = std::vector; namespace occlusion_spot_utils { @@ -98,7 +100,8 @@ struct LatLon struct PlannerParam { METHOD method; - bool debug; // [-] + bool debug; // [-] + bool use_partition_lanelet; // [-] // parameters in yaml double detection_area_length; // [m] double detection_area_max_length; // [m] @@ -177,6 +180,27 @@ struct PossibleCollisionInfo } }; +struct DebugData +{ + double z; + std::string road_type = ""; + std::string detection_type = ""; + std::vector detection_area_polygons; + std::vector partition_lanelets; + std::vector parked_vehicle_point; + std::vector possible_collisions; + std::vector occlusion_points; + PathWithLaneId path_raw; + PathWithLaneId interp_path; + void resetData() + { + detection_area_polygons.clear(); + parked_vehicle_point.clear(); + possible_collisions.clear(); + occlusion_points.clear(); + } +}; + lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path); // Note : consider offset_from_start_to_ego and safety margin for collision here void handleCollisionOffset(std::vector & possible_collisions, double offset); @@ -186,7 +210,7 @@ bool isStuckVehicle(PredictedObject obj, const double min_vel); double offsetFromStartToEgo( const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx); std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector polys); + std::vector & objs, const std::vector & polys); std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); @@ -209,21 +233,16 @@ void createPossibleCollisionBehindParkedVehicle( void calcSlowDownPointsForPossibleCollision( const int closest_idx, const PathWithLaneId & path, const double offset, std::vector & possible_collisions); -//!< @brief extract lanelet that includes target_road_type only -DetectionAreaIdx extractTargetRoadArcLength( - const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, - const ROAD_TYPE & target_road_type); //!< @brief convert a set of occlusion spots found on detection_area slice boost::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, - const double offset_from_start_to_ego, const BasicPoint2d basic_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param); + const double offset_from_start_to_ego, const BasicPoint2d base_point, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data); //!< @brief generate possible collisions coming from occlusion spots on the side of the path void createPossibleCollisionsInDetectionArea( - const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, - std::vector & debug_points); + DebugData & debug_data); } // namespace occlusion_spot_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp index 147c1e4396201..b9fc46c2cd044 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot.hpp @@ -40,19 +40,9 @@ namespace behavior_velocity_planner class OcclusionSpotModule : public SceneModuleInterface { using PlannerParam = occlusion_spot_utils::PlannerParam; + using DebugData = occlusion_spot_utils::DebugData; public: - struct DebugData - { - double z; - std::string road_type = "occupancy"; - std::vector detection_areas; - std::vector possible_collisions; - std::vector occlusion_points; - PathWithLaneId path_raw; - PathWithLaneId interp_path; - }; - OcclusionSpotModule( const int64_t module_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp index 073d3bb6aea86..b194a4fb83d91 100644 --- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -39,20 +39,9 @@ namespace behavior_velocity_planner class OcclusionSpotInPublicModule : public SceneModuleInterface { using PlannerParam = occlusion_spot_utils::PlannerParam; + using DebugData = occlusion_spot_utils::DebugData; public: - struct DebugData - { - std::string road_type = "object"; - std::vector detection_areas; - std::vector parked_vehicle_point; - std::vector possible_collisions; - std::vector occlusion_points; - PathWithLaneId path_raw; - PathWithLaneId interp_path; - double z; - }; - OcclusionSpotInPublicModule( const int64_t module_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 1761b539932ca..1fe075dc18c9b 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -69,6 +69,7 @@ struct PointWithSearchRangeIndex }; using autoware_auto_planning_msgs::msg::PathWithLaneId; using Point2d = boost::geometry::model::d2::point_xy; +using Polygons2d = std::vector; namespace planning_utils { inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } @@ -106,7 +107,7 @@ inline geometry_msgs::msg::Pose getPose( { return traj.points.at(idx).pose; } - +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys); inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } inline double square(const double & a) { return a * a; } diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index 5d97f298106ff..0d9eda6fd393b 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -85,6 +85,11 @@ The maximum length of detection area depends on ego current vehicle velocity and ![brief](./docs/occlusion_spot/detection_area_poly.svg) +##### Partition Lanelet + +By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. +![brief](./docs/occlusion_spot/occlusion_spot_partition.svg) + #### Module Parameters | Parameter | Type | Description | @@ -125,7 +130,7 @@ The maximum length of detection area depends on ego current vehicle velocity and ```plantuml @startuml -title modifyPathVelocity (Private/Public) Road +title modifyPathVelocity (Occupancy/PredictedObject) start partition process_path { @@ -135,16 +140,12 @@ note right using spline interpolation and interpolate (x,y,z,v) end note :calc closest path point from ego; -:extract target road pair; -note right -extract target road type start and end pair and early return if none -end note } partition process_sensor_data { -if (road type is PUBLIC) then (yes) +if (road type is PredictedObject) then (yes) :preprocess dynamic object; -else if (road type is PRIVATE) then (yes) +else if (road type is Occupancy) then (yes) :preprocess occupancy grid map info; else (no) stop @@ -154,14 +155,14 @@ endif partition generate_detection_area_polygon { :convert path to path lanelet; :generate left/right slice of polygon that starts from path start; -:generate interpolated polygon which is created from ego TTC and lateral distance that pedestrian can reach within ego TTC.; +:generate interpolated polygon created from ego TTC and lateral distance that pedestrian can reach within ego TTC.; } partition find_possible_collision { :generate possible collision; :calculate collision path point and intersection point; note right - - occlusion spot is calculated by longitudinally closest point of unknown cells. - - intersection point is where ego front bumper and darting object will crash. + - occlusion spot is calculated by the longitudinally closest point of unknown cells. + - intersection point is where ego front bumper and the darting object will crash. - collision path point is calculated by arc coordinate consider ego vehicle's geometry. - safe velocity and safe margin is calculated from performance of ego emergency braking system. end note @@ -197,11 +198,11 @@ stop @enduml ``` -##### Detail process for public road +##### Detail process for predicted object ```plantuml @startuml -title modifyPathVelocity For Public Road +title modifyPathVelocity start partition process_path { @@ -253,7 +254,7 @@ stop ```plantuml @startuml -title modifyPathVelocity For Private Road +title modifyPathVelocity For Occupancy start partition process_path { @@ -285,7 +286,11 @@ partition generate_possible_collision { :generate possible collision from occlusion spot; note right - occlusion spot candidate is N by N size unknown cells. - - consider occlusion which is nearer than `lateral_distance_threshold`. + - consider occlusion spot in detection area polygon. +end note +:filter occlusion spot by partition lanelets; +note right + - filter occlusion spot by partition lanelets which prevent pedestrians come out. end note :calculate collision path point and intersection point; :calculate safe velocity and safe margin for possible collision; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp index 595d97d766a94..cb435400f1feb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -28,6 +28,8 @@ namespace behavior_velocity_planner namespace { using builtin_interfaces::msg::Time; +using BasicPolygons = std::vector; +using Slices = std::vector; visualization_msgs::msg::Marker makeArrowMarker( const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, const int id) @@ -51,7 +53,7 @@ visualization_msgs::msg::Marker makeArrowMarker( std::vector makeSlowDownMarkers( const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, - const std::string road_type, const int id) + const std::string detection_type, const int id) { // virtual wall std::vector debug_markers; @@ -86,7 +88,7 @@ std::vector makeSlowDownMarkers( debug_markers.emplace_back(slowdown_reason_marker); slowdown_reason_marker.scale = tier4_autoware_utils::createMarkerScale(0.0, 0.0, 0.5); slowdown_reason_marker.id = id + 100; - slowdown_reason_marker.text = "\n \n" + road_type; + slowdown_reason_marker.text = "\n \n" + detection_type; debug_markers.emplace_back(slowdown_reason_marker); debug_markers.push_back(makeArrowMarker(possible_collision, id)); return debug_markers; @@ -142,7 +144,7 @@ std::vector makeCollisionMarkers( } visualization_msgs::msg::MarkerArray makePolygonMarker( - const std::vector & polygons, const int id, const double z) + const BasicPolygons & polygons, const std::string ns, const int id, const double z) { visualization_msgs::msg::MarkerArray debug_markers; visualization_msgs::msg::Marker debug_marker; @@ -154,9 +156,9 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); - debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.5); debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); - debug_marker.ns = "detection_area"; + debug_marker.ns = ns; for (const auto & poly : polygons) { for (const auto & p : poly) { geometry_msgs::msg::Point point = @@ -170,6 +172,35 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( return debug_markers; } +visualization_msgs::msg::MarkerArray makeSlicePolygonMarker( + const Slices & slices, const std::string ns, const int id, const double z) +{ + visualization_msgs::msg::MarkerArray debug_markers; + visualization_msgs::msg::Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.header.stamp = rclcpp::Time(0); + debug_marker.id = planning_utils::bitShift(id); + debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.action = visualization_msgs::msg::Marker::ADD; + debug_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, z); + debug_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); + debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + debug_marker.ns = ns; + for (const auto & slice : slices) { + for (const auto & p : slice.polygon) { + geometry_msgs::msg::Point point = + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + debug_marker.points.push_back(point); + } + debug_markers.markers.push_back(debug_marker); + debug_marker.id++; + debug_marker.points.clear(); + } + return debug_markers; +} + visualization_msgs::msg::MarkerArray createPathMarkerArray( const PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const double r, const double g, const double b) @@ -211,7 +242,7 @@ visualization_msgs::msg::MarkerArray createPossibleCollisionMarkers( int id = planning_utils::bitShift(module_id_); for (const auto & possible_collision : possible_collisions) { std::vector collision_markers = - makeSlowDownMarkers(possible_collision, debug_data.road_type, id++); + makeSlowDownMarkers(possible_collision, debug_data.detection_type, id++); occlusion_spot_slowdown_markers.markers.insert( occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), collision_markers.end()); @@ -261,10 +292,11 @@ visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMar appendMarkerArray( createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); } - if (!debug_data_.detection_areas.empty()) { + if (!debug_data_.detection_area_polygons.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time, - &debug_marker_array); + makeSlicePolygonMarker( + debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), + current_time, &debug_marker_array); } return debug_marker_array; @@ -278,10 +310,16 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray appendMarkerArray( createPossibleCollisionMarkers(debug_data_, module_id_), current_time, &debug_marker_array); } - if (!debug_data_.detection_areas.empty()) { + if (!debug_data_.detection_area_polygons.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.detection_areas, module_id_, debug_data_.z), current_time, - &debug_marker_array); + makeSlicePolygonMarker( + debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), + current_time, &debug_marker_array); + } + if (!debug_data_.partition_lanelets.empty()) { + appendMarkerArray( + makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z), + current_time, &debug_marker_array); } if (!debug_data_.interp_path.points.empty()) { appendMarkerArray( diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp index 9c57cff00f35d..44b30e7814e51 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -46,6 +46,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) throw std::invalid_argument{"[behavior_velocity]: occlusion spot detection method is invalid"}; } pp.debug = node.declare_parameter(ns + ".debug", false); + pp.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", false); pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0); diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp index b316bc08e063b..87d351e0635fb 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -337,38 +337,8 @@ std::vector generatePossibleCollisionBehindParkedVehicle( return possible_collisions; } -DetectionAreaIdx extractTargetRoadArcLength( - const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path, - const ROAD_TYPE & target_road_type) -{ - bool found_target = false; - double start_dist = 0.0; - double dist_sum = 0.0; - // search lanelet that includes target_road_type only - for (size_t i = 0; i < path.points.size() - 1; i++) { - ROAD_TYPE search_road_type = occlusion_spot_utils::getCurrentRoadType( - lanelet_map_ptr->laneletLayer.get(path.points[i].lane_ids[0]), lanelet_map_ptr); - if (found_target && search_road_type != target_road_type) { - break; - } - // ignore path farther than max range - if (dist_sum > max_range) { - break; - } - if (!found_target && search_road_type == target_road_type) { - start_dist = dist_sum; - found_target = true; - } - const auto & curr_p = path.points[i].point.pose.position; - const auto & next_p = path.points[i + 1].point.pose.position; - dist_sum += tier4_autoware_utils::calcDistance2d(curr_p, next_p); - } - if (!found_target) return {}; - return DetectionAreaIdx(std::make_pair(start_dist, dist_sum)); -} - std::vector filterDynamicObjectByDetectionArea( - std::vector & objs, const std::vector polys) + std::vector & objs, const std::vector & polys) { std::vector filtered_obj; // stuck points by predicted objects @@ -385,32 +355,33 @@ std::vector filterDynamicObjectByDetectionArea( } void createPossibleCollisionsInDetectionArea( - const std::vector & detection_area_polygons, std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, - std::vector & debug_points) + DebugData & debug_data) { lanelet::ConstLanelet path_lanelet = toPathLanelet(path); if (path_lanelet.centerline2d().empty()) { return; } double distance_lower_bound = std::numeric_limits::max(); - for (const Slice & detection_area_slice : detection_area_polygons) { + for (const Slice & detection_area_slice : debug_data.detection_area_polygons) { std::vector occlusion_spot_positions; grid_utils::findOcclusionSpots( occlusion_spot_positions, grid, detection_area_slice.polygon, param.detection_area.min_occlusion_spot_size); - Point p; - for (const auto & op : occlusion_spot_positions) { - p.x = op[0]; - p.y = op[1]; - debug_points.emplace_back(p); + if (param.debug) { + for (const auto & op : occlusion_spot_positions) { + Point p = + tier4_autoware_utils::createPoint(op[0], op[1], path.points.at(0).point.pose.position.z); + debug_data.occlusion_points.emplace_back(p); + } } if (occlusion_spot_positions.empty()) continue; // for each partition find nearest occlusion spot from polygon's origin BasicPoint2d base_point = detection_area_slice.polygon.at(0); const auto pc = generateOneNotableCollisionFromOcclusionSpot( - grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param); + grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, + debug_data); if (!pc) continue; const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; @@ -419,25 +390,35 @@ void createPossibleCollisionsInDetectionArea( } } +bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) +{ + for (const auto & p : partitions) { + if (bg::intersects(direction, p)) return false; + } + return true; +} + boost::optional generateOneNotableCollisionFromOcclusionSpot( const grid_map::GridMap & grid, const std::vector & occlusion_spot_positions, const double offset_from_start_to_ego, const BasicPoint2d base_point, - const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, DebugData & debug_data) { const double baselink_to_front = param.baselink_to_front; const double half_vehicle_width = param.half_vehicle_width; double distance_lower_bound = std::numeric_limits::max(); PossibleCollisionInfo candidate; bool has_collision = false; - for (grid_map::Position occlusion_spot_position : occlusion_spot_positions) { + const auto & partition_lanelets = debug_data.partition_lanelets; + for (const grid_map::Position & occlusion_spot_position : occlusion_spot_positions) { // arc intersection - lanelet::BasicPoint2d obstacle_point = {occlusion_spot_position[0], occlusion_spot_position[1]}; - lanelet::ArcCoordinates arc_coord_occlusion_point = - lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); + const lanelet::BasicPoint2d obstacle_point = { + occlusion_spot_position[0], occlusion_spot_position[1]}; const double dist = std::hypot(base_point[0] - obstacle_point[0], base_point[1] - obstacle_point[1]); // skip if absolute distance is larger if (distance_lower_bound < dist) continue; + lanelet::ArcCoordinates arc_coord_occlusion_point = + lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); const double length_to_col = arc_coord_occlusion_point.length - baselink_to_front; // skip if occlusion is behind ego bumper if (length_to_col < offset_from_start_to_ego) { @@ -451,7 +432,13 @@ boost::optional generateOneNotableCollisionFromOcclusionS const auto & ip = pc.intersection_pose.position; bool collision_free_at_intersection = grid_utils::isCollisionFree(grid, occlusion_spot_position, grid_map::Position(ip.x, ip.y)); - if (collision_free_at_intersection) { + bool obstacle_not_blocked_by_partition = true; + if (param.use_partition_lanelet) { + const auto & op = obstacle_point; + const LineString2d obstacle_vec = {{op[0], op[1]}, {ip.x, ip.y}}; + obstacle_not_blocked_by_partition = isNotBlockedByPartition(obstacle_vec, partition_lanelets); + } + if (collision_free_at_intersection && obstacle_not_blocked_by_partition) { distance_lower_bound = dist; candidate = pc; has_collision = true; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 426a2efbfbf6e..726c5e7bec799 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include #include #include +#include #include #include @@ -39,13 +41,18 @@ OcclusionSpotModule::OcclusionSpotModule( : SceneModuleInterface(module_id, logger, clock), publisher_(publisher) { param_ = planner_param; + debug_data_.detection_type = "occupancy"; + if (param_.use_partition_lanelet) { + const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); + planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); + } } bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { - debug_data_ = DebugData(); + debug_data_.resetData(); if (path->points.size() < 2) { return true; } @@ -83,16 +90,13 @@ bool OcclusionSpotModule::modifyPathVelocity( grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); - using Slice = occlusion_spot_utils::Slice; - std::vector detection_area_polygons; + auto & detection_area_polygons = debug_data_.detection_area_polygons; utils::buildDetectionAreaPolygon( detection_area_polygons, interp_path, offset_from_start_to_ego, param_); std::vector possible_collisions; // Note: Don't consider offset from path start to ego here utils::createPossibleCollisionsInDetectionArea( - detection_area_polygons, possible_collisions, grid_map, interp_path, offset_from_start_to_ego, - param_, debug_data_.occlusion_points); - if (detection_area_polygons.empty()) return true; + possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_); RCLCPP_DEBUG_STREAM_THROTTLE( logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); @@ -103,11 +107,6 @@ bool OcclusionSpotModule::modifyPathVelocity( // these debug topics needs computation resource if (param_.debug) { publisher_->publish(occ_grid); - for (const auto & p : detection_area_polygons) { - debug_data_.detection_areas.emplace_back(p.polygon); - } - } else { - debug_data_.occlusion_points.clear(); } debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index a351bce4a62c3..7040c7c221af1 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -39,13 +39,18 @@ OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( : SceneModuleInterface(module_id, logger, clock) { param_ = planner_param; + debug_data_.detection_type = "object"; + if (param_.use_partition_lanelet) { + const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); + planning_utils::getAllPartitionLanelets(ll, debug_data_.partition_lanelets); + } } bool OcclusionSpotInPublicModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { - debug_data_ = DebugData(); + debug_data_.resetData(); if (path->points.size() < 2) { return true; } @@ -79,8 +84,7 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( std::vector obj = utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); double offset_from_start_to_ego = utils::offsetFromStartToEgo(interp_path, ego_pose, closest_idx); - using Slice = occlusion_spot_utils::Slice; - std::vector detection_area_polygons; + auto & detection_area_polygons = debug_data_.detection_area_polygons; utils::buildDetectionAreaPolygon( detection_area_polygons, interp_path, offset_from_start_to_ego, param_); const auto filtered_obj = utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); @@ -93,11 +97,6 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity( utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); - if (param_.debug) { - for (const auto & p : detection_area_polygons) { - debug_data_.detection_areas.emplace_back(p.polygon); - } - } debug_data_.possible_collisions = possible_collisions; return true; } diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 5189c71572567..937818d17e787 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -23,6 +23,18 @@ namespace behavior_velocity_planner { namespace planning_utils { +void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, Polygons2d & polys) +{ + const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); + for (const auto & partition : partitions) { + lanelet::BasicLineString2d line; + for (const auto & p : partition) { + line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); + } + polys.emplace_back(lanelet::BasicPolygon2d(line)); + } +} + SearchRangeIndex getPathIndexRangeIncludeLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t lane_id) {