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 5568dea6cd3d6..2effa9e9ce55a 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 @@ -188,6 +188,7 @@ struct DebugData std::string road_type = ""; std::string detection_type = ""; Polygons2d detection_area_polygons; + std::vector close_partition; std::vector partition_lanelets; std::vector parked_vehicle_point; std::vector possible_collisions; diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp index 6632127a9e98e..9128ec53863d1 100644 --- a/planning/behavior_velocity_planner/include/utilization/util.hpp +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -38,6 +38,8 @@ #include #include +#include +#include #include #include #include @@ -131,7 +133,9 @@ bool createDetectionAreaPolygons( const double obstacle_vel_mps); PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); Point2d calculateLateralOffsetPoint2d(const Pose & p, const double offset); - +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh = 30.0); void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys); void setVelocityFrom(const size_t idx, const double vel, PathWithLaneId * input); void insertVelocity( 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 b0749792956fd..c44fa8add2ad6 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 @@ -155,12 +155,12 @@ visualization_msgs::msg::MarkerArray makePolygonMarker( 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, 1.0, 1.0, 0.5); - debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); debug_marker.ns = ns; for (const auto & poly : polygons) { for (const auto & p : poly) { geometry_msgs::msg::Point point = - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 1.0); debug_marker.points.push_back(point); } debug_markers.markers.push_back(debug_marker); @@ -298,7 +298,7 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray } if (!debug_data_.partition_lanelets.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.partition_lanelets, "partition", module_id_, debug_data_.z), + makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z), current_time, &debug_marker_array); } if (!debug_data_.interp_path.points.empty()) { 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 15e57021807b0..8b471ae929861 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 @@ -429,7 +429,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS double distance_lower_bound = std::numeric_limits::max(); PossibleCollisionInfo candidate; bool has_collision = false; - const auto & partition_lanelets = debug_data.partition_lanelets; + const auto & partition_lanelets = debug_data.close_partition; for (const grid_map::Position & occlusion_spot_position : occlusion_spot_positions) { // arc intersection const lanelet::BasicPoint2d obstacle_point = { 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 fd0c10accff6a..ff6e6dd8472f5 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 @@ -99,6 +99,11 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; // path point is not enough } std::vector possible_collisions; + // extract only close lanelet + if (param_.use_partition_lanelet) { + extractClosePartition( + ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition); + } if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp index 5e1cf7e377654..ebe3c341d8145 100644 --- a/planning/behavior_velocity_planner/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -142,14 +142,28 @@ bool createDetectionAreaPolygons( return true; } +void extractClosePartition( + const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, + BasicPolygons2d & close_partition, const double distance_thresh) +{ + for (const auto & p : all_partitions) { + if (boost::geometry::distance(Point2d(position.x, position.y), p) < distance_thresh) { + close_partition.emplace_back(p); + } + } + return; +} + void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons2d & polys) { const lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(ll); for (const auto & partition : partitions) { lanelet::BasicLineString2d line; - for (const auto & p : partition) { + for (const auto p : partition) { line.emplace_back(lanelet::BasicPoint2d{p.x(), p.y()}); } + // corect line to calculate distance accuratry + boost::geometry::correct(line); polys.emplace_back(lanelet::BasicPolygon2d(line)); } }