fix(motion_velocity_smoother): revert loading smoother params from node

instance

This reverts commit 2b8d5fc.

planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp

@@ -194,6 +194,16 @@ class MotionVelocitySmootherNode : public rclcpp::Node
   // parameter handling
   void initCommonParam();
 
+  void initSmootherBaseParam();
+
+  void initJerkFilteredSmootherParam();
+
+  void initL2PseudoJerkSmootherParam();
+
+  void initLinfPseudoJerkSmootherParam();
+
+  void initAnalyticalJerkConstrainedSmootherParam();
+
   // debug
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
   std::shared_ptr<rclcpp::Time> prev_time_;

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -38,7 +38,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     struct
     {
       double ds_resample;
-      int num_resample;
+      double num_resample;
       double delta_yaw_threshold;
     } resample;
     struct
@@ -65,7 +65,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     } backward;
   };
 
-  explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node);
+  explicit AnalyticalJerkConstrainedSmoother(const Param & p);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -40,7 +40,7 @@ class JerkFilteredSmoother : public SmootherBase
     double jerk_filter_ds;
   };
 
-  explicit JerkFilteredSmoother(rclcpp::Node & node);
+  explicit JerkFilteredSmoother(const Param & p);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp

@@ -38,7 +38,7 @@ class L2PseudoJerkSmoother : public SmootherBase
     double over_a_weight;
   };
 
-  explicit L2PseudoJerkSmoother(rclcpp::Node & node);
+  explicit L2PseudoJerkSmoother(const Param & smoother_param);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp

@@ -38,7 +38,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
     double over_a_weight;
   };
 
-  explicit LinfPseudoJerkSmoother(rclcpp::Node & node);
+  explicit LinfPseudoJerkSmoother(const Param & smoother_param);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp

@@ -50,7 +50,6 @@ class SmootherBase
     resampling::ResampleParam resample_param;
   };
 
-  explicit SmootherBase(rclcpp::Node & node);
   virtual ~SmootherBase() = default;
   virtual bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -42,9 +42,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
     declare_parameter("over_stop_velocity_warn_thr", tier4_autoware_utils::kmph2mps(5.0));
 
   // create smoother
+  initSmootherBaseParam();
   switch (node_param_.algorithm_type) {
     case AlgorithmType::JERK_FILTERED: {
-      smoother_ = std::make_shared<JerkFilteredSmoother>(*this);
+      initJerkFilteredSmootherParam();
+      smoother_ = std::make_shared<JerkFilteredSmoother>(jerk_filtered_smoother_param_);
+      smoother_->setParam(base_param_);
 
       // Set Publisher for jerk filtered algorithm
       pub_forward_filtered_trajectory_ =
@@ -58,15 +61,22 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
       break;
     }
     case AlgorithmType::L2: {
-      smoother_ = std::make_shared<L2PseudoJerkSmoother>(*this);
+      initL2PseudoJerkSmootherParam();
+      smoother_ = std::make_shared<L2PseudoJerkSmoother>(l2_pseudo_jerk_smoother_param_);
+      smoother_->setParam(base_param_);
       break;
     }
     case AlgorithmType::LINF: {
-      smoother_ = std::make_shared<LinfPseudoJerkSmoother>(*this);
+      initLinfPseudoJerkSmootherParam();
+      smoother_ = std::make_shared<LinfPseudoJerkSmoother>(linf_pseudo_jerk_smoother_param_);
+      smoother_->setParam(base_param_);
       break;
     }
     case AlgorithmType::ANALYTICAL: {
-      smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(*this);
+      initAnalyticalJerkConstrainedSmootherParam();
+      smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(
+        analytical_jerk_constrained_smoother_param_);
+      smoother_->setParam(base_param_);
       break;
     }
     default:
@@ -199,6 +209,8 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
     }
     case AlgorithmType::ANALYTICAL: {
       auto & p = analytical_jerk_constrained_smoother_param_;
+      update_param("resample.ds_resample", p.resample.ds_resample);
+      update_param("resample.num_resample", p.resample.num_resample);
       update_param("resample.delta_yaw_threshold", p.resample.delta_yaw_threshold);
       update_param(
         "latacc.constant_velocity_dist_threshold", p.latacc.constant_velocity_dist_threshold);
@@ -258,6 +270,81 @@ void MotionVelocitySmootherNode::initCommonParam()
   p.algorithm_type = getAlgorithmType(declare_parameter("algorithm_type", "JerkFiltered"));
 }
 
