Skip to content

Commit f2a4129

Browse files
kyoichi-sugaharakyoichi-sugahara
authored
refactor(autoware_behavior_path_start_planner_module): add data_structs.cpp and init method for StartPlannerParameters (#9736)
feat(autoware_behavior_path_start_planner_module): add data_structs.cpp and init method for StartPlannerParameters Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
1 parent 57f38b7 commit f2a4129

File tree

4 files changed

+346
-304
lines changed

4 files changed

+346
-304
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
1212
src/freespace_pull_out.cpp
1313
src/geometric_pull_out.cpp
1414
src/shift_pull_out.cpp
15+
src/data_structs.cpp
1516
src/util.cpp
1617
)
1718

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,7 @@ struct StartPlannerDebugData
9797

9898
struct StartPlannerParameters
9999
{
100+
static StartPlannerParameters init(rclcpp::Node & node);
100101
double th_arrived_distance{0.0};
101102
double th_stopped_velocity{0.0};
102103
double th_stopped_time{0.0};

‎planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp

+340
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,340 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "autoware/behavior_path_start_planner_module/data_structs.hpp"
16+
17+
#include "autoware/behavior_path_start_planner_module/manager.hpp"
18+
19+
#include <autoware/universe_utils/ros/parameter.hpp>
20+
#include <rclcpp/rclcpp.hpp>
21+
22+
#include <string>
23+
24+
namespace autoware::behavior_path_planner
25+
{
26+
27+
StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node)
28+
{
29+
using autoware::universe_utils::getOrDeclareParameter;
30+
StartPlannerParameters p;
31+
{
32+
const std::string ns = "start_planner.";
33+
34+
p.th_arrived_distance = getOrDeclareParameter<double>(node, ns + "th_arrived_distance");
35+
p.th_stopped_velocity = getOrDeclareParameter<double>(node, ns + "th_stopped_velocity");
36+
p.th_stopped_time = getOrDeclareParameter<double>(node, ns + "th_stopped_time");
37+
p.prepare_time_before_start =
38+
getOrDeclareParameter<double>(node, ns + "prepare_time_before_start");
39+
p.th_distance_to_middle_of_the_road =
40+
getOrDeclareParameter<double>(node, ns + "th_distance_to_middle_of_the_road");
41+
p.skip_rear_vehicle_check = getOrDeclareParameter<bool>(node, ns + "skip_rear_vehicle_check");
42+
p.extra_width_margin_for_rear_obstacle =
43+
getOrDeclareParameter<double>(node, ns + "extra_width_margin_for_rear_obstacle");
44+
p.collision_check_margins =
45+
getOrDeclareParameter<std::vector<double>>(node, ns + "collision_check_margins");
46+
p.collision_check_margin_from_front_object =
47+
getOrDeclareParameter<double>(node, ns + "collision_check_margin_from_front_object");
48+
p.th_moving_object_velocity =
49+
getOrDeclareParameter<double>(node, ns + "th_moving_object_velocity");
50+
p.center_line_path_interval =
51+
getOrDeclareParameter<double>(node, ns + "center_line_path_interval");
52+
// shift pull out
53+
p.enable_shift_pull_out = getOrDeclareParameter<bool>(node, ns + "enable_shift_pull_out");
54+
p.check_shift_path_lane_departure =
55+
getOrDeclareParameter<bool>(node, ns + "check_shift_path_lane_departure");
56+
p.allow_check_shift_path_lane_departure_override =
57+
getOrDeclareParameter<bool>(node, ns + "allow_check_shift_path_lane_departure_override");
58+
p.shift_collision_check_distance_from_end =
59+
getOrDeclareParameter<double>(node, ns + "shift_collision_check_distance_from_end");
60+
p.minimum_shift_pull_out_distance =
61+
getOrDeclareParameter<double>(node, ns + "minimum_shift_pull_out_distance");
62+
p.lateral_acceleration_sampling_num =
63+
getOrDeclareParameter<int>(node, ns + "lateral_acceleration_sampling_num");
64+
p.lateral_jerk = getOrDeclareParameter<double>(node, ns + "lateral_jerk");
65+
p.maximum_lateral_acc = getOrDeclareParameter<double>(node, ns + "maximum_lateral_acc");
66+
p.minimum_lateral_acc = getOrDeclareParameter<double>(node, ns + "minimum_lateral_acc");
67+
p.maximum_curvature = getOrDeclareParameter<double>(node, ns + "maximum_curvature");
68+
p.end_pose_curvature_threshold =
69+
getOrDeclareParameter<double>(node, ns + "end_pose_curvature_threshold");
70+
p.maximum_longitudinal_deviation =
71+
getOrDeclareParameter<double>(node, ns + "maximum_longitudinal_deviation");
72+
// geometric pull out
73+
p.enable_geometric_pull_out =
74+
getOrDeclareParameter<bool>(node, ns + "enable_geometric_pull_out");
75+
p.geometric_collision_check_distance_from_end =
76+
getOrDeclareParameter<double>(node, ns + "geometric_collision_check_distance_from_end");
77+
p.divide_pull_out_path = getOrDeclareParameter<bool>(node, ns + "divide_pull_out_path");
78+
p.parallel_parking_parameters.pull_out_velocity =
79+
getOrDeclareParameter<double>(node, ns + "geometric_pull_out_velocity");
80+
p.parallel_parking_parameters.pull_out_arc_path_interval =
81+
getOrDeclareParameter<double>(node, ns + "arc_path_interval");
82+
p.parallel_parking_parameters.pull_out_lane_departure_margin =
83+
getOrDeclareParameter<double>(node, ns + "lane_departure_margin");
84+
p.lane_departure_check_expansion_margin =
85+
getOrDeclareParameter<double>(node, ns + "lane_departure_check_expansion_margin");
86+
p.parallel_parking_parameters.pull_out_max_steer_angle =
87+
getOrDeclareParameter<double>(node, ns + "pull_out_max_steer_angle"); // 15deg
88+
p.parallel_parking_parameters.center_line_path_interval =
89+
p.center_line_path_interval; // for geometric parallel parking
90+
// search start pose backward
91+
p.search_priority = getOrDeclareParameter<std::string>(
92+
node,
93+
ns + "search_priority"); // "efficient_path" or "short_back_distance"
94+
p.enable_back = getOrDeclareParameter<bool>(node, ns + "enable_back");
95+
p.backward_velocity = getOrDeclareParameter<double>(node, ns + "backward_velocity");
96+
p.max_back_distance = getOrDeclareParameter<double>(node, ns + "max_back_distance");
97+
p.backward_search_resolution =
98+
getOrDeclareParameter<double>(node, ns + "backward_search_resolution");
99+
p.backward_path_update_duration =
100+
getOrDeclareParameter<double>(node, ns + "backward_path_update_duration");
101+
p.ignore_distance_from_lane_end =
102+
getOrDeclareParameter<double>(node, ns + "ignore_distance_from_lane_end");
103+
// stop condition
104+
p.maximum_deceleration_for_stop =
105+
getOrDeclareParameter<double>(node, ns + "stop_condition.maximum_deceleration_for_stop");
106+
p.maximum_jerk_for_stop =
107+
getOrDeclareParameter<double>(node, ns + "stop_condition.maximum_jerk_for_stop");
108+
}
109+
{
110+
const std::string ns = "start_planner.object_types_to_check_for_path_generation.";
111+
p.object_types_to_check_for_path_generation.check_car =
112+
getOrDeclareParameter<bool>(node, ns + "check_car");
113+
p.object_types_to_check_for_path_generation.check_truck =
114+
getOrDeclareParameter<bool>(node, ns + "check_truck");
115+
p.object_types_to_check_for_path_generation.check_bus =
116+
getOrDeclareParameter<bool>(node, ns + "check_bus");
117+
p.object_types_to_check_for_path_generation.check_trailer =
118+
getOrDeclareParameter<bool>(node, ns + "check_trailer");
119+
p.object_types_to_check_for_path_generation.check_unknown =
120+
getOrDeclareParameter<bool>(node, ns + "check_unknown");
121+
p.object_types_to_check_for_path_generation.check_bicycle =
122+
getOrDeclareParameter<bool>(node, ns + "check_bicycle");
123+
p.object_types_to_check_for_path_generation.check_motorcycle =
124+
getOrDeclareParameter<bool>(node, ns + "check_motorcycle");
125+
p.object_types_to_check_for_path_generation.check_pedestrian =
126+
getOrDeclareParameter<bool>(node, ns + "check_pedestrian");
127+
}
128+
// freespace planner general params
129+
{
130+
const std::string ns = "start_planner.freespace_planner.";
131+
p.enable_freespace_planner = getOrDeclareParameter<bool>(node, ns + "enable_freespace_planner");
132+
p.freespace_planner_algorithm =
133+
getOrDeclareParameter<std::string>(node, ns + "freespace_planner_algorithm");
134+
p.end_pose_search_start_distance =
135+
getOrDeclareParameter<double>(node, ns + "end_pose_search_start_distance");
136+
p.end_pose_search_end_distance =
137+
getOrDeclareParameter<double>(node, ns + "end_pose_search_end_distance");
138+
p.end_pose_search_interval =
139+
getOrDeclareParameter<double>(node, ns + "end_pose_search_interval");
140+
p.freespace_planner_velocity = getOrDeclareParameter<double>(node, ns + "velocity");
141+
p.vehicle_shape_margin = getOrDeclareParameter<double>(node, ns + "vehicle_shape_margin");
142+
p.freespace_planner_common_parameters.time_limit =
143+
getOrDeclareParameter<double>(node, ns + "time_limit");
144+
p.freespace_planner_common_parameters.max_turning_ratio =
145+
getOrDeclareParameter<double>(node, ns + "max_turning_ratio");
146+
p.freespace_planner_common_parameters.turning_steps =
147+
getOrDeclareParameter<int>(node, ns + "turning_steps");
148+
}
149+
// freespace planner search config
150+
{
151+
const std::string ns = "start_planner.freespace_planner.search_configs.";
152+
p.freespace_planner_common_parameters.theta_size =
153+
getOrDeclareParameter<int>(node, ns + "theta_size");
154+
p.freespace_planner_common_parameters.angle_goal_range =
155+
getOrDeclareParameter<double>(node, ns + "angle_goal_range");
156+
p.freespace_planner_common_parameters.curve_weight =
157+
getOrDeclareParameter<double>(node, ns + "curve_weight");
158+
p.freespace_planner_common_parameters.reverse_weight =
159+
getOrDeclareParameter<double>(node, ns + "reverse_weight");
160+
p.freespace_planner_common_parameters.lateral_goal_range =
161+
getOrDeclareParameter<double>(node, ns + "lateral_goal_range");
162+
p.freespace_planner_common_parameters.longitudinal_goal_range =
163+
getOrDeclareParameter<double>(node, ns + "longitudinal_goal_range");
164+
}
165+
// freespace planner costmap configs
166+
{
167+
const std::string ns = "start_planner.freespace_planner.costmap_configs.";
168+
p.freespace_planner_common_parameters.obstacle_threshold =
169+
getOrDeclareParameter<int>(node, ns + "obstacle_threshold");
170+
}
171+
// freespace planner astar
172+
{
173+
const std::string ns = "start_planner.freespace_planner.astar.";
174+
p.astar_parameters.search_method =
175+
getOrDeclareParameter<std::string>(node, ns + "search_method");
176+
p.astar_parameters.only_behind_solutions =
177+
getOrDeclareParameter<bool>(node, ns + "only_behind_solutions");
178+
p.astar_parameters.use_back = getOrDeclareParameter<bool>(node, ns + "use_back");
179+
p.astar_parameters.distance_heuristic_weight =
180+
getOrDeclareParameter<double>(node, ns + "distance_heuristic_weight");
181+
}
182+
// freespace planner rrtstar
183+
{
184+
const std::string ns = "start_planner.freespace_planner.rrtstar.";
185+
p.rrt_star_parameters.enable_update = getOrDeclareParameter<bool>(node, ns + "enable_update");
186+
p.rrt_star_parameters.use_informed_sampling =
187+
getOrDeclareParameter<bool>(node, ns + "use_informed_sampling");
188+
p.rrt_star_parameters.max_planning_time =
189+
getOrDeclareParameter<double>(node, ns + "max_planning_time");
190+
p.rrt_star_parameters.neighbor_radius =
191+
getOrDeclareParameter<double>(node, ns + "neighbor_radius");
192+
p.rrt_star_parameters.margin = getOrDeclareParameter<double>(node, ns + "margin");
193+
}
194+
195+
const std::string base_ns = "start_planner.path_safety_check.";
196+
// EgoPredictedPath
197+
{
198+
const std::string ego_path_ns = base_ns + "ego_predicted_path.";
199+
p.ego_predicted_path_params.min_velocity =
200+
getOrDeclareParameter<double>(node, ego_path_ns + "min_velocity");
201+
p.ego_predicted_path_params.acceleration =
202+
getOrDeclareParameter<double>(node, ego_path_ns + "min_acceleration");
203+
p.ego_predicted_path_params.time_horizon_for_front_object =
204+
getOrDeclareParameter<double>(node, ego_path_ns + "time_horizon_for_front_object");
205+
p.ego_predicted_path_params.time_horizon_for_rear_object =
206+
getOrDeclareParameter<double>(node, ego_path_ns + "time_horizon_for_rear_object");
207+
p.ego_predicted_path_params.time_resolution =
208+
getOrDeclareParameter<double>(node, ego_path_ns + "time_resolution");
209+
p.ego_predicted_path_params.delay_until_departure =
210+
getOrDeclareParameter<double>(node, ego_path_ns + "delay_until_departure");
211+
}
212+
// ObjectFilteringParams
213+
const std::string obj_filter_ns = base_ns + "target_filtering.";
214+
{
215+
p.objects_filtering_params.safety_check_time_horizon =
216+
getOrDeclareParameter<double>(node, obj_filter_ns + "safety_check_time_horizon");
217+
p.objects_filtering_params.safety_check_time_resolution =
218+
getOrDeclareParameter<double>(node, obj_filter_ns + "safety_check_time_resolution");
219+
p.objects_filtering_params.object_check_forward_distance =
220+
getOrDeclareParameter<double>(node, obj_filter_ns + "object_check_forward_distance");
221+
p.objects_filtering_params.object_check_backward_distance =
222+
getOrDeclareParameter<double>(node, obj_filter_ns + "object_check_backward_distance");
223+
p.objects_filtering_params.ignore_object_velocity_threshold =
224+
getOrDeclareParameter<double>(node, obj_filter_ns + "ignore_object_velocity_threshold");
225+
p.objects_filtering_params.include_opposite_lane =
226+
getOrDeclareParameter<bool>(node, obj_filter_ns + "include_opposite_lane");
227+
p.objects_filtering_params.invert_opposite_lane =
228+
getOrDeclareParameter<bool>(node, obj_filter_ns + "invert_opposite_lane");
229+
p.objects_filtering_params.check_all_predicted_path =
230+
getOrDeclareParameter<bool>(node, obj_filter_ns + "check_all_predicted_path");
231+
p.objects_filtering_params.use_all_predicted_path =
232+
getOrDeclareParameter<bool>(node, obj_filter_ns + "use_all_predicted_path");
233+
p.objects_filtering_params.use_predicted_path_outside_lanelet =
234+
getOrDeclareParameter<bool>(node, obj_filter_ns + "use_predicted_path_outside_lanelet");
235+
}
236+
// ObjectTypesToCheck
237+
{
238+
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
239+
p.objects_filtering_params.object_types_to_check.check_car =
240+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_car");
241+
p.objects_filtering_params.object_types_to_check.check_truck =
242+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_truck");
243+
p.objects_filtering_params.object_types_to_check.check_bus =
244+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_bus");
245+
p.objects_filtering_params.object_types_to_check.check_trailer =
246+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_trailer");
247+
p.objects_filtering_params.object_types_to_check.check_unknown =
248+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_unknown");
249+
p.objects_filtering_params.object_types_to_check.check_bicycle =
250+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_bicycle");
251+
p.objects_filtering_params.object_types_to_check.check_motorcycle =
252+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_motorcycle");
253+
p.objects_filtering_params.object_types_to_check.check_pedestrian =
254+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_pedestrian");
255+
}
256+
// ObjectLaneConfiguration
257+
{
258+
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
259+
p.objects_filtering_params.object_lane_configuration.check_current_lane =
260+
getOrDeclareParameter<bool>(node, obj_lane_ns + "check_current_lane");
261+
p.objects_filtering_params.object_lane_configuration.check_right_lane =
262+
getOrDeclareParameter<bool>(node, obj_lane_ns + "check_right_side_lane");
263+
p.objects_filtering_params.object_lane_configuration.check_left_lane =
264+
getOrDeclareParameter<bool>(node, obj_lane_ns + "check_left_side_lane");
265+
p.objects_filtering_params.object_lane_configuration.check_shoulder_lane =
266+
getOrDeclareParameter<bool>(node, obj_lane_ns + "check_shoulder_lane");
267+
p.objects_filtering_params.object_lane_configuration.check_other_lane =
268+
getOrDeclareParameter<bool>(node, obj_lane_ns + "check_other_lane");
269+
}
270+
// SafetyCheckParams
271+
const std::string safety_check_ns = base_ns + "safety_check_params.";
272+
{
273+
p.safety_check_params.enable_safety_check =
274+
getOrDeclareParameter<bool>(node, safety_check_ns + "enable_safety_check");
275+
p.safety_check_params.hysteresis_factor_expand_rate =
276+
getOrDeclareParameter<double>(node, safety_check_ns + "hysteresis_factor_expand_rate");
277+
p.safety_check_params.backward_path_length =
278+
getOrDeclareParameter<double>(node, safety_check_ns + "backward_path_length");
279+
p.safety_check_params.forward_path_length =
280+
getOrDeclareParameter<double>(node, safety_check_ns + "forward_path_length");
281+
p.safety_check_params.publish_debug_marker =
282+
getOrDeclareParameter<bool>(node, safety_check_ns + "publish_debug_marker");
283+
p.safety_check_params.collision_check_yaw_diff_threshold =
284+
getOrDeclareParameter<double>(node, safety_check_ns + "collision_check_yaw_diff_threshold");
285+
}
286+
// RSSparams
287+
{
288+
const std::string rss_ns = safety_check_ns + "rss_params.";
289+
p.safety_check_params.rss_params.rear_vehicle_reaction_time =
290+
getOrDeclareParameter<double>(node, rss_ns + "rear_vehicle_reaction_time");
291+
p.safety_check_params.rss_params.rear_vehicle_safety_time_margin =
292+
getOrDeclareParameter<double>(node, rss_ns + "rear_vehicle_safety_time_margin");
293+
p.safety_check_params.rss_params.lateral_distance_max_threshold =
294+
getOrDeclareParameter<double>(node, rss_ns + "lateral_distance_max_threshold");
295+
p.safety_check_params.rss_params.longitudinal_distance_min_threshold =
296+
getOrDeclareParameter<double>(node, rss_ns + "longitudinal_distance_min_threshold");
297+
p.safety_check_params.rss_params.longitudinal_velocity_delta_time =
298+
getOrDeclareParameter<double>(node, rss_ns + "longitudinal_velocity_delta_time");
299+
p.safety_check_params.rss_params.extended_polygon_policy =
300+
getOrDeclareParameter<std::string>(node, rss_ns + "extended_polygon_policy");
301+
}
302+
// surround moving obstacle check
303+
{
304+
const std::string surround_moving_obstacle_check_ns =
305+
"start_planner.surround_moving_obstacle_check.";
306+
p.search_radius =
307+
getOrDeclareParameter<double>(node, surround_moving_obstacle_check_ns + "search_radius");
308+
p.th_moving_obstacle_velocity = getOrDeclareParameter<double>(
309+
node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity");
310+
// ObjectTypesToCheck
311+
{
312+
const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
313+
p.surround_moving_obstacles_type_to_check.check_car =
314+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_car");
315+
p.surround_moving_obstacles_type_to_check.check_truck =
316+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_truck");
317+
p.surround_moving_obstacles_type_to_check.check_bus =
318+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_bus");
319+
p.surround_moving_obstacles_type_to_check.check_trailer =
320+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_trailer");
321+
p.surround_moving_obstacles_type_to_check.check_unknown =
322+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_unknown");
323+
p.surround_moving_obstacles_type_to_check.check_bicycle =
324+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_bicycle");
325+
p.surround_moving_obstacles_type_to_check.check_motorcycle =
326+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_motorcycle");
327+
p.surround_moving_obstacles_type_to_check.check_pedestrian =
328+
getOrDeclareParameter<bool>(node, obj_types_ns + "check_pedestrian");
329+
}
330+
}
331+
332+
// debug
333+
{
334+
const std::string debug_ns = "start_planner.debug.";
335+
p.print_debug_info = getOrDeclareParameter<bool>(node, debug_ns + "print_debug_info");
336+
}
337+
338+
return p;
339+
}
340+
} // namespace autoware::behavior_path_planner

0 commit comments

Comments
 (0)