Skip to content

Commit 87198ed

Browse files
danielsanchezaranHansRobo
danielsanchezaran
authored and
HansRobo
committed
feat(behavior_path_start_planner_module): add support for rqt reconfig (#6492)
* add support for rqt reconfig Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add missing params to update Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
1 parent deca110 commit 87198ed

File tree

1 file changed

+334
-8
lines changed
  • planning/behavior_path_start_planner_module/src

1 file changed

+334
-8
lines changed

‎planning/behavior_path_start_planner_module/src/manager.cpp

+334-8
Original file line numberDiff line numberDiff line change
@@ -334,15 +334,341 @@ 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-
}
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+
updateParam<std::vector<double>>(
354+
parameters, ns + "collision_check_margins", p->collision_check_margins);
355+
updateParam<double>(
356+
parameters, ns + "collision_check_margin_from_front_object",
357+
p->collision_check_margin_from_front_object);
358+
updateParam<double>(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity);
359+
updateParam<double>(parameters, ns + "center_line_path_interval", p->center_line_path_interval);
360+
updateParam<bool>(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out);
361+
updateParam<double>(
362+
parameters, ns + "shift_collision_check_distance_from_end",
363+
p->shift_collision_check_distance_from_end);
364+
updateParam<double>(
365+
parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance);
366+
updateParam<int>(
367+
parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num);
368+
updateParam<double>(parameters, ns + "lateral_jerk", p->lateral_jerk);
369+
updateParam<double>(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc);
370+
updateParam<double>(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc);
371+
updateParam<double>(parameters, ns + "maximum_curvature", p->maximum_curvature);
372+
updateParam<double>(parameters, ns + "deceleration_interval", p->deceleration_interval);
373+
updateParam<bool>(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out);
374+
updateParam<bool>(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path);
375+
updateParam<double>(
376+
parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval);
377+
updateParam<double>(
378+
parameters, ns + "lane_departure_margin",
379+
p->parallel_parking_parameters.pull_out_lane_departure_margin);
380+
updateParam<double>(
381+
parameters, ns + "pull_out_max_steer_angle",
382+
p->parallel_parking_parameters.pull_out_max_steer_angle);
383+
updateParam<bool>(parameters, ns + "enable_back", p->enable_back);
384+
updateParam<double>(parameters, ns + "backward_velocity", p->backward_velocity);
385+
updateParam<double>(
386+
parameters, ns + "geometric_pull_out_velocity",
387+
p->parallel_parking_parameters.pull_out_velocity);
388+
updateParam<double>(
389+
parameters, ns + "geometric_collision_check_distance_from_end",
390+
p->geometric_collision_check_distance_from_end);
391+
updateParam<bool>(
392+
parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure);
393+
updateParam<std::string>(parameters, ns + "search_priority", p->search_priority);
394+
updateParam<double>(parameters, ns + "max_back_distance", p->max_back_distance);
395+
updateParam<double>(
396+
parameters, ns + "backward_search_resolution", p->backward_search_resolution);
397+
updateParam<double>(
398+
parameters, ns + "backward_path_update_duration", p->backward_path_update_duration);
399+
updateParam<double>(
400+
parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end);
401+
}
402+
{
403+
const std::string ns = "start_planner.freespace_planner.";
404+
405+
updateParam<bool>(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner);
406+
updateParam<std::string>(
407+
parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm);
408+
updateParam<double>(
409+
parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance);
410+
updateParam<double>(
411+
parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance);
412+
updateParam<double>(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval);
413+
updateParam<double>(parameters, ns + "velocity", p->freespace_planner_velocity);
414+
updateParam<double>(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin);
415+
updateParam<double>(
416+
parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit);
417+
updateParam<double>(
418+
parameters, ns + "minimum_turning_radius",
419+
p->freespace_planner_common_parameters.minimum_turning_radius);
420+
updateParam<double>(
421+
parameters, ns + "maximum_turning_radius",
422+
p->freespace_planner_common_parameters.maximum_turning_radius);
423+
updateParam<int>(
424+
parameters, ns + "turning_radius_size",
425+
p->freespace_planner_common_parameters.turning_radius_size);
426+
p->freespace_planner_common_parameters.maximum_turning_radius = std::max(
427+
p->freespace_planner_common_parameters.maximum_turning_radius,
428+
p->freespace_planner_common_parameters.minimum_turning_radius);
429+
p->freespace_planner_common_parameters.turning_radius_size =
430+
std::max(p->freespace_planner_common_parameters.turning_radius_size, 1);
431+
}
432+
{
433+
const std::string ns = "start_planner.freespace_planner.search_configs.";
434+
435+
updateParam<int>(
436+
parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size);
437+
updateParam<double>(
438+
parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range);
439+
updateParam<double>(
440+
parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight);
441+
updateParam<double>(
442+
parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight);
443+
updateParam<double>(
444+
parameters, ns + "lateral_goal_range",
445+
p->freespace_planner_common_parameters.lateral_goal_range);
446+
updateParam<double>(
447+
parameters, ns + "longitudinal_goal_range",
448+
p->freespace_planner_common_parameters.longitudinal_goal_range);
449+
}
450+
451+
{
452+
const std::string ns = "start_planner.freespace_planner.costmap_configs.";
453+
454+
updateParam<int>(
455+
parameters, ns + "obstacle_threshold",
456+
p->freespace_planner_common_parameters.obstacle_threshold);
457+
}
458+
459+
{
460+
const std::string ns = "start_planner.freespace_planner.astar.";
461+
462+
updateParam<bool>(parameters, ns + "use_back", p->astar_parameters.use_back);
463+
updateParam<bool>(
464+
parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions);
465+
updateParam<double>(
466+
parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight);
467+
}
468+
{
469+
const std::string ns = "start_planner.freespace_planner.rrtstar.";
470+
471+
updateParam<bool>(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update);
472+
updateParam<bool>(
473+
parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling);
474+
updateParam<double>(
475+
parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time);
476+
updateParam<double>(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius);
477+
updateParam<double>(parameters, ns + "margin", p->rrt_star_parameters.margin);
478+
}
479+
480+
{
481+
updateParam<double>(
482+
parameters, ns + "stop_condition.maximum_deceleration_for_stop",
483+
p->maximum_deceleration_for_stop);
484+
updateParam<double>(
485+
parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop);
486+
}
487+
488+
const std::string base_ns = "start_planner.path_safety_check.";
489+
const std::string ego_path_ns = base_ns + "ego_predicted_path.";
490+
491+
{
492+
updateParam<double>(
493+
parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity);
494+
updateParam<double>(
495+
parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration);
496+
updateParam<double>(
497+
parameters, ego_path_ns + "time_horizon_for_front_object",
498+
p->ego_predicted_path_params.time_horizon_for_front_object);
499+
updateParam<double>(
500+
parameters, ego_path_ns + "time_horizon_for_rear_object",
501+
p->ego_predicted_path_params.time_horizon_for_rear_object);
502+
updateParam<double>(
503+
parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution);
504+
updateParam<double>(
505+
parameters, ego_path_ns + "delay_until_departure",
506+
p->ego_predicted_path_params.delay_until_departure);
507+
}
508+
509+
const std::string obj_filter_ns = base_ns + "target_filtering.";
510+
511+
{
512+
updateParam<double>(
513+
parameters, obj_filter_ns + "safety_check_time_horizon",
514+
p->objects_filtering_params.safety_check_time_horizon);
515+
updateParam<double>(
516+
parameters, obj_filter_ns + "safety_check_time_resolution",
517+
p->objects_filtering_params.safety_check_time_resolution);
518+
updateParam<double>(
519+
parameters, obj_filter_ns + "object_check_forward_distance",
520+
p->objects_filtering_params.object_check_forward_distance);
521+
updateParam<double>(
522+
parameters, obj_filter_ns + "object_check_backward_distance",
523+
p->objects_filtering_params.object_check_backward_distance);
524+
updateParam<double>(
525+
parameters, obj_filter_ns + "ignore_object_velocity_threshold",
526+
p->objects_filtering_params.ignore_object_velocity_threshold);
527+
updateParam<bool>(
528+
parameters, obj_filter_ns + "include_opposite_lane",
529+
p->objects_filtering_params.include_opposite_lane);
530+
updateParam<bool>(
531+
parameters, obj_filter_ns + "invert_opposite_lane",
532+
p->objects_filtering_params.invert_opposite_lane);
533+
updateParam<bool>(
534+
parameters, obj_filter_ns + "check_all_predicted_path",
535+
p->objects_filtering_params.check_all_predicted_path);
536+
updateParam<bool>(
537+
parameters, obj_filter_ns + "use_all_predicted_path",
538+
p->objects_filtering_params.use_all_predicted_path);
539+
updateParam<bool>(
540+
parameters, obj_filter_ns + "use_predicted_path_outside_lanelet",
541+
p->objects_filtering_params.use_predicted_path_outside_lanelet);
542+
}
543+
544+
const std::string obj_types_ns = obj_filter_ns + "object_types_to_check.";
545+
546+
{
547+
updateParam<bool>(
548+
parameters, obj_types_ns + "check_car",
549+
p->objects_filtering_params.object_types_to_check.check_car);
550+
updateParam<bool>(
551+
parameters, obj_types_ns + "check_truck",
552+
p->objects_filtering_params.object_types_to_check.check_truck);
553+
updateParam<bool>(
554+
parameters, obj_types_ns + "check_bus",
555+
p->objects_filtering_params.object_types_to_check.check_bus);
556+
updateParam<bool>(
557+
parameters, obj_types_ns + "check_trailer",
558+
p->objects_filtering_params.object_types_to_check.check_trailer);
559+
updateParam<bool>(
560+
parameters, obj_types_ns + "check_unknown",
561+
p->objects_filtering_params.object_types_to_check.check_unknown);
562+
updateParam<bool>(
563+
parameters, obj_types_ns + "check_bicycle",
564+
p->objects_filtering_params.object_types_to_check.check_bicycle);
565+
updateParam<bool>(
566+
parameters, obj_types_ns + "check_motorcycle",
567+
p->objects_filtering_params.object_types_to_check.check_motorcycle);
568+
updateParam<bool>(
569+
parameters, obj_types_ns + "check_pedestrian",
570+
p->objects_filtering_params.object_types_to_check.check_pedestrian);
571+
}
572+
const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration.";
573+
574+
{
575+
updateParam<bool>(
576+
parameters, obj_lane_ns + "check_current_lane",
577+
p->objects_filtering_params.object_lane_configuration.check_current_lane);
578+
updateParam<bool>(
579+
parameters, obj_lane_ns + "check_right_side_lane",
580+
p->objects_filtering_params.object_lane_configuration.check_right_lane);
581+
updateParam<bool>(
582+
parameters, obj_lane_ns + "check_left_side_lane",
583+
p->objects_filtering_params.object_lane_configuration.check_left_lane);
584+
updateParam<bool>(
585+
parameters, obj_lane_ns + "check_shoulder_lane",
586+
p->objects_filtering_params.object_lane_configuration.check_shoulder_lane);
587+
updateParam<bool>(
588+
parameters, obj_lane_ns + "check_other_lane",
589+
p->objects_filtering_params.object_lane_configuration.check_other_lane);
590+
}
591+
const std::string safety_check_ns = base_ns + "safety_check_params.";
592+
{
593+
updateParam<bool>(
594+
parameters, safety_check_ns + "enable_safety_check",
595+
p->safety_check_params.enable_safety_check);
596+
updateParam<double>(
597+
parameters, safety_check_ns + "hysteresis_factor_expand_rate",
598+
p->safety_check_params.hysteresis_factor_expand_rate);
599+
updateParam<double>(
600+
parameters, safety_check_ns + "backward_path_length",
601+
p->safety_check_params.backward_path_length);
602+
updateParam<double>(
603+
parameters, safety_check_ns + "forward_path_length",
604+
p->safety_check_params.forward_path_length);
605+
updateParam<bool>(
606+
parameters, safety_check_ns + "publish_debug_marker",
607+
p->safety_check_params.publish_debug_marker);
608+
}
609+
const std::string rss_ns = safety_check_ns + "rss_params.";
610+
{
611+
updateParam<double>(
612+
parameters, rss_ns + "rear_vehicle_reaction_time",
613+
p->safety_check_params.rss_params.rear_vehicle_reaction_time);
614+
updateParam<double>(
615+
parameters, rss_ns + "rear_vehicle_safety_time_margin",
616+
p->safety_check_params.rss_params.rear_vehicle_safety_time_margin);
617+
updateParam<double>(
618+
parameters, rss_ns + "lateral_distance_max_threshold",
619+
p->safety_check_params.rss_params.lateral_distance_max_threshold);
620+
updateParam<double>(
621+
parameters, rss_ns + "longitudinal_distance_min_threshold",
622+
p->safety_check_params.rss_params.longitudinal_distance_min_threshold);
623+
updateParam<double>(
624+
parameters, rss_ns + "longitudinal_velocity_delta_time",
625+
p->safety_check_params.rss_params.longitudinal_velocity_delta_time);
626+
}
627+
std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
628+
{
629+
updateParam<double>(
630+
parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius);
631+
updateParam<double>(
632+
parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity",
633+
p->th_moving_obstacle_velocity);
634+
635+
// ObjectTypesToCheck
636+
std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
637+
{
638+
updateParam<bool>(
639+
parameters, obj_types_ns + "check_car",
640+
p->surround_moving_obstacles_type_to_check.check_car);
641+
updateParam<bool>(
642+
parameters, obj_types_ns + "check_truck",
643+
p->surround_moving_obstacles_type_to_check.check_truck);
644+
updateParam<bool>(
645+
parameters, obj_types_ns + "check_bus",
646+
p->surround_moving_obstacles_type_to_check.check_bus);
647+
updateParam<bool>(
648+
parameters, obj_types_ns + "check_trailer",
649+
p->surround_moving_obstacles_type_to_check.check_trailer);
650+
updateParam<bool>(
651+
parameters, obj_types_ns + "check_unknown",
652+
p->surround_moving_obstacles_type_to_check.check_unknown);
653+
updateParam<bool>(
654+
parameters, obj_types_ns + "check_bicycle",
655+
p->surround_moving_obstacles_type_to_check.check_bicycle);
656+
updateParam<bool>(
657+
parameters, obj_types_ns + "check_motorcycle",
658+
p->surround_moving_obstacles_type_to_check.check_motorcycle);
659+
updateParam<bool>(
660+
parameters, obj_types_ns + "check_pedestrian",
661+
p->surround_moving_obstacles_type_to_check.check_pedestrian);
345662
}
663+
}
664+
665+
std::string debug_ns = ns + "debug.";
666+
{
667+
updateParam<bool>(parameters, debug_ns + "print_debug_info", p->print_debug_info);
668+
}
669+
670+
std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {
671+
if (!observer.expired()) observer.lock()->updateModuleParams(p);
346672
});
347673
}
348674

0 commit comments

Comments
 (0)