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 @@ + + + +
ego
ego
partition lanelet
- fence
- gurad_rail
- wall
partition lanelet...
detection area polygon
detection area polygon
occlusion spot inside partition is considered
occlusion spot inside p...
occlusion spot outside partition is not considered
occlusion 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) {