Skip to content

Commit ebb4172

Browse files
kyoichi-sugaharakyoichi-sugahara
authored
feat(start_planner): add object_types_to_check_for_path_generation (autowarefoundation#6370)
* add object_types_to_check_for_path_generation Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 833722b commit ebb4172

File tree

5 files changed

+71
-3
lines changed

5 files changed

+71
-3
lines changed

‎planning/behavior_path_start_planner_module/README.md

+7
Original file line numberDiff line numberDiff line change
@@ -332,6 +332,13 @@ PullOutPath --o PullOutPlannerBase
332332
| shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 |
333333
| geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 |
334334
| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 |
335+
| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true |
336+
| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true |
337+
| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true |
338+
| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true |
339+
| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true |
340+
| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true |
341+
| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true |
335342
| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 |
336343

337344
### **Ego vehicle's velocity planning**

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

+9
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,15 @@
88
collision_check_margins: [2.0, 1.0, 0.5, 0.1]
99
collision_check_margin_from_front_object: 5.0
1010
th_moving_object_velocity: 1.0
11+
object_types_to_check_for_path_generation:
12+
check_car: true
13+
check_truck: true
14+
check_bus: true
15+
check_trailer: true
16+
check_bicycle: true
17+
check_motorcycle: true
18+
check_pedestrian: true
19+
check_unknown: true
1120
th_distance_to_middle_of_the_road: 0.5
1221
center_line_path_interval: 1.0
1322
# shift pull out

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

+2
Original file line numberDiff line numberDiff line change
@@ -67,6 +67,8 @@ struct StartPlannerParameters
6767
std::vector<double> collision_check_margins{};
6868
double collision_check_margin_from_front_object{0.0};
6969
double th_moving_object_velocity{0.0};
70+
behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck
71+
object_types_to_check_for_path_generation{};
7072
double center_line_path_interval{0.0};
7173

7274
// shift pull out

‎planning/behavior_path_start_planner_module/src/manager.cpp

+46
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,25 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
4949
p.collision_check_margin_from_front_object =
5050
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
5151
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
52+
{
53+
const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
54+
p.object_types_to_check_for_path_generation.check_car =
55+
node->declare_parameter<bool>(ns + "check_car");
56+
p.object_types_to_check_for_path_generation.check_truck =
57+
node->declare_parameter<bool>(ns + "check_truck");
58+
p.object_types_to_check_for_path_generation.check_bus =
59+
node->declare_parameter<bool>(ns + "check_bus");
60+
p.object_types_to_check_for_path_generation.check_trailer =
61+
node->declare_parameter<bool>(ns + "check_trailer");
62+
p.object_types_to_check_for_path_generation.check_unknown =
63+
node->declare_parameter<bool>(ns + "check_unknown");
64+
p.object_types_to_check_for_path_generation.check_bicycle =
65+
node->declare_parameter<bool>(ns + "check_bicycle");
66+
p.object_types_to_check_for_path_generation.check_motorcycle =
67+
node->declare_parameter<bool>(ns + "check_motorcycle");
68+
p.object_types_to_check_for_path_generation.check_pedestrian =
69+
node->declare_parameter<bool>(ns + "check_pedestrian");
70+
}
5271
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
5372
// shift pull out
5473
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
@@ -358,6 +377,33 @@ void StartPlannerModuleManager::updateModuleParams(
358377
parameters, ns + "collision_check_margin_from_front_object",
359378
p->collision_check_margin_from_front_object);
360379
updateParam<double>(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity);
380+
const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation.";
381+
{
382+
updateParam<bool>(
383+
parameters, obj_types_ns + "check_car",
384+
p->object_types_to_check_for_path_generation.check_car);
385+
updateParam<bool>(
386+
parameters, obj_types_ns + "check_truck",
387+
p->object_types_to_check_for_path_generation.check_truck);
388+
updateParam<bool>(
389+
parameters, obj_types_ns + "check_bus",
390+
p->object_types_to_check_for_path_generation.check_bus);
391+
updateParam<bool>(
392+
parameters, obj_types_ns + "check_trailer",
393+
p->object_types_to_check_for_path_generation.check_trailer);
394+
updateParam<bool>(
395+
parameters, obj_types_ns + "check_unknown",
396+
p->object_types_to_check_for_path_generation.check_unknown);
397+
updateParam<bool>(
398+
parameters, obj_types_ns + "check_bicycle",
399+
p->object_types_to_check_for_path_generation.check_bicycle);
400+
updateParam<bool>(
401+
parameters, obj_types_ns + "check_motorcycle",
402+
p->object_types_to_check_for_path_generation.check_motorcycle);
403+
updateParam<bool>(
404+
parameters, obj_types_ns + "check_pedestrian",
405+
p->object_types_to_check_for_path_generation.check_pedestrian);
406+
}
361407
updateParam<double>(parameters, ns + "center_line_path_interval", p->center_line_path_interval);
362408
updateParam<bool>(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out);
363409
updateParam<double>(

‎planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -668,9 +668,10 @@ bool StartPlannerModule::findPullOutPath(
668668
// extract stop objects in pull out lane for collision check
669669
const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
670670
*dynamic_objects, parameters_->th_moving_object_velocity);
671-
const auto [pull_out_lane_stop_objects, others] =
672-
utils::path_safety_checker::separateObjectsByLanelets(
673-
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
671+
auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets(
672+
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
673+
utils::path_safety_checker::filterObjectsByClass(
674+
pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation);
674675

675676
// if start_pose_candidate is far from refined_start_pose, backward driving is necessary
676677
const bool backward_is_unnecessary =
@@ -1036,6 +1037,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
10361037
stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance,
10371038
object_check_backward_distance);
10381039

1040+
utils::path_safety_checker::filterObjectsByClass(
1041+
stop_objects_in_pull_out_lanes, parameters_->object_types_to_check_for_path_generation);
1042+
10391043
return stop_objects_in_pull_out_lanes;
10401044
}
10411045

0 commit comments

Comments
 (0)