Skip to content

Commit e6d2575

Browse files
feat(start_planner): define ignore section separately (autowarefoundation#6219)
* Update collision check distances in start planner module Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 91b8c62 commit e6d2575

File tree

6 files changed

+38
-30
lines changed

6 files changed

+38
-30
lines changed

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

+4-4
Original file line numberDiff line numberDiff line change
@@ -5,15 +5,14 @@
55
th_arrived_distance: 1.0
66
th_stopped_velocity: 0.01
77
th_stopped_time: 1.0
8-
collision_check_margins: [2.0, 1.5, 1.0]
9-
collision_check_distance_from_end: 1.0
8+
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
109
collision_check_margin_from_front_object: 5.0
1110
th_moving_object_velocity: 1.0
1211
th_distance_to_middle_of_the_road: 0.5
1312
center_line_path_interval: 1.0
1413
# shift pull out
1514
enable_shift_pull_out: true
16-
check_shift_path_lane_departure: false
15+
shift_collision_check_distance_from_end: -10.0
1716
minimum_shift_pull_out_distance: 0.0
1817
deceleration_interval: 15.0
1918
lateral_jerk: 0.5
@@ -23,6 +22,7 @@
2322
maximum_curvature: 0.07
2423
# geometric pull out
2524
enable_geometric_pull_out: true
25+
geometric_collision_check_distance_from_end: 0.0
2626
divide_pull_out_path: true
2727
geometric_pull_out_velocity: 1.0
2828
arc_path_interval: 1.0
@@ -32,7 +32,7 @@
3232
# search start pose backward
3333
enable_back: true
3434
search_priority: "efficient_path" # "efficient_path" or "short_back_distance"
35-
max_back_distance: 30.0
35+
max_back_distance: 20.0
3636
backward_search_resolution: 2.0
3737
backward_path_update_duration: 3.0
3838
ignore_distance_from_lane_end: 15.0

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -202,7 +202,8 @@ class StartPlannerModule : public SceneModuleInterface
202202
const Pose & start_pose_candidate, const std::shared_ptr<PullOutPlannerBase> & planner,
203203
const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin);
204204

205-
PathWithLaneId extractCollisionCheckSection(const PullOutPath & path);
205+
PathWithLaneId extractCollisionCheckSection(
206+
const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type);
206207
void updateStatusWithCurrentPath(
207208
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
208209
const behavior_path_planner::PlannerType & planner_type);

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -43,14 +43,14 @@ struct StartPlannerParameters
4343
double intersection_search_length{0.0};
4444
double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
4545
std::vector<double> collision_check_margins{};
46-
double collision_check_distance_from_end{0.0};
4746
double collision_check_margin_from_front_object{0.0};
4847
double th_moving_object_velocity{0.0};
4948
double center_line_path_interval{0.0};
5049

5150
// shift pull out
5251
bool enable_shift_pull_out{false};
5352
bool check_shift_path_lane_departure{false};
53+
double shift_collision_check_distance_from_end{0.0};
5454
double minimum_shift_pull_out_distance{0.0};
5555
int lateral_acceleration_sampling_num{0};
5656
double lateral_jerk{0.0};
@@ -60,6 +60,7 @@ struct StartPlannerParameters
6060
double deceleration_interval{0.0};
6161
// geometric pull out
6262
bool enable_geometric_pull_out{false};
63+
double geometric_collision_check_distance_from_end;
6364
bool divide_pull_out_path{false};
6465
ParallelParkingParameters parallel_parking_parameters{};
6566
// search start pose backward

planning/behavior_path_start_planner_module/src/manager.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
4545
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
4646
p.collision_check_margins =
4747
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
48-
p.collision_check_distance_from_end =
49-
node->declare_parameter<double>(ns + "collision_check_distance_from_end");
5048
p.collision_check_margin_from_front_object =
5149
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
5250
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
@@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
5553
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
5654
p.check_shift_path_lane_departure =
5755
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
56+
p.shift_collision_check_distance_from_end =
57+
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
5858
p.minimum_shift_pull_out_distance =
5959
node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
6060
p.lateral_acceleration_sampling_num =
@@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
6666
p.deceleration_interval = node->declare_parameter<double>(ns + "deceleration_interval");
6767
// geometric pull out
6868
p.enable_geometric_pull_out = node->declare_parameter<bool>(ns + "enable_geometric_pull_out");
69+
p.geometric_collision_check_distance_from_end =
70+
node->declare_parameter<double>(ns + "geometric_collision_check_distance_from_end");
6971
p.divide_pull_out_path = node->declare_parameter<bool>(ns + "divide_pull_out_path");
7072
p.parallel_parking_parameters.pull_out_velocity =
7173
node->declare_parameter<double>(ns + "geometric_pull_out_velocity");

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

