fix(motion_velocity_smoother): initialize params for dynamic parameter (

tier4#617) * fix(motion_velocity_smoother): initialize params for dynamic parameter settings Signed-off-by: Makoto Kurihara <[email protected]> * ci(pre-commit): autofix * Change parameter variables from member to local Signed-off-by: Makoto Kurihara <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
boyali · Sep 28, 2022 · bb15a25 · bb15a25
1 parent 7ac1b63
commit bb15a25
Show file tree

Hide file tree

Showing 12 changed files with 23 additions and 11 deletions.
diff --git a/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
@@ -124,12 +124,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node
     AlgorithmType algorithm_type;  // Option : JerkFiltered, Linf, L2
   } node_param_{};
 
-  SmootherBase::BaseParam base_param_{};
-  JerkFilteredSmoother::Param jerk_filtered_smoother_param_{};
-  L2PseudoJerkSmoother::Param l2_pseudo_jerk_smoother_param_{};
-  LinfPseudoJerkSmoother::Param linf_pseudo_jerk_smoother_param_{};
-  AnalyticalJerkConstrainedSmoother::Param analytical_jerk_constrained_smoother_param_{};
-
   std::shared_ptr<SmootherBase> smoother_;
 
   bool publish_debug_trajs_;  // publish planned trajectories

diff --git a/...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
@@ -78,6 +78,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     const TrajectoryPoints & input) const override;
 
   void setParam(const Param & param);
+  Param getParam() const;
 
 private:
   Param smoother_param_;

diff --git a/...on_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/...on_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
@@ -50,6 +50,7 @@ class JerkFilteredSmoother : public SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
 
   void setParam(const Param & param);
+  Param getParam() const;
 
 private:
   Param smoother_param_;

diff --git a/...n_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/...n_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
@@ -48,6 +48,7 @@ class L2PseudoJerkSmoother : public SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
 
   void setParam(const Param & smoother_param);
+  Param getParam() const;
 
 private:
   Param smoother_param_;

diff --git a/...velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/...velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
@@ -48,6 +48,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
     const TrajectoryPoints & input, const double v_current, const int closest_id) const override;
 
   void setParam(const Param & smoother_param);
+  Param getParam() const;
 
 private:
   Param smoother_param_;

diff --git a/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
@@ -68,6 +68,7 @@ class SmootherBase
   double getMinJerk() const;
 
   void setParam(const BaseParam & param);
+  BaseParam getBaseParam() const;
 
 protected:
   BaseParam base_param_;

diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
@@ -150,7 +150,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
   }
 
   {
-    auto & p = base_param_;
+    auto p = smoother_->getBaseParam();
     update_param("normal.max_acc", p.max_accel);
     update_param("normal.min_acc", p.min_decel);
     update_param("stop_decel", p.stop_decel);
@@ -172,7 +172,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
 
   switch (node_param_.algorithm_type) {
     case AlgorithmType::JERK_FILTERED: {
-      auto & p = jerk_filtered_smoother_param_;
+      auto p = std::dynamic_pointer_cast<JerkFilteredSmoother>(smoother_)->getParam();
       update_param("jerk_weight", p.jerk_weight);
       update_param("over_v_weight", p.over_v_weight);
       update_param("over_a_weight", p.over_a_weight);
@@ -182,23 +182,23 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
       break;
     }
     case AlgorithmType::L2: {
-      auto & p = l2_pseudo_jerk_smoother_param_;
+      auto p = std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->getParam();
       update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
       update_param("over_v_weight", p.over_v_weight);
       update_param("over_a_weight", p.over_a_weight);
       std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->setParam(p);
       break;
     }
     case AlgorithmType::LINF: {
-      auto & p = linf_pseudo_jerk_smoother_param_;
+      auto p = std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->getParam();
       update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
       update_param("over_v_weight", p.over_v_weight);
       update_param("over_a_weight", p.over_a_weight);
       std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->setParam(p);
       break;
     }
     case AlgorithmType::ANALYTICAL: {
-      auto & p = analytical_jerk_constrained_smoother_param_;
+      auto p = std::dynamic_pointer_cast<AnalyticalJerkConstrainedSmoother>(smoother_)->getParam();
       update_param("resample.delta_yaw_threshold", p.resample.delta_yaw_threshold);
       update_param(
         "latacc.constant_velocity_dist_threshold", p.latacc.constant_velocity_dist_threshold);

diff --git a/...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
@@ -91,6 +91,11 @@ void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param)
   smoother_param_ = smoother_param;
 }
 
+AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getParam() const
+{
+  return smoother_param_;
+}
+
 bool AnalyticalJerkConstrainedSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
   TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories)

diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
@@ -47,6 +47,8 @@ void JerkFilteredSmoother::setParam(const Param & smoother_param)
   smoother_param_ = smoother_param;
 }
 
+JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const { return smoother_param_; }
+
 bool JerkFilteredSmoother::apply(
   const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
   std::vector<TrajectoryPoints> & debug_trajectories)

diff --git a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
@@ -43,6 +43,8 @@ void L2PseudoJerkSmoother::setParam(const Param & smoother_param)
   smoother_param_ = smoother_param;
 }
 
+L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const { return smoother_param_; }
+
 bool L2PseudoJerkSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
   TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)

diff --git a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
@@ -43,6 +43,8 @@ void LinfPseudoJerkSmoother::setParam(const Param & smoother_param)
   smoother_param_ = smoother_param;
 }
 
+LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const { return smoother_param_; }
+
 bool LinfPseudoJerkSmoother::apply(
   const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
   TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories)

diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -48,6 +48,8 @@ SmootherBase::SmootherBase(rclcpp::Node & node)
 
 void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }
 
+SmootherBase::BaseParam SmootherBase::getBaseParam() const { return base_param_; }
+
 double SmootherBase::getMaxAccel() const { return base_param_.max_accel; }
 
 double SmootherBase::getMinDecel() const { return base_param_.min_decel; }