diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 96e4e873ce111..30a1a81a1b50a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -15,6 +15,7 @@ #include "manager.hpp" #include +#include #include #include @@ -26,86 +27,93 @@ namespace behavior_velocity_planner { using lanelet::autoware::Crosswalk; +using tier4_autoware_utils::getOrDeclareParameter; CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".common.enable_rtc")) + getOrDeclareParameter(node, std::string(getModuleName()) + ".common.enable_rtc")) { const std::string ns(getModuleName()); // for crosswalk parameters auto & cp = crosswalk_planner_param_; - cp.show_processing_time = node.declare_parameter(ns + ".common.show_processing_time"); + cp.show_processing_time = getOrDeclareParameter(node, ns + ".common.show_processing_time"); // param for input data cp.traffic_light_state_timeout = - node.declare_parameter(ns + ".common.traffic_light_state_timeout"); + getOrDeclareParameter(node, ns + ".common.traffic_light_state_timeout"); // param for stop position cp.stop_distance_from_crosswalk = - node.declare_parameter(ns + ".stop_position.stop_distance_from_crosswalk"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); cp.stop_distance_from_object = - node.declare_parameter(ns + ".stop_position.stop_distance_from_object"); + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); cp.far_object_threshold = - node.declare_parameter(ns + ".stop_position.far_object_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = - node.declare_parameter(ns + ".stop_position.stop_position_threshold"); + getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); // param for ego velocity cp.min_slow_down_velocity = - node.declare_parameter(ns + ".slow_down.min_slow_down_velocity"); - cp.max_slow_down_jerk = node.declare_parameter(ns + ".slow_down.max_slow_down_jerk"); - cp.max_slow_down_accel = node.declare_parameter(ns + ".slow_down.max_slow_down_accel"); - cp.no_relax_velocity = node.declare_parameter(ns + ".slow_down.no_relax_velocity"); + getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); + cp.max_slow_down_jerk = getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_jerk"); + cp.max_slow_down_accel = + getOrDeclareParameter(node, ns + ".slow_down.max_slow_down_accel"); + cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle cp.stuck_vehicle_velocity = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_velocity"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = - node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); - cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); - cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); - cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = getOrDeclareParameter(node, ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic cp.ego_pass_first_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_x"); cp.ego_pass_first_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_first_margin_y"); cp.ego_pass_first_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_first_additional_margin"); cp.ego_pass_later_margin_x = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_x"); cp.ego_pass_later_margin_y = - node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + getOrDeclareParameter>(node, ns + ".pass_judge.ego_pass_later_margin_y"); cp.ego_pass_later_additional_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); cp.max_offset_to_crosswalk_for_yield = - node.declare_parameter(ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.max_offset_to_crosswalk_for_yield"); cp.stop_object_velocity = - node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); - cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); + getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); + cp.min_object_velocity = + getOrDeclareParameter(node, ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = - node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = - node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); + getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = - node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); + getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = - node.declare_parameter(ns + ".pass_judge.timeout_ego_stop_for_yield"); + getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); // param for target area & object cp.crosswalk_attention_range = - node.declare_parameter(ns + ".object_filtering.crosswalk_attention_range"); - cp.look_unknown = node.declare_parameter(ns + ".object_filtering.target_object.unknown"); - cp.look_bicycle = node.declare_parameter(ns + ".object_filtering.target_object.bicycle"); + getOrDeclareParameter(node, ns + ".object_filtering.crosswalk_attention_range"); + cp.look_unknown = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.unknown"); + cp.look_bicycle = + getOrDeclareParameter(node, ns + ".object_filtering.target_object.bicycle"); cp.look_motorcycle = - node.declare_parameter(ns + ".object_filtering.target_object.motorcycle"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = - node.declare_parameter(ns + ".object_filtering.target_object.pedestrian"); + getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)