From d7a2838b8ce657cd0a1f95494b98f1cb552906ed Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Tue, 24 Dec 2024 15:48:48 +0900 Subject: [PATCH] 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 --- .../CMakeLists.txt | 1 + .../data_structs.hpp | 1 + .../src/data_structs.cpp | 340 ++++++++++++++++++ .../src/manager.cpp | 308 +--------------- 4 files changed, 346 insertions(+), 304 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index 3fa8ad7218fa2..e28339fb51a6c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/freespace_pull_out.cpp src/geometric_pull_out.cpp src/shift_pull_out.cpp + src/data_structs.cpp src/util.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 9c439e9b3b921..5d8331318484d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -97,6 +97,7 @@ struct StartPlannerDebugData struct StartPlannerParameters { + static StartPlannerParameters init(rclcpp::Node & node); double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp new file mode 100644 index 0000000000000..f613b9345ce42 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -0,0 +1,340 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_path_start_planner_module/data_structs.hpp" + +#include "autoware/behavior_path_start_planner_module/manager.hpp" + +#include +#include + +#include + +namespace autoware::behavior_path_planner +{ + +StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) +{ + using autoware::universe_utils::getOrDeclareParameter; + StartPlannerParameters p; + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = getOrDeclareParameter(node, ns + "th_arrived_distance"); + p.th_stopped_velocity = getOrDeclareParameter(node, ns + "th_stopped_velocity"); + p.th_stopped_time = getOrDeclareParameter(node, ns + "th_stopped_time"); + p.prepare_time_before_start = + getOrDeclareParameter(node, ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + getOrDeclareParameter(node, ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = getOrDeclareParameter(node, ns + "skip_rear_vehicle_check"); + p.extra_width_margin_for_rear_obstacle = + getOrDeclareParameter(node, ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + getOrDeclareParameter>(node, ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + getOrDeclareParameter(node, ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = + getOrDeclareParameter(node, ns + "th_moving_object_velocity"); + p.center_line_path_interval = + getOrDeclareParameter(node, ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = getOrDeclareParameter(node, ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + getOrDeclareParameter(node, ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + getOrDeclareParameter(node, ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + getOrDeclareParameter(node, ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + getOrDeclareParameter(node, ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = getOrDeclareParameter(node, ns + "lateral_jerk"); + p.maximum_lateral_acc = getOrDeclareParameter(node, ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = getOrDeclareParameter(node, ns + "minimum_lateral_acc"); + p.maximum_curvature = getOrDeclareParameter(node, ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + getOrDeclareParameter(node, ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + getOrDeclareParameter(node, ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = + getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + getOrDeclareParameter(node, ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + getOrDeclareParameter(node, ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + getOrDeclareParameter(node, ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + getOrDeclareParameter(node, ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + getOrDeclareParameter(node, 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 = getOrDeclareParameter( + node, + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = getOrDeclareParameter(node, ns + "enable_back"); + p.backward_velocity = getOrDeclareParameter(node, ns + "backward_velocity"); + p.max_back_distance = getOrDeclareParameter(node, ns + "max_back_distance"); + p.backward_search_resolution = + getOrDeclareParameter(node, ns + "backward_search_resolution"); + p.backward_path_update_duration = + getOrDeclareParameter(node, ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + getOrDeclareParameter(node, ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + getOrDeclareParameter(node, ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + getOrDeclareParameter(node, ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + getOrDeclareParameter(node, ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + getOrDeclareParameter(node, ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + getOrDeclareParameter(node, ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + getOrDeclareParameter(node, ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + getOrDeclareParameter(node, ns + "check_pedestrian"); + } + // freespace planner general params + { + const std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = getOrDeclareParameter(node, ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + getOrDeclareParameter(node, ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + getOrDeclareParameter(node, ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + getOrDeclareParameter(node, ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = + getOrDeclareParameter(node, ns + "end_pose_search_interval"); + p.freespace_planner_velocity = getOrDeclareParameter(node, ns + "velocity"); + p.vehicle_shape_margin = getOrDeclareParameter(node, ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + getOrDeclareParameter(node, ns + "time_limit"); + p.freespace_planner_common_parameters.max_turning_ratio = + getOrDeclareParameter(node, ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + getOrDeclareParameter(node, ns + "turning_steps"); + } + // freespace planner search config + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + getOrDeclareParameter(node, ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + getOrDeclareParameter(node, ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + getOrDeclareParameter(node, ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + getOrDeclareParameter(node, ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + getOrDeclareParameter(node, ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + getOrDeclareParameter(node, ns + "longitudinal_goal_range"); + } + // freespace planner costmap configs + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + p.freespace_planner_common_parameters.obstacle_threshold = + getOrDeclareParameter(node, ns + "obstacle_threshold"); + } + // freespace planner astar + { + const std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.search_method = + getOrDeclareParameter(node, ns + "search_method"); + p.astar_parameters.only_behind_solutions = + getOrDeclareParameter(node, ns + "only_behind_solutions"); + p.astar_parameters.use_back = getOrDeclareParameter(node, ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + getOrDeclareParameter(node, ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = getOrDeclareParameter(node, ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + getOrDeclareParameter(node, ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + getOrDeclareParameter(node, ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = + getOrDeclareParameter(node, ns + "neighbor_radius"); + p.rrt_star_parameters.margin = getOrDeclareParameter(node, ns + "margin"); + } + + const std::string base_ns = "start_planner.path_safety_check."; + // EgoPredictedPath + { + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(node, ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(node, ego_path_ns + "min_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(node, ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(node, ego_path_ns + "delay_until_departure"); + } + // ObjectFilteringParams + const std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + getOrDeclareParameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + getOrDeclareParameter(node, obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + // ObjectTypesToCheck + { + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + p.objects_filtering_params.object_types_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + // ObjectLaneConfiguration + { + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + p.objects_filtering_params.object_lane_configuration.check_current_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_other_lane"); + } + // SafetyCheckParams + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + getOrDeclareParameter(node, safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + getOrDeclareParameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + getOrDeclareParameter(node, safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + getOrDeclareParameter(node, safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + getOrDeclareParameter(node, safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + getOrDeclareParameter(node, safety_check_ns + "collision_check_yaw_diff_threshold"); + } + // RSSparams + { + const std::string rss_ns = safety_check_ns + "rss_params."; + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(node, rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(node, rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(node, rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + getOrDeclareParameter(node, rss_ns + "extended_polygon_policy"); + } + // surround moving obstacle check + { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; + p.search_radius = + getOrDeclareParameter(node, surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = getOrDeclareParameter( + node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + p.surround_moving_obstacles_type_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + } + + // debug + { + const std::string debug_ns = "start_planner.debug."; + p.print_debug_info = getOrDeclareParameter(node, debug_ns + "print_debug_info"); + } + + return p; +} +} // namespace autoware::behavior_path_planner 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 f5739ce322248..9ea51d56ee1cc 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 @@ -29,318 +29,18 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) // init manager interface initInterface(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.skip_rear_vehicle_check = node->declare_parameter(ns + "skip_rear_vehicle_check"); - 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 = - node->declare_parameter(ns + "check_car"); - p.object_types_to_check_for_path_generation.check_truck = - node->declare_parameter(ns + "check_truck"); - p.object_types_to_check_for_path_generation.check_bus = - node->declare_parameter(ns + "check_bus"); - p.object_types_to_check_for_path_generation.check_trailer = - node->declare_parameter(ns + "check_trailer"); - p.object_types_to_check_for_path_generation.check_unknown = - node->declare_parameter(ns + "check_unknown"); - p.object_types_to_check_for_path_generation.check_bicycle = - node->declare_parameter(ns + "check_bicycle"); - p.object_types_to_check_for_path_generation.check_motorcycle = - node->declare_parameter(ns + "check_motorcycle"); - p.object_types_to_check_for_path_generation.check_pedestrian = - node->declare_parameter(ns + "check_pedestrian"); - } - // freespace planner general params - { - const std::string ns = "start_planner.freespace_planner."; - p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); - p.freespace_planner_algorithm = - node->declare_parameter(ns + "freespace_planner_algorithm"); - p.end_pose_search_start_distance = - node->declare_parameter(ns + "end_pose_search_start_distance"); - p.end_pose_search_end_distance = - node->declare_parameter(ns + "end_pose_search_end_distance"); - p.end_pose_search_interval = node->declare_parameter(ns + "end_pose_search_interval"); - p.freespace_planner_velocity = node->declare_parameter(ns + "velocity"); - p.vehicle_shape_margin = node->declare_parameter(ns + "vehicle_shape_margin"); - p.freespace_planner_common_parameters.time_limit = - node->declare_parameter(ns + "time_limit"); - p.freespace_planner_common_parameters.max_turning_ratio = - node->declare_parameter(ns + "max_turning_ratio"); - p.freespace_planner_common_parameters.turning_steps = - node->declare_parameter(ns + "turning_steps"); - } - // freespace planner search config - { - const std::string ns = "start_planner.freespace_planner.search_configs."; - p.freespace_planner_common_parameters.theta_size = - node->declare_parameter(ns + "theta_size"); - p.freespace_planner_common_parameters.angle_goal_range = - node->declare_parameter(ns + "angle_goal_range"); - p.freespace_planner_common_parameters.curve_weight = - node->declare_parameter(ns + "curve_weight"); - p.freespace_planner_common_parameters.reverse_weight = - node->declare_parameter(ns + "reverse_weight"); - p.freespace_planner_common_parameters.lateral_goal_range = - node->declare_parameter(ns + "lateral_goal_range"); - p.freespace_planner_common_parameters.longitudinal_goal_range = - node->declare_parameter(ns + "longitudinal_goal_range"); - } - // freespace planner costmap configs - { - const std::string ns = "start_planner.freespace_planner.costmap_configs."; - p.freespace_planner_common_parameters.obstacle_threshold = - node->declare_parameter(ns + "obstacle_threshold"); - } - // freespace planner astar - { - const std::string ns = "start_planner.freespace_planner.astar."; - p.astar_parameters.search_method = node->declare_parameter(ns + "search_method"); - p.astar_parameters.only_behind_solutions = - node->declare_parameter(ns + "only_behind_solutions"); - p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); - p.astar_parameters.distance_heuristic_weight = - node->declare_parameter(ns + "distance_heuristic_weight"); - } - // freespace planner rrtstar - { - const std::string ns = "start_planner.freespace_planner.rrtstar."; - p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); - p.rrt_star_parameters.use_informed_sampling = - node->declare_parameter(ns + "use_informed_sampling"); - p.rrt_star_parameters.max_planning_time = - node->declare_parameter(ns + "max_planning_time"); - p.rrt_star_parameters.neighbor_radius = node->declare_parameter(ns + "neighbor_radius"); - p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); - } - - const std::string base_ns = "start_planner.path_safety_check."; - // EgoPredictedPath - { - 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 = - node->declare_parameter(ego_path_ns + "min_acceleration"); - p.ego_predicted_path_params.time_horizon_for_front_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); - p.ego_predicted_path_params.time_horizon_for_rear_object = - node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); - p.ego_predicted_path_params.time_resolution = - node->declare_parameter(ego_path_ns + "time_resolution"); - 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."; - { - p.objects_filtering_params.safety_check_time_horizon = - node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); - p.objects_filtering_params.safety_check_time_resolution = - node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); - p.objects_filtering_params.object_check_forward_distance = - node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); - p.objects_filtering_params.object_check_backward_distance = - node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); - p.objects_filtering_params.ignore_object_velocity_threshold = - node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); - p.objects_filtering_params.include_opposite_lane = - node->declare_parameter(obj_filter_ns + "include_opposite_lane"); - p.objects_filtering_params.invert_opposite_lane = - node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); - p.objects_filtering_params.check_all_predicted_path = - node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); - p.objects_filtering_params.use_all_predicted_path = - node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); - 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."; - 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 = - node->declare_parameter(obj_types_ns + "check_truck"); - p.objects_filtering_params.object_types_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.objects_filtering_params.object_types_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.objects_filtering_params.object_types_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.objects_filtering_params.object_types_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.objects_filtering_params.object_types_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - 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."; - 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 = - node->declare_parameter(obj_lane_ns + "check_right_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_left_lane = - node->declare_parameter(obj_lane_ns + "check_left_side_lane"); - p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = - node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); - 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."; - { - p.safety_check_params.enable_safety_check = - node->declare_parameter(safety_check_ns + "enable_safety_check"); - p.safety_check_params.hysteresis_factor_expand_rate = - node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); - p.safety_check_params.backward_path_length = - node->declare_parameter(safety_check_ns + "backward_path_length"); - p.safety_check_params.forward_path_length = - node->declare_parameter(safety_check_ns + "forward_path_length"); - p.safety_check_params.publish_debug_marker = - node->declare_parameter(safety_check_ns + "publish_debug_marker"); - 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."; - 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 = - node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); - p.safety_check_params.rss_params.lateral_distance_max_threshold = - node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); - p.safety_check_params.rss_params.longitudinal_distance_min_threshold = - node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); - p.safety_check_params.rss_params.longitudinal_velocity_delta_time = - node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); - p.safety_check_params.rss_params.extended_polygon_policy = - node->declare_parameter(rss_ns + "extended_polygon_policy"); - } - // 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 - { - 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 = - node->declare_parameter(obj_types_ns + "check_truck"); - p.surround_moving_obstacles_type_to_check.check_bus = - node->declare_parameter(obj_types_ns + "check_bus"); - p.surround_moving_obstacles_type_to_check.check_trailer = - node->declare_parameter(obj_types_ns + "check_trailer"); - p.surround_moving_obstacles_type_to_check.check_unknown = - node->declare_parameter(obj_types_ns + "check_unknown"); - p.surround_moving_obstacles_type_to_check.check_bicycle = - node->declare_parameter(obj_types_ns + "check_bicycle"); - p.surround_moving_obstacles_type_to_check.check_motorcycle = - node->declare_parameter(obj_types_ns + "check_motorcycle"); - p.surround_moving_obstacles_type_to_check.check_pedestrian = - node->declare_parameter(obj_types_ns + "check_pedestrian"); - } - } - - // debug - { - const std::string debug_ns = "start_planner.debug."; - p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); - } - + StartPlannerParameters parameters = StartPlannerParameters::init(*node); // validation of parameters - if (p.lateral_acceleration_sampling_num < 1) { + if (parameters.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( node->get_logger().get_child(name()), "lateral_acceleration_sampling_num must be positive integer. Given parameter: " - << p.lateral_acceleration_sampling_num << std::endl + << parameters.lateral_acceleration_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } - parameters_ = std::make_shared(p); + parameters_ = std::make_shared(parameters); } void StartPlannerModuleManager::updateModuleParams(