feat(velocity_smoother): add constructor for setting params from node…

… instance Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Mar 30, 2022 · 9faeac5 · 9faeac5
1 parent ca59048
commit 9faeac5
Show file tree

Hide file tree

Showing 4 changed files with 58 additions and 0 deletions.
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
@@ -66,6 +66,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
   };
 
   explicit AnalyticalJerkConstrainedSmoother(const Param & p);
+  explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

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

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
@@ -66,6 +66,33 @@ AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(const Param
 : smoother_param_(smoother_param)
 {
 }
+
+AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node)
+: SmootherBase(node)
+{
+  smoother_param_.resample.ds_resample = node.declare_parameter("resample.ds_resample", 0.1);
+  smoother_param_.resample.num_resample = node.declare_parameter("resample.num_resample", 1.0);
+  smoother_param_.resample.delta_yaw_threshold =
+    node.declare_parameter("resample.delta_yaw_threshold", 0.785);
+  smoother_param_.latacc.enable_constant_velocity_while_turning =
+    node.declare_parameter("latacc.enable_constant_velocity_while_turning", false);
+  smoother_param_.latacc.constant_velocity_dist_threshold =
+    node.declare_parameter("latacc.constant_velocity_dist_threshold", 2.0);
+  smoother_param_.forward.max_acc = node.declare_parameter("forward.max_acc", 1.0);
+  smoother_param_.forward.min_acc = node.declare_parameter("forward.min_acc", -1.0);
+  smoother_param_.forward.max_jerk = node.declare_parameter("forward.max_jerk", 0.3);
+  smoother_param_.forward.min_jerk = node.declare_parameter("forward.min_jerk", -0.3);
+  smoother_param_.forward.kp = node.declare_parameter("forward.kp", 0.3);
+  smoother_param_.backward.start_jerk = node.declare_parameter("backward.start_jerk", -0.1);
+  smoother_param_.backward.min_jerk_mild_stop =
+    node.declare_parameter("backward.min_jerk_mild_stop", -0.3);
+  smoother_param_.backward.min_jerk = node.declare_parameter("backward.min_jerk", -1.5);
+  smoother_param_.backward.min_acc_mild_stop =
+    node.declare_parameter("backward.min_acc_mild_stop", -1.0);
+  smoother_param_.backward.min_acc = node.declare_parameter("backward.min_acc", -2.5);
+  smoother_param_.backward.span_jerk = node.declare_parameter("backward.span_jerk", -0.01);
+}
+
 void AnalyticalJerkConstrainedSmoother::setParam(const Param & smoother_param)
 {
   smoother_param_ = smoother_param;

diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
@@ -23,6 +23,34 @@
 
 namespace motion_velocity_smoother
 {
+SmootherBase::SmootherBase(rclcpp::Node & node)
+{
+  std::cout << "SmootherBase constructor!" << std::endl;
+  base_param_.max_accel = node.declare_parameter("normal.max_acc", 2.0);
+  std::cout << "max_accel: " << base_param_.max_accel << std::endl;
+  base_param_.min_decel = node.declare_parameter("normal.min_acc", -3.0);
+  base_param_.stop_decel = node.declare_parameter("stop_decel", 0.0);
+  base_param_.max_jerk = node.declare_parameter("normal.max_jerk", 0.3);
+  base_param_.min_jerk = node.declare_parameter("normal.min_jerk", -0.1);
+  base_param_.max_lateral_accel = node.declare_parameter("max_lateral_accel", 0.2);
+  base_param_.decel_distance_before_curve =
+    node.declare_parameter("decel_distance_before_curve", 3.5);
+  base_param_.decel_distance_after_curve =
+    node.declare_parameter("decel_distance_after_curve", 0.0);
+  base_param_.min_curve_velocity = node.declare_parameter("min_curve_velocity", 1.38);
+  base_param_.resample_param.max_trajectory_length =
+    node.declare_parameter("max_trajectory_length", 200.0);
+  base_param_.resample_param.min_trajectory_length =
+    node.declare_parameter("min_trajectory_length", 30.0);
+  base_param_.resample_param.resample_time = node.declare_parameter("resample_time", 10.0);
+  base_param_.resample_param.dense_resample_dt = node.declare_parameter("dense_resample_dt", 0.1);
+  base_param_.resample_param.dense_min_interval_distance =
+    node.declare_parameter("dense_min_interval_distance", 0.1);
+  base_param_.resample_param.sparse_resample_dt = node.declare_parameter("sparse_resample_dt", 0.5);
+  base_param_.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; }