Skip to content

Commit 04ef74c

Browse files
kyoichi-sugaharakyoichi-sugahara
committed
Update collision check margins in start planner module
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 1aee603 commit 04ef74c

File tree

5 files changed

+27
-22
lines changed

5 files changed

+27
-22
lines changed

‎planning/behavior_path_start_planner_module/README.md

+14-13
Original file line numberDiff line numberDiff line change
@@ -65,19 +65,20 @@ PullOutPath --o PullOutPlannerBase
6565

6666
## General parameters for start_planner
6767

68-
| Name | Unit | Type | Description | Default value |
69-
| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- |
70-
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
71-
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
72-
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
73-
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
74-
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
75-
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
76-
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
77-
| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] |
78-
| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 |
79-
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
80-
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
68+
| Name | Unit | Type | Description | Default value |
69+
| :---------------------------------------------------------- | :---- | :------- | :---------------------------------------------------------------------------------------------------------------------- | :------------------- |
70+
| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 |
71+
| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 |
72+
| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 |
73+
| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 |
74+
| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 |
75+
| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 |
76+
| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 |
77+
| front_objects_collision_check_margins | [m] | [double] | obstacle collision check margins against front static objects 　 from the footprint on the trajectory in pull_out_lanes | [2.0, 1.0, 0.5, 0.1] |
78+
| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 |
79+
| back_objects_collision_check_margin | [m] | double | obstacle collision check margins against back static objects from footprint on the start pose in pull_out_lanes | 3.0 |
80+
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
81+
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
8182

8283
## Safety check with static obstacles
8384

‎planning/behavior_path_start_planner_module/config/start_planner.param.yaml

+2-1
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,9 @@
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]
8+
front_objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1]
99
collision_check_distance_from_end: 1.0
10+
back_objects_collision_check_margin: 3.0
1011
collision_check_margin_from_front_object: 5.0
1112
th_moving_object_velocity: 1.0
1213
th_distance_to_middle_of_the_road: 0.5

‎planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -62,8 +62,9 @@ struct StartPlannerParameters
6262
double th_distance_to_middle_of_the_road{0.0};
6363
double intersection_search_length{0.0};
6464
double length_ratio_for_turn_signal_deactivation_near_intersection{0.0};
65-
std::vector<double> collision_check_margins{};
65+
std::vector<double> front_objects_collision_check_margins{};
6666
double collision_check_distance_from_end{0.0};
67+
double back_objects_collision_check_margin{0.0};
6768
double collision_check_margin_from_front_object{0.0};
6869
double th_moving_object_velocity{0.0};
6970
double center_line_path_interval{0.0};

‎planning/behavior_path_start_planner_module/src/manager.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -43,10 +43,12 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
4343
p.intersection_search_length = node->declare_parameter<double>(ns + "intersection_search_length");
4444
p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter<double>(
4545
ns + "length_ratio_for_turn_signal_deactivation_near_intersection");
46-
p.collision_check_margins =
47-
node->declare_parameter<std::vector<double>>(ns + "collision_check_margins");
46+
p.front_objects_collision_check_margins =
47+
node->declare_parameter<std::vector<double>>(ns + "front_objects_collision_check_margins");
4848
p.collision_check_distance_from_end =
4949
node->declare_parameter<double>(ns + "collision_check_distance_from_end");
50+
p.back_objects_collision_check_margin =
51+
node->declare_parameter<double>(ns + "back_objects_collision_check_margin");
5052
p.collision_check_margin_from_front_object =
5153
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
5254
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");

‎planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -595,7 +595,7 @@ void StartPlannerModule::planWithPriority(
595595
const PriorityOrder order_priority =
596596
determinePriorityOrder(search_priority, start_pose_candidates.size());
597597

598-
for (const auto & collision_check_margin : parameters_->collision_check_margins) {
598+
for (const auto & collision_check_margin : parameters_->front_objects_collision_check_margins) {
599599
for (const auto & [index, planner] : order_priority) {
600600
if (findPullOutPath(
601601
start_pose_candidates[index], planner, refined_start_pose, goal_pose,
@@ -923,9 +923,9 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
923923
const double backward_path_length =
924924
planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
925925

926-
const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
926+
const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
927927
pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity,
928-
backward_path_length, std::numeric_limits<double>::max());
928+
backward_path_length, 0);
929929

930930
const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes(
931931
pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0,
@@ -964,8 +964,8 @@ std::vector<Pose> StartPlannerModule::searchPullOutStartPoseCandidates(
964964
}
965965

966966
if (utils::checkCollisionBetweenFootprintAndObjects(
967-
local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes,
968-
parameters_->collision_check_margins.back())) {
967+
local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes,
968+
parameters_->back_objects_collision_check_margin)) {
969969
break; // poses behind this has a collision, so break.
970970
}
971971

0 commit comments

Comments
 (0)