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 2effa9e9ce55a..356212191c7c5 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 @@ -189,7 +189,6 @@ struct DebugData 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; std::vector occlusion_points; @@ -197,6 +196,7 @@ struct DebugData PathWithLaneId interp_path; void resetData() { + close_partition.clear(); detection_area_polygons.clear(); parked_vehicle_point.clear(); possible_collisions.clear(); @@ -220,7 +220,7 @@ std::vector filterDynamicObjectByDetectionArea( std::vector getParkedVehicles( const PredictedObjects & dyn_objects, const PlannerParam & param, std::vector & debug_point); -bool generatePossibleCollisionBehindParkedVehicle( +bool generatePossibleCollisionsFromObjects( std::vector & possible_collisions, const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects); @@ -246,7 +246,7 @@ boost::optional generateOneNotableCollisionFromOcclusionS const double offset_from_start_to_ego, const Point2d 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 -bool createPossibleCollisionsInDetectionArea( +bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data); 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 60424f15e19d2..dde073cdde3f0 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 @@ -61,6 +61,8 @@ class OcclusionSpotModule : public SceneModuleInterface // Parameter PlannerParam param_; tier4_autoware_utils::StopWatch stop_watch_; + std::vector partition_lanelets_; + std::vector close_partition_; protected: int64_t module_id_{}; 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 b688641a9bd0b..83d60cf3d350e 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 @@ -295,9 +295,9 @@ visualization_msgs::msg::MarkerArray OcclusionSpotModule::createDebugMarkerArray debug_data_.detection_area_polygons, "detection_area", module_id_, debug_data_.z), current_time, &debug_marker_array); } - if (!debug_data_.partition_lanelets.empty()) { + if (!debug_data_.close_partition.empty()) { appendMarkerArray( - makePolygonMarker(debug_data_.close_partition, "partition", module_id_, debug_data_.z), + makePolygonMarker(debug_data_.close_partition, "close_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 8b471ae929861..55f7047654749 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 @@ -73,6 +73,7 @@ bool buildDetectionAreaPolygon( da_range.min_longitudinal_distance; da_range.min_lateral_distance = p.half_vehicle_width; da_range.max_lateral_distance = p.detection_area.max_lateral_distance; + slices.clear(); return planning_utils::createDetectionAreaPolygons(slices, path, da_range, p.pedestrian_vel); } @@ -275,7 +276,7 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( const ArcCoordinates & arc_coord_occlusion_with_offset, const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param) { - auto setPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { + auto calcPose = [](const lanelet::ConstLanelet & pl, const ArcCoordinates & arc) { const auto & ll = pl.centerline2d(); BasicPoint2d bp = fromArcCoordinates(ll, arc); Pose pose; @@ -306,15 +307,15 @@ PossibleCollisionInfo calculateCollisionPathPointFromOcclusionSpot( pc.arc_lane_dist_at_collision = {distance_to_stop, arc_coord_occlusion_with_offset.distance}; pc.obstacle_info.safe_motion = sm; pc.obstacle_info.ttc = ttc; - pc.obstacle_info.position = setPose(path_lanelet, arc_coord_occlusion).position; + pc.obstacle_info.position = calcPose(path_lanelet, arc_coord_occlusion).position; pc.obstacle_info.max_velocity = param.pedestrian_vel; - pc.collision_pose = setPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); - pc.collision_with_margin.pose = setPose(path_lanelet, {distance_to_stop, 0.0}); - pc.intersection_pose = setPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); + pc.collision_pose = calcPose(path_lanelet, {arc_coord_occlusion_with_offset.length, 0.0}); + pc.collision_with_margin.pose = calcPose(path_lanelet, {distance_to_stop, 0.0}); + pc.intersection_pose = calcPose(path_lanelet, {arc_coord_occlusion.length, 0.0}); return pc; } -bool generatePossibleCollisionBehindParkedVehicle( +bool generatePossibleCollisionsFromObjects( std::vector & possible_collisions, const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego, const std::vector & dyn_objects) @@ -339,14 +340,7 @@ bool generatePossibleCollisionBehindParkedVehicle( arc_coord_occlusion, arc_coord_occlusion_with_offset, path_lanelet, param); possible_collisions.emplace_back(pc); } - // sort by arc length - std::sort( - possible_collisions.begin(), possible_collisions.end(), - [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { - return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; - }); - if (possible_collisions.empty()) return false; - return true; + return !possible_collisions.empty(); } std::vector filterDynamicObjectByDetectionArea( @@ -366,7 +360,7 @@ std::vector filterDynamicObjectByDetectionArea( return filtered_obj; } -bool createPossibleCollisionsInDetectionArea( +bool generatePossibleCollisionsFromGridMap( std::vector & possible_collisions, const grid_map::GridMap & grid, const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param, DebugData & debug_data) @@ -395,28 +389,21 @@ bool createPossibleCollisionsInDetectionArea( const auto pc = generateOneNotableCollisionFromOcclusionSpot( grid, occlusion_spot_positions, offset_from_start_to_ego, base_point, path_lanelet, param, debug_data); - if (!pc) continue; + if (pc == boost::none) continue; const double lateral_distance = std::abs(pc.get().arc_lane_dist_at_collision.distance); if (lateral_distance > distance_lower_bound) continue; distance_lower_bound = lateral_distance; possible_collisions.emplace_back(pc.get()); } - // sort by arc length - std::sort( - possible_collisions.begin(), possible_collisions.end(), - [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { - return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; - }); - if (possible_collisions.empty()) return false; - return true; + return !possible_collisions.empty(); } -bool isNotBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) +bool isBlockedByPartition(const LineString2d & direction, const BasicPolygons2d & partitions) { for (const auto & p : partitions) { - if (bg::intersects(direction, p)) return false; + if (bg::intersects(direction, p)) return true; } - return true; + return false; } boost::optional generateOneNotableCollisionFromOcclusionSpot( @@ -453,23 +440,20 @@ 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), param.pedestrian_radius); - bool obstacle_not_blocked_by_partition = true; + bool is_obstacle_blocked_by_partition = false; if (!collision_free_at_intersection) continue; 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); + is_obstacle_blocked_by_partition = isBlockedByPartition(obstacle_vec, partition_lanelets); } - if (!obstacle_not_blocked_by_partition) continue; + if (is_obstacle_blocked_by_partition) continue; distance_lower_bound = dist; candidate = pc; has_collision = true; } - if (has_collision) { - return candidate; - } else { - return {}; - } + if (has_collision) return candidate; + return boost::none; } } // namespace occlusion_spot_utils diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp index 5b642d80df942..709d2f75c5a90 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -27,8 +27,6 @@ void applySafeVelocityConsideringPossibleCollision( PathWithLaneId * inout_path, std::vector & possible_collisions, const PlannerParam & param) { - const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")}; - rclcpp::Clock clock{RCL_ROS_TIME}; // return nullptr or too few points if (!inout_path || inout_path->points.size() < 2) { return; 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 f3f510445d7ed..6dd75b302387c 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 @@ -29,8 +29,11 @@ #include // turn on only when debugging. -#define DEBUG_PRINT(enable, x) \ - if (enable) RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, x); +#define DEBUG_PRINT(enable, n, x) \ + if (enable) { \ + const std::string time_msg = n + std::to_string(x); \ + RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ + } namespace behavior_velocity_planner { @@ -45,13 +48,14 @@ OcclusionSpotModule::OcclusionSpotModule( if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) - param_.detection_area_length = std::min(50.0, param_.detection_area_length); + const double max_length = 35.0; // current available length + param_.detection_area_length = std::min(max_length, param_.detection_area_length); } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { 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); + planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); } } @@ -59,6 +63,7 @@ bool OcclusionSpotModule::modifyPathVelocity( autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) { + if (param_.is_show_processing_time) stop_watch_.tic("total_processing_time"); debug_data_.resetData(); if (path->points.size() < 2) { return true; @@ -83,42 +88,43 @@ bool OcclusionSpotModule::modifyPathVelocity( PathWithLaneId interp_path; //! never change this interpolation interval(will affect module accuracy) splineInterpolate(clipped_path, 1.0, &interp_path, logger_); - PathWithLaneId predicted_path; - if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { - predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); - } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { - } - debug_data_.interp_path = interp_path; const geometry_msgs::msg::Point start_point = interp_path.points.at(0).point.pose.position; const auto offset = tier4_autoware_utils::calcSignedArcLength( interp_path.points, ego_pose, start_point, param_.dist_thr, param_.angle_thr); if (offset == boost::none) return true; const double offset_from_start_to_ego = -offset.get(); - auto & detection_area_polygons = debug_data_.detection_area_polygons; + const bool show_time = param_.is_show_processing_time; + if (show_time) stop_watch_.tic("processing_time"); + PathWithLaneId predicted_path; + if (param_.pass_judge == utils::PASS_JUDGE::CURRENT_VELOCITY) { + predicted_path = utils::applyVelocityToPath(interp_path, param_.v.v_ego); + } else if (param_.pass_judge == utils::PASS_JUDGE::SMOOTH_VELOCITY) { + } + DEBUG_PRINT(show_time, "apply velocity [ms]: ", stop_watch_.toc("processing_time", true)); if (!utils::buildDetectionAreaPolygon( - detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) { + debug_data_.detection_area_polygons, predicted_path, offset_from_start_to_ego, param_)) { return true; // path point is not enough } + DEBUG_PRINT(show_time, "generate poly[ms]: ", stop_watch_.toc("processing_time", true)); std::vector possible_collisions; // extract only close lanelet if (param_.use_partition_lanelet) { - planning_utils::extractClosePartition( - ego_pose.position, debug_data_.partition_lanelets, debug_data_.close_partition); + planning_utils::extractClosePartition(ego_pose.position, partition_lanelets_, close_partition_); } - if (param_.is_show_processing_time) stop_watch_.tic("processing_time"); + DEBUG_PRINT(show_time, "extract[ms]: ", stop_watch_.toc("processing_time", true)); + std::vector parked_vehicle_point; if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { const auto & occ_grid_ptr = planner_data_->occupancy_grid; - if (!occ_grid_ptr) return true; // mo data + if (!occ_grid_ptr) return true; // no data grid_map::GridMap grid_map; grid_utils::denoiseOccupancyGridCV( occ_grid_ptr, grid_map, param_.grid, param_.is_show_cv_window, param_.filter_occupancy_grid); if (param_.use_object_info) { grid_utils::addObjectsToGridMap(*planner_data_->predicted_objects, grid_map); } - DEBUG_PRINT( - param_.is_show_processing_time, "grid [ms]: " << stop_watch_.toc("processing_time", true)); + DEBUG_PRINT(show_time, "grid [ms]: ", stop_watch_.toc("processing_time", true)); // Note: Don't consider offset from path start to ego here - if (!utils::createPossibleCollisionsInDetectionArea( + if (!utils::generatePossibleCollisionsFromGridMap( possible_collisions, grid_map, interp_path, offset_from_start_to_ego, param_, debug_data_)) { // no occlusion spot @@ -126,31 +132,33 @@ bool OcclusionSpotModule::modifyPathVelocity( } } else if (param_.detection_method == utils::DETECTION_METHOD::PREDICTED_OBJECT) { const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; - if (!dynamic_obj_arr_ptr) return true; // mo data + if (!dynamic_obj_arr_ptr) return true; // no data std::vector obj = - utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, debug_data_.parked_vehicle_point); + utils::getParkedVehicles(*dynamic_obj_arr_ptr, param_, parked_vehicle_point); const auto filtered_obj = - utils::filterDynamicObjectByDetectionArea(obj, detection_area_polygons); + utils::filterDynamicObjectByDetectionArea(obj, debug_data_.detection_area_polygons); // Note: Don't consider offset from path start to ego here - if (!utils::generatePossibleCollisionBehindParkedVehicle( + if (!utils::generatePossibleCollisionsFromObjects( possible_collisions, interp_path, param_, offset_from_start_to_ego, filtered_obj)) { // no occlusion spot return true; } } - DEBUG_PRINT( - param_.is_show_processing_time, "occlusion [ms]: " << stop_watch_.toc("processing_time", true)); - DEBUG_PRINT(param_.is_show_occlusion, "num collision:" << possible_collisions.size()); + DEBUG_PRINT(show_time, "occlusion [ms]: ", stop_watch_.toc("processing_time", true)); + DEBUG_PRINT(show_time, "num collision:", possible_collisions.size()); utils::calcSlowDownPointsForPossibleCollision(0, interp_path, 0.0, possible_collisions); // Note: Consider offset from path start to ego here utils::handleCollisionOffset(possible_collisions, offset_from_start_to_ego); // apply safe velocity using ebs and pbs deceleration utils::applySafeVelocityConsideringPossibleCollision(path, possible_collisions, param_); // these debug topics needs computation resource - + debug_data_.parked_vehicle_point = parked_vehicle_point; + debug_data_.close_partition = close_partition_; debug_data_.z = path->points.front().point.pose.position.z; debug_data_.possible_collisions = possible_collisions; + debug_data_.interp_path = interp_path; debug_data_.path_raw = clipped_path; + DEBUG_PRINT(show_time, "total [ms]: ", stop_watch_.toc("total_processing_time", true)); return true; }