diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index 194d75e14228e..facb39c821764 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -31,21 +31,78 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) StartPlannerParameters p; - const std::string ns = "start_planner."; - - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); - p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); - p.th_distance_to_middle_of_the_road = - node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); - p.extra_width_margin_for_rear_obstacle = - node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); - p.collision_check_margins = - node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_margin_from_front_object = - node->declare_parameter(ns + "collision_check_margin_from_front_object"); - p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); + p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); + p.extra_width_margin_for_rear_obstacle = + node->declare_parameter(ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + node->declare_parameter>(ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + node->declare_parameter(ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + node->declare_parameter(ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + node->declare_parameter(ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); + p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); + p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + node->declare_parameter(ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + node->declare_parameter(ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + node->declare_parameter(ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + node->declare_parameter(ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + node->declare_parameter(ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + node->declare_parameter(ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + // search start pose backward + p.search_priority = node->declare_parameter( + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = node->declare_parameter(ns + "enable_back"); + p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); + p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); + p.backward_search_resolution = + node->declare_parameter(ns + "backward_search_resolution"); + p.backward_path_update_duration = + node->declare_parameter(ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + node->declare_parameter(ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } { const std::string ns = "start_planner.object_types_to_check_for_path_generation."; p.object_types_to_check_for_path_generation.check_car = @@ -65,55 +122,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.object_types_to_check_for_path_generation.check_pedestrian = node->declare_parameter(ns + "check_pedestrian"); } - p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); - // shift pull out - p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); - p.check_shift_path_lane_departure = - node->declare_parameter(ns + "check_shift_path_lane_departure"); - p.allow_check_shift_path_lane_departure_override = - node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); - p.shift_collision_check_distance_from_end = - node->declare_parameter(ns + "shift_collision_check_distance_from_end"); - p.minimum_shift_pull_out_distance = - node->declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.lateral_acceleration_sampling_num = - node->declare_parameter(ns + "lateral_acceleration_sampling_num"); - p.lateral_jerk = node->declare_parameter(ns + "lateral_jerk"); - p.maximum_lateral_acc = node->declare_parameter(ns + "maximum_lateral_acc"); - p.minimum_lateral_acc = node->declare_parameter(ns + "minimum_lateral_acc"); - p.maximum_curvature = node->declare_parameter(ns + "maximum_curvature"); - p.end_pose_curvature_threshold = - node->declare_parameter(ns + "end_pose_curvature_threshold"); - p.maximum_longitudinal_deviation = - node->declare_parameter(ns + "maximum_longitudinal_deviation"); - // geometric pull out - p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); - p.geometric_collision_check_distance_from_end = - node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); - p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); - p.parallel_parking_parameters.pull_out_velocity = - node->declare_parameter(ns + "geometric_pull_out_velocity"); - p.parallel_parking_parameters.pull_out_arc_path_interval = - node->declare_parameter(ns + "arc_path_interval"); - p.parallel_parking_parameters.pull_out_lane_departure_margin = - node->declare_parameter(ns + "lane_departure_margin"); - p.lane_departure_check_expansion_margin = - node->declare_parameter(ns + "lane_departure_check_expansion_margin"); - p.parallel_parking_parameters.pull_out_max_steer_angle = - node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg - p.parallel_parking_parameters.center_line_path_interval = - p.center_line_path_interval; // for geometric parallel parking - // search start pose backward - p.search_priority = node->declare_parameter( - ns + "search_priority"); // "efficient_path" or "short_back_distance" - p.enable_back = node->declare_parameter(ns + "enable_back"); - p.backward_velocity = node->declare_parameter(ns + "backward_velocity"); - p.max_back_distance = node->declare_parameter(ns + "max_back_distance"); - p.backward_search_resolution = node->declare_parameter(ns + "backward_search_resolution"); - p.backward_path_update_duration = - node->declare_parameter(ns + "backward_path_update_duration"); - p.ignore_distance_from_lane_end = - node->declare_parameter(ns + "ignore_distance_from_lane_end"); // freespace planner general params { const std::string ns = "start_planner.freespace_planner."; @@ -184,19 +192,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } - // stop condition - { - p.maximum_deceleration_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); - p.maximum_jerk_for_stop = - node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); - } - const std::string base_ns = "start_planner.path_safety_check."; - // EgoPredictedPath - const std::string ego_path_ns = base_ns + "ego_predicted_path."; { + const std::string ego_path_ns = base_ns + "ego_predicted_path."; p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = @@ -210,7 +209,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.ego_predicted_path_params.delay_until_departure = node->declare_parameter(ego_path_ns + "delay_until_departure"); } - // ObjectFilteringParams const std::string obj_filter_ns = base_ns + "target_filtering."; { @@ -235,10 +233,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.use_predicted_path_outside_lanelet = node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); } - // ObjectTypesToCheck - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; p.objects_filtering_params.object_types_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); p.objects_filtering_params.object_types_to_check.check_truck = @@ -256,10 +253,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.object_types_to_check.check_pedestrian = node->declare_parameter(obj_types_ns + "check_pedestrian"); } - // ObjectLaneConfiguration - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; p.objects_filtering_params.object_lane_configuration.check_current_lane = node->declare_parameter(obj_lane_ns + "check_current_lane"); p.objects_filtering_params.object_lane_configuration.check_right_lane = @@ -271,7 +267,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.objects_filtering_params.object_lane_configuration.check_other_lane = node->declare_parameter(obj_lane_ns + "check_other_lane"); } - // SafetyCheckParams const std::string safety_check_ns = base_ns + "safety_check_params."; { @@ -288,10 +283,9 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.collision_check_yaw_diff_threshold = node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); } - // RSSparams - const std::string rss_ns = safety_check_ns + "rss_params."; { + const std::string rss_ns = safety_check_ns + "rss_params."; p.safety_check_params.rss_params.rear_vehicle_reaction_time = node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = @@ -305,17 +299,17 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.safety_check_params.rss_params.extended_polygon_policy = node->declare_parameter(rss_ns + "extended_polygon_policy"); } - // surround moving obstacle check - std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; p.search_radius = node->declare_parameter(surround_moving_obstacle_check_ns + "search_radius"); p.th_moving_obstacle_velocity = node->declare_parameter( surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); // ObjectTypesToCheck - std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; p.surround_moving_obstacles_type_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); p.surround_moving_obstacles_type_to_check.check_truck = @@ -336,8 +330,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) } // debug - std::string debug_ns = ns + "debug."; { + const std::string debug_ns = "start_planner.debug."; p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); } @@ -361,9 +355,8 @@ void StartPlannerModuleManager::updateModuleParams( auto & p = parameters_; - const std::string ns = "start_planner."; - { + const std::string ns = "start_planner."; updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); @@ -458,6 +451,11 @@ void StartPlannerModuleManager::updateModuleParams( parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); updateParam( parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); } { const std::string ns = "start_planner.freespace_planner."; @@ -525,6 +523,7 @@ void StartPlannerModuleManager::updateModuleParams( updateParam( parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); } + { const std::string ns = "start_planner.freespace_planner.rrtstar."; @@ -537,14 +536,6 @@ void StartPlannerModuleManager::updateModuleParams( updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); } - { - updateParam( - parameters, ns + "stop_condition.maximum_deceleration_for_stop", - p->maximum_deceleration_for_stop); - updateParam( - parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); - } - const std::string base_ns = "start_planner.path_safety_check."; const std::string ego_path_ns = base_ns + "ego_predicted_path."; @@ -567,7 +558,6 @@ void StartPlannerModuleManager::updateModuleParams( } const std::string obj_filter_ns = base_ns + "target_filtering."; - { updateParam( parameters, obj_filter_ns + "safety_check_time_horizon", @@ -601,9 +591,8 @@ void StartPlannerModuleManager::updateModuleParams( p->objects_filtering_params.use_predicted_path_outside_lanelet); } - const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; updateParam( parameters, obj_types_ns + "check_car", p->objects_filtering_params.object_types_to_check.check_car); @@ -629,8 +618,8 @@ void StartPlannerModuleManager::updateModuleParams( parameters, obj_types_ns + "check_pedestrian", p->objects_filtering_params.object_types_to_check.check_pedestrian); } - const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { updateParam( parameters, obj_lane_ns + "check_current_lane", @@ -648,6 +637,7 @@ void StartPlannerModuleManager::updateModuleParams( parameters, obj_lane_ns + "check_other_lane", p->objects_filtering_params.object_lane_configuration.check_other_lane); } + const std::string safety_check_ns = base_ns + "safety_check_params."; { updateParam( @@ -669,8 +659,9 @@ void StartPlannerModuleManager::updateModuleParams( parameters, safety_check_ns + "collision_check_yaw_diff_threshold", p->safety_check_params.collision_check_yaw_diff_threshold); } - const std::string rss_ns = safety_check_ns + "rss_params."; + { + const std::string rss_ns = safety_check_ns + "rss_params."; updateParam( parameters, rss_ns + "rear_vehicle_reaction_time", p->safety_check_params.rss_params.rear_vehicle_reaction_time); @@ -690,8 +681,9 @@ void StartPlannerModuleManager::updateModuleParams( parameters, rss_ns + "extended_polygon_policy", p->safety_check_params.rss_params.extended_polygon_policy); } - std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; updateParam( parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); updateParam( @@ -699,8 +691,8 @@ void StartPlannerModuleManager::updateModuleParams( p->th_moving_obstacle_velocity); // ObjectTypesToCheck - std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; { + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; updateParam( parameters, obj_types_ns + "check_car", p->surround_moving_obstacles_type_to_check.check_car); @@ -728,8 +720,8 @@ void StartPlannerModuleManager::updateModuleParams( } } - std::string debug_ns = ns + "debug."; { + const std::string debug_ns = "start_planner.debug."; updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 909a9574faa43..c621b16824110 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -652,7 +652,6 @@ BehaviorModuleOutput StartPlannerModule::plan() resetPathReference(); } - BehaviorModuleOutput output; if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); @@ -702,6 +701,7 @@ BehaviorModuleOutput StartPlannerModule::plan() return getCurrentPath(); }); + BehaviorModuleOutput output; output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo(); @@ -777,7 +777,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); - BehaviorModuleOutput output; if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); @@ -806,6 +805,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() p.point.longitudinal_velocity_mps = 0.0; } + BehaviorModuleOutput output; output.path = stop_path; output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = calcTurnSignalInfo();