From 61630e6721310b9ae3de08b125184eb62dc851f6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 28 Aug 2023 16:50:45 +0900 Subject: [PATCH] refactor(intersection): use getOrDeclareParameter (#4767) Signed-off-by: Takamasa Horibe --- .../src/manager.cpp | 102 +++++++++--------- 1 file changed, 54 insertions(+), 48 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6a2b7200abb2e..9d6359ebe9b9b 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include @@ -29,94 +30,99 @@ namespace behavior_velocity_planner { +using tier4_autoware_utils::getOrDeclareParameter; + IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) : SceneModuleManagerInterfaceWithRTC( node, getModuleName(), - node.declare_parameter(std::string(getModuleName()) + ".enable_rtc.intersection")), + getOrDeclareParameter(node, std::string(getModuleName()) + ".enable_rtc.intersection")), occlusion_rtc_interface_( &node, "intersection_occlusion", - node.declare_parameter( - std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) + getOrDeclareParameter( + node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) { const std::string ns(getModuleName()); auto & ip = intersection_param_; const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = - node.declare_parameter(ns + ".common.attention_area_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = - node.declare_parameter(ns + ".common.attention_area_length"); + getOrDeclareParameter(node, ns + ".common.attention_area_length"); ip.common.attention_area_angle_thr = - node.declare_parameter(ns + ".common.attention_area_angle_threshold"); - ip.common.stop_line_margin = node.declare_parameter(ns + ".common.stop_line_margin"); + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.stop_line_margin = getOrDeclareParameter(node, ns + ".common.stop_line_margin"); ip.common.intersection_velocity = - node.declare_parameter(ns + ".common.intersection_velocity"); + getOrDeclareParameter(node, ns + ".common.intersection_velocity"); ip.common.intersection_max_acc = - node.declare_parameter(ns + ".common.intersection_max_accel"); + getOrDeclareParameter(node, ns + ".common.intersection_max_accel"); ip.common.stop_overshoot_margin = - node.declare_parameter(ns + ".common.stop_overshoot_margin"); + getOrDeclareParameter(node, ns + ".common.stop_overshoot_margin"); ip.common.use_intersection_area = - node.declare_parameter(ns + ".common.use_intersection_area"); + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); ip.common.path_interpolation_ds = - node.declare_parameter(ns + ".common.path_interpolation_ds"); + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = - node.declare_parameter(ns + ".common.consider_wrong_direction_vehicle"); + getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); ip.stuck_vehicle.use_stuck_stopline = - node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_ignore_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* ip.stuck_vehicle.assumed_front_car_decel = - node.declare_parameter(ns + ".stuck_vehicle.assumed_front_car_decel"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.assumed_front_car_decel"); ip.stuck_vehicle.enable_front_car_decel_prediction = - node.declare_parameter(ns + ".stuck_vehicle.enable_front_car_decel_prediction"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_front_car_decel_prediction"); */ ip.stuck_vehicle.timeout_private_area = - node.declare_parameter(ns + ".stuck_vehicle.timeout_private_area"); + getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); ip.collision_detection.min_predicted_path_confidence = - node.declare_parameter(ns + ".collision_detection.min_predicted_path_confidence"); + getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); ip.collision_detection.minimum_ego_predicted_velocity = - node.declare_parameter(ns + ".collision_detection.minimum_ego_predicted_velocity"); + getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = - node.declare_parameter(ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = - node.declare_parameter(ns + ".collision_detection.relaxed.collision_end_margin_time"); + getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); + ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.normal.collision_start_margin_time"); + ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.normal.collision_end_margin_time"); + ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.relaxed.collision_start_margin_time"); + ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.relaxed.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = - node.declare_parameter(ns + ".collision_detection.keep_detection_vel_thr"); + getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); - ip.occlusion.enable = node.declare_parameter(ns + ".occlusion.enable"); + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = - node.declare_parameter(ns + ".occlusion.occlusion_attention_area_length"); - ip.occlusion.enable_creeping = node.declare_parameter(ns + ".occlusion.enable_creeping"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.enable_creeping = + getOrDeclareParameter(node, ns + ".occlusion.enable_creeping"); ip.occlusion.occlusion_creep_velocity = - node.declare_parameter(ns + ".occlusion.occlusion_creep_velocity"); - ip.occlusion.peeking_offset = node.declare_parameter(ns + ".occlusion.peeking_offset"); - ip.occlusion.free_space_max = node.declare_parameter(ns + ".occlusion.free_space_max"); - ip.occlusion.occupied_min = node.declare_parameter(ns + ".occlusion.occupied_min"); - ip.occlusion.do_dp = node.declare_parameter(ns + ".occlusion.do_dp"); + getOrDeclareParameter(node, ns + ".occlusion.occlusion_creep_velocity"); + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.do_dp = getOrDeclareParameter(node, ns + ".occlusion.do_dp"); ip.occlusion.before_creep_stop_time = - node.declare_parameter(ns + ".occlusion.before_creep_stop_time"); + getOrDeclareParameter(node, ns + ".occlusion.before_creep_stop_time"); ip.occlusion.min_vehicle_brake_for_rss = - node.declare_parameter(ns + ".occlusion.min_vehicle_brake_for_rss"); + getOrDeclareParameter(node, ns + ".occlusion.min_vehicle_brake_for_rss"); ip.occlusion.max_vehicle_velocity_for_rss = - node.declare_parameter(ns + ".occlusion.max_vehicle_velocity_for_rss"); - ip.occlusion.denoise_kernel = node.declare_parameter(ns + ".occlusion.denoise_kernel"); + getOrDeclareParameter(node, ns + ".occlusion.max_vehicle_velocity_for_rss"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); ip.occlusion.possible_object_bbox = - node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = - node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); } void IntersectionModuleManager::launchNewModules( @@ -263,10 +269,10 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node { const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; - mp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec"); + mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin"); + mp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); }