From e6b5afd0df3247a27fe11a07568656ba905ca8dd Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Tue, 12 Sep 2023 16:17:33 +0900 Subject: [PATCH 1/4] fix(start_planner): keep max back distace whithin lanes Signed-off-by: kosuke55 --- .../scene_module/start_planner/start_planner_module.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 29e16fd570720..db220d94aeeb9 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -690,9 +690,14 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); + // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( start_pose_candidates.points, current_pose.position, -back_distance); From da4eae2acee393ee9aada811b93b7ce0a2fb733b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 17:16:09 +0900 Subject: [PATCH 2/4] Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp Co-authored-by: Kyoichi Sugahara --- .../src/scene_module/start_planner/start_planner_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index db220d94aeeb9..402c333b806eb 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -692,6 +692,7 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const double s_current = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; +// Set the maximum backward distance less than the distance from the vehicle's base_link to the lane's rearmost point to prevent lane departure. const double max_back_distance = std::clamp( s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); From 2776672973d35c06ff342f5abe78ecaf2cfa7c8a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 12 Sep 2023 17:17:27 +0900 Subject: [PATCH 3/4] Update planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp --- .../src/scene_module/start_planner/start_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 402c333b806eb..edcf77e6afbcc 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -690,9 +690,9 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); +// Set the maximum backward distance less than the distance from the vehicle's base_link to the lane's rearmost point to prevent lane departure. const double s_current = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; -// Set the maximum backward distance less than the distance from the vehicle's base_link to the lane's rearmost point to prevent lane departure. const double max_back_distance = std::clamp( s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); From b050967ef8bbb96bcaf542f29079ef52b23078ea Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 12 Sep 2023 08:19:35 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- .../src/scene_module/start_planner/start_planner_module.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index edcf77e6afbcc..a285224404f12 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -690,7 +690,8 @@ std::vector StartPlannerModule::searchPullOutStartPoses( const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); -// Set the maximum backward distance less than the distance from the vehicle's base_link to the lane's rearmost point to prevent lane departure. + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. const double s_current = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; const double max_back_distance = std::clamp(