Skip to content

Commit e8ec8f0

Browse files
add object_types_to_check_for_path_generation
Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 4409d20 commit e8ec8f0

File tree

4 files changed

+32
-0
lines changed

4 files changed

+32
-0
lines changed

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
@@ -66,6 +66,8 @@ struct StartPlannerParameters
6666
std::vector<double> collision_check_margins{};
6767
double collision_check_margin_from_front_object{0.0};
6868
double th_moving_object_velocity{0.0};
69+
behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck
70+
object_types_to_check_for_path_generation{};
6971
double center_line_path_interval{0.0};
7072

7173
// shift pull out

planning/behavior_path_start_planner_module/src/manager.cpp

+16
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,22 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
4848
p.collision_check_margin_from_front_object =
4949
node->declare_parameter<double>(ns + "collision_check_margin_from_front_object");
5050
p.th_moving_object_velocity = node->declare_parameter<double>(ns + "th_moving_object_velocity");
51+
p.object_types_to_check_for_path_generation.check_car =
52+
node->declare_parameter<bool>(ns + "check_car");
53+
p.object_types_to_check_for_path_generation.check_truck =
54+
node->declare_parameter<bool>(ns + "check_truck");
55+
p.object_types_to_check_for_path_generation.check_bus =
56+
node->declare_parameter<bool>(ns + "check_bus");
57+
p.object_types_to_check_for_path_generation.check_trailer =
58+
node->declare_parameter<bool>(ns + "check_trailer");
59+
p.object_types_to_check_for_path_generation.check_unknown =
60+
node->declare_parameter<bool>(ns + "check_unknown");
61+
p.object_types_to_check_for_path_generation.check_bicycle =
62+
node->declare_parameter<bool>(ns + "check_bicycle");
63+
p.object_types_to_check_for_path_generation.check_motorcycle =
64+
node->declare_parameter<bool>(ns + "check_motorcycle");
65+
p.object_types_to_check_for_path_generation.check_pedestrian =
66+
node->declare_parameter<bool>(ns + "check_pedestrian");
5167
p.center_line_path_interval = node->declare_parameter<double>(ns + "center_line_path_interval");
5268
// shift pull out
5369
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -679,6 +679,8 @@ bool StartPlannerModule::findPullOutPath(
679679
const auto [pull_out_lane_stop_objects, others] =
680680
utils::path_safety_checker::separateObjectsByLanelets(
681681
stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
682+
utils::path_safety_checker::filterObjectsByClass(
683+
pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation);
682684

683685
// if start_pose_candidate is far from refined_start_pose, backward driving is necessary
684686
const bool backward_is_unnecessary =
@@ -1035,6 +1037,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
10351037
stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance,
10361038
object_check_backward_distance);
10371039

1040+
utils::path_safety_checker::filterObjectsByClass(
1041+
stop_objects_in_pull_out_lanes, parameters_->object_types_to_check_for_path_generation);
1042+
10381043
return stop_objects_in_pull_out_lanes;
10391044
}
10401045

0 commit comments

Comments
 (0)