From 4987e459297f5743ad3690def99cd20c4a2842fb Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 18 Mar 2025 16:28:54 +0900 Subject: [PATCH] feat(goal_planner): expand outer collision check margin Signed-off-by: kosuke55 margin comment Signed-off-by: kosuke55 update svg Signed-off-by: kosuke55 --- .../README.md | 1 + .../config/goal_planner.param.yaml | 1 + ..._planner-collision_check_margin.drawio.svg | 408 ++++++++++++++---- .../goal_planner_parameters.hpp | 1 + .../util.hpp | 20 +- .../src/decision_state.cpp | 2 +- .../src/goal_planner_module.cpp | 7 +- .../src/manager.cpp | 2 + .../src/util.cpp | 30 +- 9 files changed, 352 insertions(+), 120 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 63b1cc4427bd0..226ae9dbd88b0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -375,6 +375,7 @@ Then there is the concept of soft and hard margins. Although not currently param | object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented. | [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] | | object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] | | object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| collision_check_outer_margin_factor | [-] | double | factor to extend the collision check margin from the inside margin to the outside in the curved path | 2.0 | | detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | ## **safety check** diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 765c9283ba668..5d78cedf554c0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -42,6 +42,7 @@ collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] # the maximum margin when ego and objects are oriented. collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + collision_check_outer_margin_factor: 2.0 th_moving_object_velocity: 1.0 detection_bound_offset: 15.0 outer_road_detection_offset: 1.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg index 9db7d90d72d35..34b4b3b94ac94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/images/goal_planner-collision_check_margin.drawio.svg @@ -1,108 +1,326 @@ - + - - - - - - - - - - - - - - - - - - -
-
-
- collision_check_margin -
-
-
- - collision_check_margin - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision_check_margin +
+
+
+ + + + + + + + + + + + + + +
+
+
+ extra_stopping_margin(v, max_decel) +
+
+
+ + + + + + + + + + + + + + +
+
+
+ extra_lateral_margin(v, κ) +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ extra_lateral_margin(v, κ) * collision_check_outer_margin_factor +
+
+
+ + + + + + + - - - - -
-
-
- extra_stopping_margin(v, max_decel) -
-
-
- - extra_stopping_margin(v, max_decel) - - - - - - -
-
-
- extra_lateral_margin(v, κ) -
-
-
- - extra_lateral_margin(v, κ) - - - - - - - - Text is not SVG - cannot display - - diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index f20ba48f1ebc0..1a2d6b8ce1217 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -80,6 +80,7 @@ struct GoalPlannerParameters std::vector object_recognition_collision_check_soft_margins{}; std::vector object_recognition_collision_check_hard_margins{}; double object_recognition_collision_check_max_extra_stopping_margin{0.0}; + double collision_check_outer_margin_factor{0.0}; double th_moving_object_velocity{0.0}; double detection_bound_offset{0.0}; double outer_road_detection_offset{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 8ec70f136cf6c..867c689ecaf43 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -128,6 +128,23 @@ bool isWithinAreas( */ std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes); +/** + * @brief check collision between objects and ego path footprints + * @param path ego path to check collision + * @param curvatures curvatures of ego path + * @param static_target_objects static objects to check collision + * @param dynamic_target_objects dynamic objects to check collision + * @param behavior_path_parameters behavior path parameters + * @param collision_check_margin margin to check collision + * @param extract_static_objects flag to extract static objects + * @param maximum_deceleration maximum deceleration + * @param object_recognition_collision_check_max_extra_stopping_margin maximum extra stopping margin + * @param collision_check_outer_margin_factor factor to extend the collision check margin from the + * inside margin to the outside in the curved path + * @param ego_polygons_expanded expanded ego polygons + * @param update_debug_data flag to update debug data + * @return true if collision is detected + */ bool checkObjectsCollision( const PathWithLaneId & path, const std::vector & curvatures, const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, @@ -135,7 +152,8 @@ bool checkObjectsCollision( const double collision_check_margin, const bool extract_static_objects, const double maximum_deceleration, const double object_recognition_collision_check_max_extra_stopping_margin, - std::vector & ego_polygons_expanded, const bool update_debug_data = false); + const double collision_check_outer_margin_factor, std::vector & ego_polygons_expanded, + const bool update_debug_data = false); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp index 9538c51d73c82..a1e415ec5c715 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/decision_state.cpp @@ -114,7 +114,7 @@ PathDecisionState PathDecisionStateController::get_next_state( planner_data->parameters, margin, /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, - ego_polygons_expanded, true)) { + parameters.collision_check_outer_margin_factor, ego_polygons_expanded, true)) { RCLCPP_DEBUG( logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index bca1105f4b4cc..cf73e7c963cc3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -966,7 +966,7 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte parameters_.object_recognition_collision_check_hard_margins.back(), /*extract_static_objects=*/false, parameters_.maximum_deceleration, parameters_.object_recognition_collision_check_max_extra_stopping_margin, - debug_data_.ego_polygons_expanded)) { + parameters_.collision_check_outer_margin_factor, debug_data_.ego_polygons_expanded)) { return false; } @@ -1258,7 +1258,8 @@ std::optional GoalPlannerModule::selectPullOverPath( context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, true, parameters_.maximum_deceleration, parameters_.object_recognition_collision_check_max_extra_stopping_margin, - debug_data_.ego_polygons_expanded, true)) { + parameters_.collision_check_outer_margin_factor, debug_data_.ego_polygons_expanded, + true)) { continue; } if ( @@ -1933,7 +1934,7 @@ bool FreespaceParkingPlanner::isStuck( parameters.object_recognition_collision_check_hard_margins.back(), /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, - ego_polygons_expanded)) { + parameters.collision_check_outer_margin_factor, ego_polygons_expanded)) { return true; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 51359214189fe..6db30c0970470 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -113,6 +113,8 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.collision_check_outer_margin_factor = + node->declare_parameter(ns + "collision_check_outer_margin_factor"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); p.outer_road_detection_offset = diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 8b2ed3bdf9e71..d9a995b16f5e0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -309,6 +309,7 @@ bool checkObjectsCollision( const double collision_check_margin, const bool extract_static_objects, const double maximum_deceleration, const double object_recognition_collision_check_max_extra_stopping_margin, + const double collision_check_outer_margin_factor, std::vector & debug_ego_polygons_expanded, const bool update_debug_data) { if (path.points.size() != curvatures.size()) { @@ -324,18 +325,6 @@ bool checkObjectsCollision( return false; } - // check collision roughly with {min_distance, max_distance} between ego footprint and objects - // footprint - std::pair has_collision_rough = - utils::path_safety_checker::checkObjectsCollisionRough( - path, target_objects, collision_check_margin, behavior_path_parameters, false); - if (!has_collision_rough.first) { - return false; - } - if (has_collision_rough.second) { - return true; - } - std::vector obj_polygons; for (const auto & object : target_objects.objects) { obj_polygons.push_back(autoware_utils::to_polygon2d(object)); @@ -352,20 +341,21 @@ bool checkObjectsCollision( const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, object_recognition_collision_check_max_extra_stopping_margin); - // The square is meant to imply centrifugal force, but it is not a very well-founded formula. - // TODO(kosuke55): It is needed to consider better way because there is an inherently - // different conception of the inside and outside margins. + const double curvature = curvatures.at(i); const double extra_lateral_margin = std::min( - extra_stopping_margin, - std::abs(curvatures.at(i) * std::pow(p.point.longitudinal_velocity_mps, 2))); - + extra_stopping_margin, std::abs(curvature * std::pow(p.point.longitudinal_velocity_mps, 2))); + // The outer margin is `collision_check_outer_margin_factor` times larger than the inner margin. + const double sign = curvature > 0.0 ? -1.0 : 1.0; + const auto ego_pose_offset = calc_offset_pose( + p.point.pose, 0.0, sign * (collision_check_outer_margin_factor - 1.0) * extra_lateral_margin, + 0.0); const auto ego_polygon = autoware_utils::to_footprint( - p.point.pose, + ego_pose_offset, behavior_path_parameters.base_link2front + collision_check_margin + extra_stopping_margin, behavior_path_parameters.base_link2rear + collision_check_margin, behavior_path_parameters.vehicle_width + collision_check_margin * 2.0 + - extra_lateral_margin * 2.0); + extra_lateral_margin * (collision_check_outer_margin_factor + 1.0)); ego_polygons_expanded.push_back(ego_polygon); }