+7-17
Original file line numberDiff line numberDiff line change
@@ -69,25 +69,15 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
6969
pull_out_path.partial_paths.front(); // shift path is not separate but only one.
7070

7171
// check lane_departure with path between pull_out_start to pull_out_end
72-
PathWithLaneId path_start_to_end{};
72+
PathWithLaneId path_shift_start_to_end{};
7373
{
7474
const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position);
75+
const size_t pull_out_end_idx =
76+
findNearestIndex(shift_path.points, pull_out_path.end_pose.position);
7577

76-
// calculate collision check end idx
77-
const size_t collision_check_end_idx = std::invoke([&]() {
78-
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
79-
shift_path.points, pull_out_path.end_pose.position,
80-
parameters_.collision_check_distance_from_end);
81-
82-
if (collision_check_end_pose) {
83-
return findNearestIndex(shift_path.points, collision_check_end_pose->position);
84-
} else {
85-
return shift_path.points.size() - 1;
86-
}
87-
});
88-
path_start_to_end.points.insert(
89-
path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
90-
shift_path.points.begin() + collision_check_end_idx + 1);
78+
path_shift_start_to_end.points.insert(
79+
path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx,
80+
shift_path.points.begin() + pull_out_end_idx + 1);
9181
}
9282

9383
// extract shoulder lanes from pull out lanes
@@ -131,7 +121,7 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
131121
// check lane departure
132122
if (
133123
parameters_.check_shift_path_lane_departure &&
134-
lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) {
124+
lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_shift_start_to_end)) {
135125
continue;
136126
}
137127

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+19-5
Original file line numberDiff line numberDiff line change
@@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath(
657657

658658
// check collision
659659
if (utils::checkCollisionBetweenPathFootprintsAndObjects(
660-
vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects,
661-
collision_check_margin)) {
660+
vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()),
661+
pull_out_lane_stop_objects, collision_check_margin)) {
662662
return false;
663663
}
664664

@@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath(
672672
return true;
673673
}
674674

675-
PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path)
675+
PathWithLaneId StartPlannerModule::extractCollisionCheckSection(
676+
const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type)
676677
{
678+
const std::map<PlannerType, double> collision_check_distances = {
679+
{behavior_path_planner::PlannerType::SHIFT,
680+
parameters_->shift_collision_check_distance_from_end},
681+
{behavior_path_planner::PlannerType::GEOMETRIC,
682+
parameters_->geometric_collision_check_distance_from_end}};
683+
684+
const double collision_check_distance_from_end = collision_check_distances.at(planner_type);
685+
677686
PathWithLaneId combined_path;
678687
for (const auto & partial_path : path.partial_paths) {
679688
combined_path.points.insert(
@@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat
682691
// calculate collision check end idx
683692
const size_t collision_check_end_idx = std::invoke([&]() {
684693
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
685-
combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end);
694+
combined_path.points, path.end_pose.position, collision_check_distance_from_end);
686695

687696
if (collision_check_end_pose) {
688697
return motion_utils::findNearestIndex(
@@ -1371,9 +1380,14 @@ void StartPlannerModule::setDebugData()
13711380
// visualize collision_check_end_pose and footprint
13721381
{
13731382
const auto local_footprint = vehicle_info_.createFootprint();
1383+
std::map<PlannerType, double> collision_check_distances = {
1384+
{PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end},
1385+
{PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}};
1386+
1387+
double collision_check_distance_from_end = collision_check_distances[status_.planner_type];
13741388
const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose(
13751389
getFullPath().points, status_.pull_out_path.end_pose.position,
1376-
parameters_->collision_check_distance_from_end);
1390+
collision_check_distance_from_end);
13771391
if (collision_check_end_pose) {
13781392
add(createPoseMarkerArray(
13791393
*collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0));

0 commit comments

Comments
 (0)