Skip to content

Commit 9e99178

Browse files
danielsanchezarandanielsanchezaran
committed
add support for rqt reconfig
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 754742e commit 9e99178

File tree

1 file changed

+282
-9
lines changed
  • planning/behavior_path_start_planner_module/src

1 file changed

+282
-9
lines changed

‎planning/behavior_path_start_planner_module/src/manager.cpp

+282-9
Original file line numberDiff line numberDiff line change
@@ -334,15 +334,288 @@ void StartPlannerModuleManager::updateModuleParams(
334334

335335
auto & p = parameters_;
336336

337-
[[maybe_unused]] const std::string ns = name_ + ".";
338-
339-
std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) {
340-
if (!observer.expired()) {
341-
const auto start_planner_ptr = std::dynamic_pointer_cast<StartPlannerModule>(observer.lock());
342-
if (start_planner_ptr) {
343-
start_planner_ptr->updateModuleParams(p);
344-
}
345-
}
337+
const std::string ns = "start_planner.";
338+
339+
{
340+
updateParam<double>(parameters, ns + "th_arrived_distance", p->th_arrived_distance);
341+
updateParam<double>(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity);
342+
updateParam<double>(parameters, ns + "th_stopped_time", p->th_stopped_time);
343+
updateParam<double>(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start);
344+
updateParam<double>(
345+
parameters, ns + "th_turn_signal_on_lateral_offset", p->th_turn_signal_on_lateral_offset);
346+
updateParam<double>(
347+
parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road);
348+
updateParam<double>(
349+
parameters, ns + "intersection_search_length", p->intersection_search_length);
350+
updateParam<double>(
351+
parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection",
352+
p->length_ratio_for_turn_signal_deactivation_near_intersection);
353+
354+
updateParam<double>(
355+
parameters, ns + "collision_check_margin_from_front_object",
356+
p->collision_check_margin_from_front_object);
357+
updateParam<double>(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity);
358+
updateParam<double>(parameters, ns + "center_line_path_interval", p->center_line_path_interval);
359+
updateParam<bool>(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out);
360+
updateParam<double>(
361+
parameters, ns + "shift_collision_check_distance_from_end",
362+
p->shift_collision_check_distance_from_end);
363+
updateParam<double>(
364+
parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance);
365+
updateParam<int>(
366+
parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num);
367+
updateParam<double>(parameters, ns + "lateral_jerk", p->lateral_jerk);
368+
updateParam<double>(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc);
369+
updateParam<double>(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc);
370+
updateParam<double>(parameters, ns + "maximum_curvature", p->maximum_curvature);
371+
updateParam<double>(parameters, ns + "deceleration_interval", p->deceleration_interval);
372+
updateParam<bool>(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out);
373+
updateParam<bool>(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path);
374+
updateParam<double>(
375+
parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval);
376+
updateParam<double>(
377+
parameters, ns + "lane_departure_margin",
378+
p->parallel_parking_parameters.pull_out_lane_departure_margin);
379+
updateParam<double>(
380+
parameters, ns + "pull_out_max_steer_angle",
381+
p->parallel_parking_parameters.pull_out_max_steer_angle);
382+
updateParam<bool>(parameters, ns + "enable_back", p->enable_back);
383+
updateParam<double>(parameters, ns + "max_back_distance", p->max_back_distance);
384+
updateParam<double>(
385+
parameters, ns + "backward_search_resolution", p->backward_search_resolution);
386+
updateParam<double>(
387+
parameters, ns + "backward_path_update_duration", p->backward_path_update_duration);
388+
updateParam<double>(
389+
parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end);
390+
}
391+
{
392+
const std::string ns = "start_planner.freespace_planner.";
393+
394+
updateParam<bool>(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner);
395+
updateParam<double>(
396+
parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance);
397+
updateParam<double>(
398+
parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance);
399+
updateParam<double>(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval);
400+
updateParam<double>(parameters, ns + "velocity", p->freespace_planner_velocity);
401+
updateParam<double>(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin);
402+
updateParam<double>(
403+
parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit);
404+
updateParam<double>(
405+
parameters, ns + "minimum_turning_radius",
406+
p->freespace_planner_common_parameters.minimum_turning_radius);
407+
updateParam<double>(
408+
parameters, ns + "maximum_turning_radius",
409+
p->freespace_planner_common_parameters.maximum_turning_radius);
410+
updateParam<int>(
411+
parameters, ns + "turning_radius_size",
412+
p->freespace_planner_common_parameters.turning_radius_size);
413+
}
414+
{
415+
const std::string ns = "start_planner.freespace_planner.search_configs.";
416+
417+
updateParam<int>(
418+
parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size);
419+
updateParam<double>(
420+
parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range);
421+
updateParam<double>(
422+
parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight);
423+
updateParam<double>(
424+
parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight);
425+
updateParam<double>(
426+
parameters, ns + "lateral_goal_range",
427+
p->freespace_planner_common_parameters.lateral_goal_range);
428+
updateParam<double>(
429+
parameters, ns + "longitudinal_goal_range",
430+
p->freespace_planner_common_parameters.longitudinal_goal_range);
431+
}
432+
433+
{
434+
const std::string ns = "start_planner.freespace_planner.costmap_configs.";
435+
436+
updateParam<int>(
437+
parameters, ns + "obstacle_threshold",
438+
p->freespace_planner_common_parameters.obstacle_threshold);
439+
}
440+
441+
{
442+
const std::string ns = "start_planner.freespace_planner.astar.";
443+
444+
updateParam<bool>(parameters, ns + "use_back", p->astar_parameters.use_back);
445+
}
446+
{
447+
const std::string ns = "start_planner.freespace_planner.rrtstar.";
448+
449+
updateParam<bool>(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update);
450+
updateParam<bool>(
451+
parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling);
452+
updateParam<double>(
453+
parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time);
454+
updateParam<double>(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius);
455+
updateParam<double>(parameters, ns + "margin", p->rrt_star_parameters.margin);
456+
updateParam<double>(
457+
parameters, ns + "stop_condition.maximum_deceleration_for_stop",
458+
p->maximum_deceleration_for_stop);
459+
updateParam<double>(
460+
parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop);
461+
}
462+
463+
const std::string base_ns = "start_planner.path_safety_check.";
464+
465+
const std::string ego_path_ns = base_ns + "ego_predicted_path.";
466+
467+
{
468+
updateParam<double>(
469+
parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity);
470+
updateParam<double>(
471+
parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration);
472+
updateParam<double>(
473+
parameters, ego_path_ns + "time_horizon_for_front_object",
474+
p->ego_predicted_path_params.time_horizon_for_front_object);
475+
updateParam<double>(
476+
parameters, ego_path_ns + "time_horizon_for_rear_object",
477+
p->ego_predicted_path_params.time_horizon_for_rear_object);
478+
updateParam<double>(
479+
parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution);
480+
updateParam<double>(
481+
parameters, ego_path_ns + "delay_until_departure",
482+
p->ego_predicted_path_params.delay_until_departure);
483+
}
484+
485+
const std::string obj_filter_ns = base_ns + "target_filtering.";
486+
487+
{
488+
updateParam<double>(
489+
parameters, obj_filter_ns + "safety_check_time_horizon",
490+
p->objects_filtering_params.safety_check_time_horizon);
491+
updateParam<double>(
492+
parameters, obj_filter_ns + "safety_check_time_resolution",
493+
p->objects_filtering_params.safety_check_time_resolution);
494+
updateParam<double>(
495+
parameters, obj_filter_ns + "object_check_forward_distance",
496+
p->objects_filtering_params.object_check_forward_distance);
497+
updateParam<double>(
498+
parameters, obj_filter_ns + "object_check_backward_distance",
499+
p->objects_filtering_params.object_check_backward_distance);
500+
updateParam<double>(
501+
parameters, obj_filter_ns + "ignore_object_velocity_threshold",
502+
p->objects_filtering_params.ignore_object_velocity_threshold);
503+
updateParam<bool>(
504+
parameters, obj_filter_ns + "include_opposite_lane",
505+
p->objects_filtering_params.include_opposite_lane);
506+
updateParam<bool>(
507+
parameters, obj_filter_ns + "invert_opposite_lane",
508+
p->objects_filtering_params.invert_opposite_lane);
509+
updateParam<bool>(
510+
parameters, obj_filter_ns + "check_all_predicted_path",
511+
p->objects_filtering_params.check_all_predicted_path);
512+
updateParam<bool>(
513+
parameters, obj_filter_ns + "use_all_predicted_path",
514+
p->objects_filtering_params.use_all_predicted_path);
515+
updateParam<bool>(
516+
parameters, obj_filter_ns + "use_predicted_path_outside_lanelet",
517+
p->objects_filtering_params.use_predicted_path_outside_lanelet);
518+
}
519+
520+
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
521+
522+
{
523+
updateParam<bool>(
524+
parameters, obj_types_ns + "check_car",
525+
p->objects_filtering_params.object_types_to_check.check_car);
526+
updateParam<bool>(
527+
parameters, obj_types_ns + "check_truck",
528+
p->objects_filtering_params.object_types_to_check.check_truck);
529+
updateParam<bool>(
530+
parameters, obj_types_ns + "check_bus",
531+
p->objects_filtering_params.object_types_to_check.check_bus);
532+
updateParam<bool>(
533+
parameters, obj_types_ns + "check_trailer",
534+
p->objects_filtering_params.object_types_to_check.check_trailer);
535+
updateParam<bool>(
536+
parameters, obj_types_ns + "check_unknown",
537+
p->objects_filtering_params.object_types_to_check.check_unknown);
538+
updateParam<bool>(
539+
parameters, obj_types_ns + "check_bicycle",
540+
p->objects_filtering_params.object_types_to_check.check_bicycle);
541+
updateParam<bool>(
542+
parameters, obj_types_ns + "check_motorcycle",
543+
p->objects_filtering_params.object_types_to_check.check_motorcycle);
544+
updateParam<bool>(
545+
parameters, obj_types_ns + "check_pedestrian",
546+
p->objects_filtering_params.object_types_to_check.check_pedestrian);
547+
}
548+
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
549+
550+
{
551+
updateParam<bool>(
552+
parameters, obj_lane_ns + "check_current_lane",
553+
p->objects_filtering_params.object_lane_configuration.check_current_lane);
554+
updateParam<bool>(
555+
parameters, obj_lane_ns + "check_right_side_lane",
556+
p->objects_filtering_params.object_lane_configuration.check_right_lane);
557+
updateParam<bool>(
558+
parameters, obj_lane_ns + "check_left_side_lane",
559+
p->objects_filtering_params.object_lane_configuration.check_left_lane);
560+
updateParam<bool>(
561+
parameters, obj_lane_ns + "check_shoulder_lane",
562+
p->objects_filtering_params.object_lane_configuration.check_shoulder_lane);
563+
updateParam<bool>(
564+
parameters, obj_lane_ns + "check_other_lane",
565+
p->objects_filtering_params.object_lane_configuration.check_other_lane);
566+
}
567+
const std::string safety_check_ns = base_ns + "safety_check_params.";
568+
{
569+
updateParam<bool>(
570+
parameters, safety_check_ns + "enable_safety_check",
571+
p->safety_check_params.enable_safety_check);
572+
updateParam<double>(
573+
parameters, safety_check_ns + "hysteresis_factor_expand_rate",
574+
p->safety_check_params.hysteresis_factor_expand_rate);
575+
updateParam<double>(
576+
parameters, safety_check_ns + "backward_path_length",
577+
p->safety_check_params.backward_path_length);
578+
updateParam<double>(
579+
parameters, safety_check_ns + "forward_path_length",
580+
p->safety_check_params.forward_path_length);
581+
updateParam<bool>(
582+
parameters, safety_check_ns + "publish_debug_marker",
583+
p->safety_check_params.publish_debug_marker);
584+
}
585+
const std::string rss_ns = safety_check_ns + "rss_params.";
586+
{
587+
updateParam<double>(
588+
parameters, rss_ns + "rear_vehicle_reaction_time",
589+
p->safety_check_params.rss_params.rear_vehicle_reaction_time);
590+
updateParam<double>(
591+
parameters, rss_ns + "rear_vehicle_safety_time_margin",
592+
p->safety_check_params.rss_params.rear_vehicle_safety_time_margin);
593+
updateParam<double>(
594+
parameters, rss_ns + "lateral_distance_max_threshold",
595+
p->safety_check_params.rss_params.lateral_distance_max_threshold);
596+
updateParam<double>(
597+
parameters, rss_ns + "longitudinal_distance_min_threshold",
598+
p->safety_check_params.rss_params.longitudinal_distance_min_threshold);
599+
updateParam<double>(
600+
parameters, rss_ns + "longitudinal_velocity_delta_time",
601+
p->safety_check_params.rss_params.longitudinal_velocity_delta_time);
602+
}
603+
std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
604+
{
605+
updateParam<double>(
606+
parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius);
607+
updateParam<double>(
608+
parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity",
609+
p->th_moving_obstacle_velocity);
610+
}
611+
612+
std::string debug_ns = ns + "debug.";
613+
{
614+
updateParam<bool>(parameters, debug_ns + "print_debug_info", p->print_debug_info);
615+
}
616+
617+
std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {
618+
if (!observer.expired()) observer.lock()->updateModuleParams(p);
346619
});
347620
}
348621

0 commit comments

Comments
 (0)