refactor(motion_velocity_smoother): delete default values

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -42,8 +42,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo();
   wheelbase_ = vehicle_info.wheel_base_m;
   initCommonParam();
-  over_stop_velocity_warn_thr_ =
-    declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0));
+  over_stop_velocity_warn_thr_ = declare_parameter<double>("over_stop_velocity_warn_thr");
 
   // create smoother
   switch (node_param_.algorithm_type) {
@@ -101,7 +100,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     std::bind(&MotionVelocitySmootherNode::onParameter, this, _1));
 
   // debug
-  publish_debug_trajs_ = declare_parameter("publish_debug_trajs", true);
+  publish_debug_trajs_ = declare_parameter<bool>("publish_debug_trajs");
   debug_closest_velocity_ = create_publisher<Float32Stamped>("~/closest_velocity", 1);
   debug_closest_acc_ = create_publisher<Float32Stamped>("~/closest_acceleration", 1);
   debug_closest_jerk_ = create_publisher<Float32Stamped>("~/closest_jerk", 1);
@@ -243,34 +242,33 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
 void MotionVelocitySmootherNode::initCommonParam()
 {
   auto & p = node_param_;
-  p.max_velocity = declare_parameter("max_velocity", 20.0);  // 72.0 kmph
+  p.max_velocity = declare_parameter<double>("max_velocity");  // 72.0 kmph
   p.margin_to_insert_external_velocity_limit =
-    declare_parameter("margin_to_insert_external_velocity_limit", 0.3);
-  p.replan_vel_deviation = declare_parameter("replan_vel_deviation", 3.0);
-  p.engage_velocity = declare_parameter("engage_velocity", 0.3);
-  p.engage_acceleration = declare_parameter("engage_acceleration", 0.1);
-  p.engage_exit_ratio = declare_parameter("engage_exit_ratio", 0.5);
+    declare_parameter<double>("margin_to_insert_external_velocity_limit");
+  p.replan_vel_deviation = declare_parameter<double>("replan_vel_deviation");
+  p.engage_velocity = declare_parameter<double>("engage_velocity");
+  p.engage_acceleration = declare_parameter<double>("engage_acceleration");
+  p.engage_exit_ratio = declare_parameter<double>("engage_exit_ratio");
   p.engage_exit_ratio = std::min(std::max(p.engage_exit_ratio, 0.0), 1.0);
-  p.stopping_velocity =
-    declare_parameter("stopping_velocity", tier4_autoware_utils::kmph2mps(10.0));
-  p.stopping_distance = declare_parameter("stopping_distance", 0.0);
-  p.extract_ahead_dist = declare_parameter("extract_ahead_dist", 200.0);
-  p.extract_behind_dist = declare_parameter("extract_behind_dist", 3.0);
-  p.stop_dist_to_prohibit_engage = declare_parameter("stop_dist_to_prohibit_engage", 1.5);
+  p.stopping_velocity = declare_parameter<double>("stopping_velocity");
+  p.stopping_distance = declare_parameter<double>("stopping_distance");
+  p.extract_ahead_dist = declare_parameter<double>("extract_ahead_dist");
+  p.extract_behind_dist = declare_parameter<double>("extract_behind_dist");
+  p.stop_dist_to_prohibit_engage = declare_parameter<double>("stop_dist_to_prohibit_engage");
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
   p.post_resample_param.max_trajectory_length =
-    declare_parameter("post_max_trajectory_length", 300.0);
+    declare_parameter<double>("post_max_trajectory_length");
   p.post_resample_param.min_trajectory_length =
-    declare_parameter("post_min_trajectory_length", 30.0);
-  p.post_resample_param.resample_time = declare_parameter("post_resample_time", 10.0);
-  p.post_resample_param.dense_resample_dt = declare_parameter("post_dense_resample_dt", 0.1);
+    declare_parameter<double>("post_min_trajectory_length");
+  p.post_resample_param.resample_time = declare_parameter<double>("post_resample_time");
+  p.post_resample_param.dense_resample_dt = declare_parameter<double>("post_dense_resample_dt");
   p.post_resample_param.dense_min_interval_distance =
-    declare_parameter("post_dense_min_interval_distance", 0.1);
-  p.post_resample_param.sparse_resample_dt = declare_parameter("post_sparse_resample_dt", 0.1);
+    declare_parameter<double>("post_dense_min_interval_distance");
+  p.post_resample_param.sparse_resample_dt = declare_parameter<double>("post_sparse_resample_dt");
   p.post_resample_param.sparse_min_interval_distance =
-    declare_parameter("post_sparse_min_interval_distance", 1.0);
-  p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered"));
+    declare_parameter<double>("post_sparse_min_interval_distance");
+  p.algorithm_type = getAlgorithmType(declare_parameter<std::string>("algorithm_type"));
 }
 
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const

planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -69,24 +69,24 @@ AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Nod
 : SmootherBase(node)
 {
   auto & p = smoother_param_;
-  p.resample.ds_resample = node.declare_parameter("resample.ds_resample", 0.1);
-  p.resample.num_resample = node.declare_parameter("resample.num_resample", 1);
-  p.resample.delta_yaw_threshold = node.declare_parameter("resample.delta_yaw_threshold", 0.785);
+  p.resample.ds_resample = node.declare_parameter<double>("resample.ds_resample");
+  p.resample.num_resample = node.declare_parameter<int>("resample.num_resample");
+  p.resample.delta_yaw_threshold = node.declare_parameter<double>("resample.delta_yaw_threshold");
   p.latacc.enable_constant_velocity_while_turning =
-    node.declare_parameter("latacc.enable_constant_velocity_while_turning", false);
+    node.declare_parameter<bool>("latacc.enable_constant_velocity_while_turning");
   p.latacc.constant_velocity_dist_threshold =
-    node.declare_parameter("latacc.constant_velocity_dist_threshold", 2.0);
-  p.forward.max_acc = node.declare_parameter("forward.max_acc", 1.0);
-  p.forward.min_acc = node.declare_parameter("forward.min_acc", -1.0);
-  p.forward.max_jerk = node.declare_parameter("forward.max_jerk", 0.3);
-  p.forward.min_jerk = node.declare_parameter("forward.min_jerk", -0.3);
-  p.forward.kp = node.declare_parameter("forward.kp", 0.3);
-  p.backward.start_jerk = node.declare_parameter("backward.start_jerk", -0.1);
-  p.backward.min_jerk_mild_stop = node.declare_parameter("backward.min_jerk_mild_stop", -0.3);
-  p.backward.min_jerk = node.declare_parameter("backward.min_jerk", -1.5);
-  p.backward.min_acc_mild_stop = node.declare_parameter("backward.min_acc_mild_stop", -1.0);
-  p.backward.min_acc = node.declare_parameter("backward.min_acc", -2.5);
-  p.backward.span_jerk = node.declare_parameter("backward.span_jerk", -0.01);
+    node.declare_parameter<double>("latacc.constant_velocity_dist_threshold");
+  p.forward.max_acc = node.declare_parameter<double>("forward.max_acc");
+  p.forward.min_acc = node.declare_parameter<double>("forward.min_acc");
+  p.forward.max_jerk = node.declare_parameter<double>("forward.max_jerk");
+  p.forward.min_jerk = node.declare_parameter<double>("forward.min_jerk");
+  p.forward.kp = node.declare_parameter<double>("forward.kp");
+  p.backward.start_jerk = node.declare_parameter<double>("backward.start_jerk");
+  p.backward.min_jerk_mild_stop = node.declare_parameter<double>("backward.min_jerk_mild_stop");
+  p.backward.min_jerk = node.declare_parameter<double>("backward.min_jerk");
+  p.backward.min_acc_mild_stop = node.declare_parameter<double>("backward.min_acc_mild_stop");
+  p.backward.min_acc = node.declare_parameter<double>("backward.min_acc");
+  p.backward.span_jerk = node.declare_parameter<double>("backward.span_jerk");
 }
 
 void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param)

planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -31,11 +31,11 @@ namespace motion_velocity_smoother
 JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node)
 {
   auto & p = smoother_param_;
-  p.jerk_weight = node.declare_parameter("jerk_weight", 10.0);
-  p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0);
-  p.over_a_weight = node.declare_parameter("over_a_weight", 5000.0);
-  p.over_j_weight = node.declare_parameter("over_j_weight", 2000.0);
-  p.jerk_filter_ds = node.declare_parameter("jerk_filter_ds", 0.1);
+  p.jerk_weight = node.declare_parameter<double>("jerk_weight");
+  p.over_v_weight = node.declare_parameter<double>("over_v_weight");
+  p.over_a_weight = node.declare_parameter<double>("over_a_weight");
+  p.over_j_weight = node.declare_parameter<double>("over_j_weight");
+  p.jerk_filter_ds = node.declare_parameter<double>("jerk_filter_ds");
 
   qp_solver_.updateMaxIter(20000);
   qp_solver_.updateRhoInterval(0);  // 0 means automatic

planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp

@@ -27,9 +27,9 @@ namespace motion_velocity_smoother
 L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node)
 {
   auto & p = smoother_param_;
-  p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight", 100.0);
-  p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0);
-  p.over_a_weight = node.declare_parameter("over_a_weight", 1000.0);
+  p.pseudo_jerk_weight = node.declare_parameter<double>("pseudo_jerk_weight");
+  p.over_v_weight = node.declare_parameter<double>("over_v_weight");
+  p.over_a_weight = node.declare_parameter<double>("over_a_weight");
 
   qp_solver_.updateMaxIter(4000);
   qp_solver_.updateRhoInterval(0);  // 0 means automatic

planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

@@ -27,9 +27,9 @@ namespace motion_velocity_smoother
 LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node)
 {
   auto & p = smoother_param_;
-  p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight", 200.0);
-  p.over_v_weight = node.declare_parameter("over_v_weight", 100000.0);
-  p.over_a_weight = node.declare_parameter("over_a_weight", 5000.0);
+  p.pseudo_jerk_weight = node.declare_parameter<double>("pseudo_jerk_weight");
+  p.over_v_weight = node.declare_parameter<double>("over_v_weight");
+  p.over_a_weight = node.declare_parameter<double>("over_a_weight");
 
   qp_solver_.updateMaxIter(20000);
   qp_solver_.updateRhoInterval(5000);

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -31,30 +31,31 @@ using vehicle_info_util::VehicleInfoUtil;
 SmootherBase::SmootherBase(rclcpp::Node & node)
 {
   auto & p = base_param_;
-  p.max_accel = node.declare_parameter("normal.max_acc", 2.0);
-  p.min_decel = node.declare_parameter("normal.min_acc", -3.0);
-  p.stop_decel = node.declare_parameter("stop_decel", 0.0);
-  p.max_jerk = node.declare_parameter("normal.max_jerk", 0.3);
-  p.min_jerk = node.declare_parameter("normal.min_jerk", -0.1);
-  p.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2);
+  p.max_accel = node.declare_parameter<double>("normal.max_acc");
+  p.min_decel = node.declare_parameter<double>("normal.min_acc");
+  p.stop_decel = node.declare_parameter<double>("stop_decel");
+  p.max_jerk = node.declare_parameter<double>("normal.max_jerk");
+  p.min_jerk = node.declare_parameter<double>("normal.min_jerk");
+  p.max_lateral_accel = node.declare_parameter<double>("max_lateral_accel");
   p.min_decel_for_lateral_acc_lim_filter =
-    node.declare_parameter("min_decel_for_lateral_acc_lim_filter", -2.5);
-  p.sample_ds = node.declare_parameter("resample_ds", 0.5);
-  p.curvature_threshold = node.declare_parameter("curvature_threshold", 0.2);
-  p.max_steering_angle_rate = node.declare_parameter("max_steering_angle_rate", 5.0);
-  p.curvature_calculation_distance = node.declare_parameter("curvature_calculation_distance", 1.0);
-  p.decel_distance_before_curve = node.declare_parameter("decel_distance_before_curve", 3.5);
-  p.decel_distance_after_curve = node.declare_parameter("decel_distance_after_curve", 0.0);
-  p.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38);
-  p.resample_param.max_trajectory_length = node.declare_parameter("max_trajectory_length", 200.0);
-  p.resample_param.min_trajectory_length = node.declare_parameter("min_trajectory_length", 30.0);
-  p.resample_param.resample_time = node.declare_parameter("resample_time", 10.0);
-  p.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt", 0.1);
+    node.declare_parameter<double>("min_decel_for_lateral_acc_lim_filter");
+  p.sample_ds = node.declare_parameter<double>("resample_ds");
+  p.curvature_threshold = node.declare_parameter<double>("curvature_threshold");
+  p.max_steering_angle_rate = node.declare_parameter<double>("max_steering_angle_rate");
+  p.curvature_calculation_distance =
+    node.declare_parameter<double>("curvature_calculation_distance");
+  p.decel_distance_before_curve = node.declare_parameter<double>("decel_distance_before_curve");
+  p.decel_distance_after_curve = node.declare_parameter<double>("decel_distance_after_curve");
+  p.min_curve_velocity = node.declare_parameter<double>("min_curve_velocity");
+  p.resample_param.max_trajectory_length = node.declare_parameter<double>("max_trajectory_length");
+  p.resample_param.min_trajectory_length = node.declare_parameter<double>("min_trajectory_length");
+  p.resample_param.resample_time = node.declare_parameter<double>("resample_time");
+  p.resample_param.dense_resample_dt = node.declare_parameter<double>("dense_resample_dt");
   p.resample_param.dense_min_interval_distance =
-    node.declare_parameter("dense_min_interval_distance", 0.1);
-  p.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt", 0.5);
+    node.declare_parameter<double>("dense_min_interval_distance");
+  p.resample_param.sparse_resample_dt = node.declare_parameter<double>("sparse_resample_dt");
   p.resample_param.sparse_min_interval_distance =
-    node.declare_parameter("sparse_min_interval_distance", 4.0);
+    node.declare_parameter<double>("sparse_min_interval_distance");
 }
 
 void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }