Skip to content

Commit

Permalink
refactor(intersection): use getOrDeclareParameter (autowarefoundation…
Browse files Browse the repository at this point in the history 
…#4767)

Signed-off-by: Takamasa Horibe <[email protected]>
  • Loading branch information
@TakaHoribe
TakaHoribe authored Aug 28, 2023
1 parent ddb49ec commit 61630e6
Showing 1 changed file with 54 additions and 48 deletions.
102 changes: 54 additions & 48 deletions planning/behavior_velocity_intersection_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
#include <behavior_velocity_planner_common/utilization/util.hpp>
#include <lanelet2_extension/utility/utilities.hpp>
#include <tier4_autoware_utils/ros/parameter.hpp>

#include <lanelet2_core/primitives/BasicRegulatoryElements.h>

Expand All @@ -29,94 +30,99 @@

namespace behavior_velocity_planner
{
using tier4_autoware_utils::getOrDeclareParameter;

IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterfaceWithRTC(
node, getModuleName(),
node.declare_parameter<bool>(std::string(getModuleName()) + ".enable_rtc.intersection")),
getOrDeclareParameter<bool>(node, std::string(getModuleName()) + ".enable_rtc.intersection")),
occlusion_rtc_interface_(
&node, "intersection_occlusion",
node.declare_parameter<bool>(
std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion"))
getOrDeclareParameter<bool>(
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<double>(ns + ".common.attention_area_margin");
getOrDeclareParameter<double>(node, ns + ".common.attention_area_margin");
ip.common.attention_area_length =
node.declare_parameter<double>(ns + ".common.attention_area_length");
getOrDeclareParameter<double>(node, ns + ".common.attention_area_length");
ip.common.attention_area_angle_thr =
node.declare_parameter<double>(ns + ".common.attention_area_angle_threshold");
ip.common.stop_line_margin = node.declare_parameter<double>(ns + ".common.stop_line_margin");
getOrDeclareParameter<double>(node, ns + ".common.attention_area_angle_threshold");
ip.common.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".common.stop_line_margin");
ip.common.intersection_velocity =
node.declare_parameter<double>(ns + ".common.intersection_velocity");
getOrDeclareParameter<double>(node, ns + ".common.intersection_velocity");
ip.common.intersection_max_acc =
node.declare_parameter<double>(ns + ".common.intersection_max_accel");
getOrDeclareParameter<double>(node, ns + ".common.intersection_max_accel");
ip.common.stop_overshoot_margin =
node.declare_parameter<double>(ns + ".common.stop_overshoot_margin");
getOrDeclareParameter<double>(node, ns + ".common.stop_overshoot_margin");
ip.common.use_intersection_area =
node.declare_parameter<bool>(ns + ".common.use_intersection_area");
getOrDeclareParameter<bool>(node, ns + ".common.use_intersection_area");
ip.common.path_interpolation_ds =
node.declare_parameter<double>(ns + ".common.path_interpolation_ds");
getOrDeclareParameter<double>(node, ns + ".common.path_interpolation_ds");
ip.common.consider_wrong_direction_vehicle =
node.declare_parameter<bool>(ns + ".common.consider_wrong_direction_vehicle");
getOrDeclareParameter<bool>(node, ns + ".common.consider_wrong_direction_vehicle");

ip.stuck_vehicle.use_stuck_stopline =
node.declare_parameter<bool>(ns + ".stuck_vehicle.use_stuck_stopline");
getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.use_stuck_stopline");
ip.stuck_vehicle.stuck_vehicle_detect_dist =
node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_detect_dist");
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist");
ip.stuck_vehicle.stuck_vehicle_ignore_dist =
node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") +
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") +
vehicle_info.max_longitudinal_offset_m;
ip.stuck_vehicle.stuck_vehicle_vel_thr =
node.declare_parameter<double>(ns + ".stuck_vehicle.stuck_vehicle_vel_thr");
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr");
/*
ip.stuck_vehicle.assumed_front_car_decel =
node.declare_parameter<double>(ns + ".stuck_vehicle.assumed_front_car_decel");
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.assumed_front_car_decel");
ip.stuck_vehicle.enable_front_car_decel_prediction =
node.declare_parameter<bool>(ns + ".stuck_vehicle.enable_front_car_decel_prediction");
getOrDeclareParameter<bool>(node, ns + ".stuck_vehicle.enable_front_car_decel_prediction");
*/
ip.stuck_vehicle.timeout_private_area =
node.declare_parameter<double>(ns + ".stuck_vehicle.timeout_private_area");
getOrDeclareParameter<double>(node, ns + ".stuck_vehicle.timeout_private_area");

ip.collision_detection.min_predicted_path_confidence =
node.declare_parameter<double>(ns + ".collision_detection.min_predicted_path_confidence");
getOrDeclareParameter<double>(node, ns + ".collision_detection.min_predicted_path_confidence");
ip.collision_detection.minimum_ego_predicted_velocity =
node.declare_parameter<double>(ns + ".collision_detection.minimum_ego_predicted_velocity");
getOrDeclareParameter<double>(node, ns + ".collision_detection.minimum_ego_predicted_velocity");
ip.collision_detection.state_transit_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.state_transit_margin_time");
ip.collision_detection.normal.collision_start_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.normal.collision_start_margin_time");
ip.collision_detection.normal.collision_end_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.normal.collision_end_margin_time");
ip.collision_detection.relaxed.collision_start_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_start_margin_time");
ip.collision_detection.relaxed.collision_end_margin_time =
node.declare_parameter<double>(ns + ".collision_detection.relaxed.collision_end_margin_time");
getOrDeclareParameter<double>(node, ns + ".collision_detection.state_transit_margin_time");
ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter<double>(
node, ns + ".collision_detection.normal.collision_start_margin_time");
ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter<double>(
node, ns + ".collision_detection.normal.collision_end_margin_time");
ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter<double>(
node, ns + ".collision_detection.relaxed.collision_start_margin_time");
ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter<double>(
node, ns + ".collision_detection.relaxed.collision_end_margin_time");
ip.collision_detection.keep_detection_vel_thr =
node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");
getOrDeclareParameter<double>(node, ns + ".collision_detection.keep_detection_vel_thr");

ip.occlusion.enable = node.declare_parameter<bool>(ns + ".occlusion.enable");
ip.occlusion.enable = getOrDeclareParameter<bool>(node, ns + ".occlusion.enable");
ip.occlusion.occlusion_attention_area_length =
node.declare_parameter<double>(ns + ".occlusion.occlusion_attention_area_length");
ip.occlusion.enable_creeping = node.declare_parameter<bool>(ns + ".occlusion.enable_creeping");
getOrDeclareParameter<double>(node, ns + ".occlusion.occlusion_attention_area_length");
ip.occlusion.enable_creeping =
getOrDeclareParameter<bool>(node, ns + ".occlusion.enable_creeping");
ip.occlusion.occlusion_creep_velocity =
node.declare_parameter<double>(ns + ".occlusion.occlusion_creep_velocity");
ip.occlusion.peeking_offset = node.declare_parameter<double>(ns + ".occlusion.peeking_offset");
ip.occlusion.free_space_max = node.declare_parameter<int>(ns + ".occlusion.free_space_max");
ip.occlusion.occupied_min = node.declare_parameter<int>(ns + ".occlusion.occupied_min");
ip.occlusion.do_dp = node.declare_parameter<bool>(ns + ".occlusion.do_dp");
getOrDeclareParameter<double>(node, ns + ".occlusion.occlusion_creep_velocity");
ip.occlusion.peeking_offset =
getOrDeclareParameter<double>(node, ns + ".occlusion.peeking_offset");
ip.occlusion.free_space_max = getOrDeclareParameter<int>(node, ns + ".occlusion.free_space_max");
ip.occlusion.occupied_min = getOrDeclareParameter<int>(node, ns + ".occlusion.occupied_min");
ip.occlusion.do_dp = getOrDeclareParameter<bool>(node, ns + ".occlusion.do_dp");
ip.occlusion.before_creep_stop_time =
node.declare_parameter<double>(ns + ".occlusion.before_creep_stop_time");
getOrDeclareParameter<double>(node, ns + ".occlusion.before_creep_stop_time");
ip.occlusion.min_vehicle_brake_for_rss =
node.declare_parameter<double>(ns + ".occlusion.min_vehicle_brake_for_rss");
getOrDeclareParameter<double>(node, ns + ".occlusion.min_vehicle_brake_for_rss");
ip.occlusion.max_vehicle_velocity_for_rss =
node.declare_parameter<double>(ns + ".occlusion.max_vehicle_velocity_for_rss");
ip.occlusion.denoise_kernel = node.declare_parameter<double>(ns + ".occlusion.denoise_kernel");
getOrDeclareParameter<double>(node, ns + ".occlusion.max_vehicle_velocity_for_rss");
ip.occlusion.denoise_kernel =
getOrDeclareParameter<double>(node, ns + ".occlusion.denoise_kernel");
ip.occlusion.possible_object_bbox =
node.declare_parameter<std::vector<double>>(ns + ".occlusion.possible_object_bbox");
getOrDeclareParameter<std::vector<double>>(node, ns + ".occlusion.possible_object_bbox");
ip.occlusion.ignore_parked_vehicle_speed_threshold =
node.declare_parameter<double>(ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold");
}

void IntersectionModuleManager::launchNewModules(
Expand Down Expand Up @@ -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<double>(ns + ".stop_duration_sec");
mp.stop_duration_sec = getOrDeclareParameter<double>(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<double>(ns + ".stop_line_margin");
mp.stop_line_margin = getOrDeclareParameter<double>(node, ns + ".stop_line_margin");
mp.path_interpolation_ds =
node.get_parameter("intersection.common.path_interpolation_ds").as_double();
}
Expand Down

0 comments on commit 61630e6

Please sign in to comment.