+void MotionVelocitySmootherNode::initSmootherBaseParam()
+{
+  auto & p = base_param_;
+  p.max_accel = declare_parameter("normal.max_acc", 2.0);   // 0.11G
+  p.min_decel = declare_parameter("normal.min_acc", -3.0);  // -0.2G
+  p.stop_decel = declare_parameter("stop_decel", 0.0);
+  p.max_jerk = declare_parameter("normal.max_jerk", 0.3);
+  p.min_jerk = declare_parameter("normal.min_jerk", -0.1);
+  p.max_lateral_accel = declare_parameter("max_lateral_accel", 0.2);
+  p.decel_distance_before_curve = declare_parameter("decel_distance_before_curve", 3.5);
+  p.decel_distance_after_curve = declare_parameter("decel_distance_after_curve", 0.0);
+  p.min_curve_velocity = declare_parameter("min_curve_velocity", 1.38);
+  p.resample_param.max_trajectory_length = declare_parameter("max_trajectory_length", 200.0);
+  p.resample_param.min_trajectory_length = declare_parameter("min_trajectory_length", 30.0);
+  p.resample_param.resample_time = declare_parameter("resample_time", 10.0);
+  p.resample_param.dense_resample_dt = declare_parameter("dense_resample_dt", 0.1);
+  p.resample_param.dense_min_interval_distance =
+    declare_parameter("dense_min_interval_distance", 0.1);
+  p.resample_param.sparse_resample_dt = declare_parameter("sparse_resample_dt", 0.5);
+  p.resample_param.sparse_min_interval_distance =
+    declare_parameter("sparse_min_interval_distance", 4.0);
+}
+
+void MotionVelocitySmootherNode::initJerkFilteredSmootherParam()
+{
+  auto & p = jerk_filtered_smoother_param_;
+  p.jerk_weight = declare_parameter("jerk_weight", 10.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 5000.0);
+  p.over_j_weight = declare_parameter("over_j_weight", 2000.0);
+  p.jerk_filter_ds = declare_parameter("jerk_filter_ds", 0.1);
+}
+
+void MotionVelocitySmootherNode::initL2PseudoJerkSmootherParam()
+{
+  auto & p = l2_pseudo_jerk_smoother_param_;
+  p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 100.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 1000.0);
+}
+
+void MotionVelocitySmootherNode::initLinfPseudoJerkSmootherParam()
+{
+  auto & p = linf_pseudo_jerk_smoother_param_;
+  p.pseudo_jerk_weight = declare_parameter("pseudo_jerk_weight", 200.0);
+  p.over_v_weight = declare_parameter("over_v_weight", 100000.0);
+  p.over_a_weight = declare_parameter("over_a_weight", 5000.0);
+}
+
+void MotionVelocitySmootherNode::initAnalyticalJerkConstrainedSmootherParam()
+{
+  auto & p = analytical_jerk_constrained_smoother_param_;
+  p.resample.ds_resample = declare_parameter("resample.ds_resample", 0.1);
+  p.resample.num_resample = declare_parameter("resample.num_resample", 1);
+  p.resample.delta_yaw_threshold = declare_parameter("resample.delta_yaw_threshold", 0.785);
+
+  p.latacc.enable_constant_velocity_while_turning =
+    declare_parameter("latacc.enable_constant_velocity_while_turning", false);
+  p.latacc.constant_velocity_dist_threshold =
+    declare_parameter("latacc.constant_velocity_dist_threshold", 2.0);
+
+  p.forward.max_acc = declare_parameter("forward.max_acc", 1.0);
+  p.forward.min_acc = declare_parameter("forward.min_acc", -1.0);
+  p.forward.max_jerk = declare_parameter("forward.max_jerk", 0.3);
+  p.forward.min_jerk = declare_parameter("forward.min_jerk", -0.3);
+  p.forward.kp = declare_parameter("forward.kp", 0.3);
+
+  p.backward.start_jerk = declare_parameter("backward.start_jerk", -0.1);
+  p.backward.min_jerk_mild_stop = declare_parameter("backward.min_jerk_mild_stop", -0.3);
+  p.backward.min_jerk = declare_parameter("backward.min_jerk", -1.5);
+  p.backward.min_acc_mild_stop = declare_parameter("backward.min_acc_mild_stop", -1.0);
+  p.backward.min_acc = declare_parameter("backward.min_acc", -2.5);
+  p.backward.span_jerk = declare_parameter("backward.span_jerk", -0.01);
+}
+
 void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory) const
 {
   Trajectory publishing_trajectory = tier4_autoware_utils::convertToTrajectory(trajectory);

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

@@ -62,30 +62,10 @@ bool applyMaxVelocity(
 
 namespace motion_velocity_smoother
 {
-AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node)
-: SmootherBase(node)
+AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(const Param & smoother_param)
+: smoother_param_(smoother_param)
 {
-  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.latacc.enable_constant_velocity_while_turning =
-    node.declare_parameter("latacc.enable_constant_velocity_while_turning", false);
-  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);
 }
-
 void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param)
 {
   smoother_param_ = smoother_param;
@@ -247,7 +227,7 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
       continue;
     }
 
