Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor(behavior_velocity_planner): delete default values #3201

Merged
merged 3 commits into from
Apr 6, 2023
Merged

refactor(behavior_velocity_planner): delete default values #3201

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c3f7d62
delete param
Mar 29, 2023
c4e4fc6
fix(intersection): remove unnecessary namespace
satoshi-ota Apr 6, 2023
12a535f
fix(intersection): remove param
satoshi-ota Apr 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 5 additions & 5 deletions planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,11 +60,11 @@ struct PlannerData
explicit PlannerData(rclcpp::Node & node)
: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo())
{
max_stop_acceleration_threshold = node.declare_parameter(
"max_accel", -5.0); // TODO(someone): read min_acc in velocity_controller.param.yaml?
max_stop_jerk_threshold = node.declare_parameter("max_jerk", -5.0);
system_delay = node.declare_parameter("system_delay", 0.50);
delay_response_time = node.declare_parameter("delay_response_time", 0.50);
max_stop_acceleration_threshold = node.declare_parameter<double>(
"max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml?
max_stop_jerk_threshold = node.declare_parameter<double>("max_jerk");
system_delay = node.declare_parameter<double>("system_delay");
delay_response_time = node.declare_parameter<double>("delay_response_time");
}

// msgs from callbacks that are used for data-ready
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@ class SceneModuleManagerInterface
const auto ns = std::string("~/debug/") + module_name;
pub_debug_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(ns, 1);
if (!node.has_parameter("is_publish_debug_path")) {
is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path", false);
is_publish_debug_path_ = node.declare_parameter<bool>("is_publish_debug_path");
} else {
is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool();
}
Expand Down
29 changes: 15 additions & 14 deletions planning/behavior_velocity_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,9 +149,10 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
debug_viz_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/path", 1);

// Parameters
forward_path_length_ = this->declare_parameter("forward_path_length", 1000.0);
backward_path_length_ = this->declare_parameter("backward_path_length", 5.0);
planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0);
forward_path_length_ = this->declare_parameter<double>("forward_path_length");
backward_path_length_ = this->declare_parameter<double>("backward_path_length");
planner_data_.stop_line_extend_length =
this->declare_parameter<double>("stop_line_extend_length");

// nearest search
planner_data_.ego_nearest_dist_threshold =
Expand All @@ -160,43 +161,43 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
this->declare_parameter<double>("ego_nearest_yaw_threshold");

// Initialize PlannerManager
if (this->declare_parameter("launch_crosswalk", true)) {
if (this->declare_parameter<bool>("launch_crosswalk")) {
planner_manager_.launchSceneModule(std::make_shared<CrosswalkModuleManager>(*this));
planner_manager_.launchSceneModule(std::make_shared<WalkwayModuleManager>(*this));
}
if (this->declare_parameter("launch_traffic_light", true)) {
if (this->declare_parameter<bool>("launch_traffic_light")) {
planner_manager_.launchSceneModule(std::make_shared<TrafficLightModuleManager>(*this));
}
if (this->declare_parameter("launch_intersection", true)) {
if (this->declare_parameter<bool>("launch_intersection")) {
// intersection module should be before merge from private to declare intersection parameters
planner_manager_.launchSceneModule(std::make_shared<IntersectionModuleManager>(*this));
planner_manager_.launchSceneModule(std::make_shared<MergeFromPrivateModuleManager>(*this));
}
if (this->declare_parameter("launch_blind_spot", true)) {
if (this->declare_parameter<bool>("launch_blind_spot")) {
planner_manager_.launchSceneModule(std::make_shared<BlindSpotModuleManager>(*this));
}
if (this->declare_parameter("launch_detection_area", true)) {
if (this->declare_parameter<bool>("launch_detection_area")) {
planner_manager_.launchSceneModule(std::make_shared<DetectionAreaModuleManager>(*this));
}
if (this->declare_parameter("launch_virtual_traffic_light", true)) {
if (this->declare_parameter<bool>("launch_virtual_traffic_light")) {
planner_manager_.launchSceneModule(std::make_shared<VirtualTrafficLightModuleManager>(*this));
}
// this module requires all the stop line.Therefore this modules should be placed at the bottom.
if (this->declare_parameter("launch_no_stopping_area", true)) {
if (this->declare_parameter<bool>("launch_no_stopping_area")) {
planner_manager_.launchSceneModule(std::make_shared<NoStoppingAreaModuleManager>(*this));
}
// permanent stop line module should be after no stopping area
if (this->declare_parameter("launch_stop_line", true)) {
if (this->declare_parameter<bool>("launch_stop_line")) {
planner_manager_.launchSceneModule(std::make_shared<StopLineModuleManager>(*this));
}
// to calculate ttc it's better to be after stop line
if (this->declare_parameter("launch_occlusion_spot", true)) {
if (this->declare_parameter<bool>("launch_occlusion_spot")) {
planner_manager_.launchSceneModule(std::make_shared<OcclusionSpotModuleManager>(*this));
}
if (this->declare_parameter("launch_run_out", false)) {
if (this->declare_parameter<bool>("launch_run_out")) {
planner_manager_.launchSceneModule(std::make_shared<RunOutModuleManager>(*this));
}
if (this->declare_parameter("launch_speed_bump", true)) {
if (this->declare_parameter<bool>("launch_speed_bump")) {
planner_manager_.launchSceneModule(std::make_shared<SpeedBumpModuleManager>(*this));
}
}
Expand Down
16 changes: 8 additions & 8 deletions planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,16 +29,16 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
{
const std::string ns(getModuleName());
planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false);
planner_param_.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0);
planner_param_.backward_length = node.declare_parameter(ns + ".backward_length", 15.0);
planner_param_.use_pass_judge_line = node.declare_parameter<bool>(ns + ".use_pass_judge_line");
planner_param_.stop_line_margin = node.declare_parameter<double>(ns + ".stop_line_margin");
planner_param_.backward_length = node.declare_parameter<double>(ns + ".backward_length");
planner_param_.ignore_width_from_center_line =
node.declare_parameter(ns + ".ignore_width_from_center_line", 1.0);
node.declare_parameter<double>(ns + ".ignore_width_from_center_line");
planner_param_.max_future_movement_time =
node.declare_parameter(ns + ".max_future_movement_time", 10.0);
planner_param_.threshold_yaw_diff =
node.declare_parameter(ns + ".threshold_yaw_diff", M_PI / 6.0);
planner_param_.adjacent_extend_width = node.declare_parameter(ns + ".adjacent_extend_width", 0.5);
node.declare_parameter<double>(ns + ".max_future_movement_time");
planner_param_.threshold_yaw_diff = node.declare_parameter<double>(ns + ".threshold_yaw_diff");
planner_param_.adjacent_extend_width =
node.declare_parameter<double>(ns + ".adjacent_extend_width");
}

void BlindSpotModuleManager::launchNewModules(
Expand Down
57 changes: 28 additions & 29 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,46 +83,45 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)

// for crosswalk parameters
auto & cp = crosswalk_planner_param_;
cp.show_processing_time = node.declare_parameter(ns + ".show_processing_time", true);
cp.show_processing_time = node.declare_parameter<bool>(ns + ".show_processing_time");

// param for stop position
cp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance", 1.5);
cp.stop_margin = node.declare_parameter(ns + ".stop_margin", 1.0);
cp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 5.0);
cp.stop_position_threshold = node.declare_parameter(ns + ".stop_position_threshold", 1.0);
cp.stop_line_distance = node.declare_parameter<double>(ns + ".stop_line_distance");
cp.stop_margin = node.declare_parameter<double>(ns + ".stop_margin");
cp.stop_line_margin = node.declare_parameter<double>(ns + ".stop_line_margin");
cp.stop_position_threshold = node.declare_parameter<double>(ns + ".stop_position_threshold");

// param for ego velocity
cp.min_slow_down_velocity = node.declare_parameter(ns + ".min_slow_down_velocity", 5.0 / 3.6);
cp.max_slow_down_jerk = node.declare_parameter(ns + ".max_slow_down_jerk", -0.7);
cp.max_slow_down_accel = node.declare_parameter(ns + ".max_slow_down_accel", -2.5);
cp.no_relax_velocity = node.declare_parameter(ns + ".no_relax_velocity", 2.78);
cp.min_slow_down_velocity = node.declare_parameter<double>(ns + ".min_slow_down_velocity");
cp.max_slow_down_jerk = node.declare_parameter<double>(ns + ".max_slow_down_jerk");
cp.max_slow_down_accel = node.declare_parameter<double>(ns + ".max_slow_down_accel");
cp.no_relax_velocity = node.declare_parameter<double>(ns + ".no_relax_velocity");

// param for stuck vehicle
cp.stuck_vehicle_velocity = node.declare_parameter(ns + ".stuck_vehicle_velocity", 1.0);
cp.max_lateral_offset = node.declare_parameter(ns + ".max_lateral_offset", 2.0);
cp.stuck_vehicle_velocity = node.declare_parameter<double>(ns + ".stuck_vehicle_velocity");
cp.max_lateral_offset = node.declare_parameter<double>(ns + ".max_lateral_offset");
cp.stuck_vehicle_attention_range =
node.declare_parameter(ns + ".stuck_vehicle_attention_range", 10.0);
node.declare_parameter<double>(ns + ".stuck_vehicle_attention_range");

// param for pass judge logic
cp.ego_pass_first_margin = node.declare_parameter(ns + ".ego_pass_first_margin", 6.0);
cp.ego_pass_later_margin = node.declare_parameter(ns + ".ego_pass_later_margin", 10.0);
cp.stop_object_velocity =
node.declare_parameter(ns + ".stop_object_velocity_threshold", 0.5 / 3.6);
cp.min_object_velocity = node.declare_parameter(ns + ".min_object_velocity", 5.0 / 3.6);
cp.max_yield_timeout = node.declare_parameter(ns + ".max_yield_timeout", 3.0);
cp.ego_pass_first_margin = node.declare_parameter<double>(ns + ".ego_pass_first_margin");
cp.ego_pass_later_margin = node.declare_parameter<double>(ns + ".ego_pass_later_margin");
cp.stop_object_velocity = node.declare_parameter<double>(ns + ".stop_object_velocity_threshold");
cp.min_object_velocity = node.declare_parameter<double>(ns + ".min_object_velocity");
cp.max_yield_timeout = node.declare_parameter<double>(ns + ".max_yield_timeout");
cp.ego_yield_query_stop_duration =
node.declare_parameter(ns + ".ego_yield_query_stop_duration", 0.1);
node.declare_parameter<double>(ns + ".ego_yield_query_stop_duration");

// param for input data
cp.external_input_timeout = node.declare_parameter(ns + ".external_input_timeout", 1.0);
cp.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout", 1.0);
cp.external_input_timeout = node.declare_parameter<double>(ns + ".external_input_timeout");
cp.tl_state_timeout = node.declare_parameter<double>(ns + ".tl_state_timeout");

// param for target area & object
cp.crosswalk_attention_range = node.declare_parameter(ns + ".crosswalk_attention_range", 10.0);
cp.look_unknown = node.declare_parameter(ns + ".target_object.unknown", true);
cp.look_bicycle = node.declare_parameter(ns + ".target_object.bicycle", true);
cp.look_motorcycle = node.declare_parameter(ns + ".target_object.motorcycle", true);
cp.look_pedestrian = node.declare_parameter(ns + ".target_object.pedestrian", true);
cp.crosswalk_attention_range = node.declare_parameter<double>(ns + ".crosswalk_attention_range");
cp.look_unknown = node.declare_parameter<bool>(ns + ".target_object.unknown");
cp.look_bicycle = node.declare_parameter<bool>(ns + ".target_object.bicycle");
cp.look_motorcycle = node.declare_parameter<bool>(ns + ".target_object.motorcycle");
cp.look_pedestrian = node.declare_parameter<bool>(ns + ".target_object.pedestrian");
}

void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path)
Expand Down Expand Up @@ -162,9 +161,9 @@ WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)

// for walkway parameters
auto & wp = walkway_planner_param_;
wp.stop_line_distance = node.declare_parameter(ns + ".stop_line_distance", 1.0);
wp.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0);
wp.external_input_timeout = node.declare_parameter(ns + ".external_input_timeout", 1.0);
wp.stop_line_distance = node.declare_parameter<double>(ns + ".stop_line_distance");
wp.stop_duration_sec = node.declare_parameter<double>(ns + ".stop_duration_sec");
wp.external_input_timeout = node.declare_parameter<double>(ns + ".external_input_timeout");
}

void WalkwayModuleManager::launchNewModules(
Expand Down
14 changes: 7 additions & 7 deletions planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
{
const std::string ns(getModuleName());
planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0);
planner_param_.use_dead_line = node.declare_parameter(ns + ".use_dead_line", false);
planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 5.0);
planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false);
planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0);
planner_param_.stop_margin = node.declare_parameter<double>(ns + ".stop_margin");
planner_param_.use_dead_line = node.declare_parameter<bool>(ns + ".use_dead_line");
planner_param_.dead_line_margin = node.declare_parameter<double>(ns + ".dead_line_margin");
planner_param_.use_pass_judge_line = node.declare_parameter<bool>(ns + ".use_pass_judge_line");
planner_param_.state_clear_time = node.declare_parameter<double>(ns + ".state_clear_time");
planner_param_.hold_stop_margin_distance =
node.declare_parameter(ns + ".hold_stop_margin_distance", 0.0);
node.declare_parameter<double>(ns + ".hold_stop_margin_distance");
planner_param_.distance_to_judge_over_stop_line =
node.declare_parameter(ns + ".distance_to_judge_over_stop_line", 0.5);
node.declare_parameter<double>(ns + ".distance_to_judge_over_stop_line");
}

void DetectionAreaModuleManager::launchNewModules(
Expand Down
49 changes: 26 additions & 23 deletions planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,33 +32,36 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
const std::string ns(getModuleName());
auto & ip = intersection_param_;
const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0);
ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0);
ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833);
ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0);
ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) +
ip.state_transit_margin_time = node.declare_parameter<double>(ns + ".state_transit_margin_time");
ip.stop_line_margin = node.declare_parameter<double>(ns + ".stop_line_margin");
ip.keep_detection_vel_thr = node.declare_parameter<double>(ns + ".keep_detection_vel_thr");
ip.stuck_vehicle_detect_dist = node.declare_parameter<double>(ns + ".stuck_vehicle_detect_dist");
ip.stuck_vehicle_ignore_dist = node.declare_parameter<double>(ns + ".stuck_vehicle_ignore_dist") +
vehicle_info.max_longitudinal_offset_m;
ip.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle_vel_thr", 3.0 / 3.6);
ip.intersection_velocity = node.declare_parameter(ns + ".intersection_velocity", 10.0 / 3.6);
ip.intersection_max_acc = node.declare_parameter(ns + ".intersection_max_accel", 0.5);
ip.detection_area_margin = node.declare_parameter(ns + ".detection_area_margin", 0.5);
ip.detection_area_right_margin = node.declare_parameter(ns + ".detection_area_right_margin", 0.5);
ip.detection_area_left_margin = node.declare_parameter(ns + ".detection_area_left_margin", 0.5);
ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0);
ip.stuck_vehicle_vel_thr = node.declare_parameter<double>(ns + ".stuck_vehicle_vel_thr");
ip.intersection_velocity = node.declare_parameter<double>(ns + ".intersection_velocity");
ip.intersection_max_acc = node.declare_parameter<double>(ns + ".intersection_max_accel");
ip.detection_area_margin = node.declare_parameter<double>(ns + ".detection_area_margin");
ip.detection_area_right_margin =
node.declare_parameter<double>(ns + ".detection_area_right_margin");
ip.detection_area_left_margin =
node.declare_parameter<double>(ns + ".detection_area_left_margin");
ip.detection_area_length = node.declare_parameter<double>(ns + ".detection_area_length");
ip.detection_area_angle_thr =
node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0);
node.declare_parameter<double>(ns + ".detection_area_angle_threshold");
ip.min_predicted_path_confidence =
node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05);
ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
node.declare_parameter<double>(ns + ".min_predicted_path_confidence");
ip.external_input_timeout = node.declare_parameter<double>(ns + ".external_input_timeout");
ip.minimum_ego_predicted_velocity =
node.declare_parameter(ns + ".minimum_ego_predicted_velocity", 1.388);
ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0);
ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0);
ip.use_stuck_stopline = node.declare_parameter(ns + ".use_stuck_stopline", true);
ip.assumed_front_car_decel = node.declare_parameter(ns + ".assumed_front_car_decel", 1.0);
node.declare_parameter<double>(ns + ".minimum_ego_predicted_velocity");
ip.collision_start_margin_time =
node.declare_parameter<double>(ns + ".collision_start_margin_time");
ip.collision_end_margin_time = node.declare_parameter<double>(ns + ".collision_end_margin_time");
ip.use_stuck_stopline = node.declare_parameter<bool>(ns + ".use_stuck_stopline");
ip.assumed_front_car_decel = node.declare_parameter<double>(ns + ".assumed_front_car_decel");
ip.enable_front_car_decel_prediction =
node.declare_parameter(ns + ".enable_front_car_decel_prediction", false);
ip.stop_overshoot_margin = node.declare_parameter(ns + ".stop_overshoot_margin", 0.5);
node.declare_parameter<bool>(ns + ".enable_front_car_decel_prediction");
ip.stop_overshoot_margin = node.declare_parameter<double>(ns + ".stop_overshoot_margin");
}

MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node)
Expand All @@ -67,7 +70,7 @@ 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 + ".merge_from_private_area.stop_duration_sec", 1.0);
node.declare_parameter<double>(ns + ".merge_from_private_area.stop_duration_sec");
mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double();
mp.detection_area_right_margin =
node.get_parameter("intersection.detection_area_right_margin").as_double();
Expand Down
Loading