-    for (size_t j = 0; j < static_cast<size_t>(smoother_param_.resample.num_resample); ++j) {
+    for (size_t j = 0; j < smoother_param_.resample.num_resample; ++j) {
       auto tp = input.at(i);
 
       tp.pose = lerpByPose(tp0.pose, tp1.pose, s);

planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -26,15 +26,9 @@
 
 namespace motion_velocity_smoother
 {
-JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node)
+JerkFilteredSmoother::JerkFilteredSmoother(const Param & smoother_param)
+: smoother_param_{smoother_param}
 {
-  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);
-
   qp_solver_.updateMaxIter(20000);
   qp_solver_.updateRhoInterval(0);  // 0 means automatic
   qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4

planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp

@@ -24,13 +24,9 @@
 
 namespace motion_velocity_smoother
 {
-L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node)
+L2PseudoJerkSmoother::L2PseudoJerkSmoother(const Param & smoother_param)
+: smoother_param_{smoother_param}
 {
-  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);
-
   qp_solver_.updateMaxIter(4000);
   qp_solver_.updateRhoInterval(0);  // 0 means automatic
   qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4

planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp

@@ -24,13 +24,9 @@
 
 namespace motion_velocity_smoother
 {
-LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node)
+LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(const Param & smoother_param)
+: smoother_param_(smoother_param)
 {
-  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);
-
   qp_solver_.updateMaxIter(20000);
   qp_solver_.updateRhoInterval(5000);
   qp_solver_.updateEpsRel(1.0e-4);  // def: 1.0e-4

planning/motion_velocity_smoother/src/smoother/smoother_base.cpp

@@ -23,29 +23,6 @@
 
 namespace motion_velocity_smoother
 {
-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.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);
-  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);
-  p.resample_param.sparse_min_interval_distance =
-    node.declare_parameter("sparse_min_interval_distance", 4.0);
-}
-
 void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }
 
 double SmootherBase::getMaxAccel() const { return base_param_.max_accel; }