diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 7c8c3f660bf07..d2dbf69ec398b 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -194,16 +194,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node // parameter handling void initCommonParam(); - void initSmootherBaseParam(); - - void initJerkFilteredSmootherParam(); - - void initL2PseudoJerkSmootherParam(); - - void initLinfPseudoJerkSmootherParam(); - - void initAnalyticalJerkConstrainedSmootherParam(); - // debug tier4_autoware_utils::StopWatch stop_watch_; std::shared_ptr prev_time_; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index cf583639bfc6c..f3ff57eb68434 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/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; - double num_resample; + int num_resample; double delta_yaw_threshold; } resample; struct @@ -65,7 +65,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } backward; }; - explicit AnalyticalJerkConstrainedSmoother(const Param & p); + explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -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/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 0cec057d172b3..9246c198befd9 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/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(const Param & p); + explicit JerkFilteredSmoother(rclcpp::Node & node); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -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/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 8088d0cbfb1f6..a5159ff42efce 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/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(const Param & smoother_param); + explicit L2PseudoJerkSmoother(rclcpp::Node & node); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -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/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 2036050b59657..0604d913ebfc3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/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(const Param & smoother_param); + explicit LinfPseudoJerkSmoother(rclcpp::Node & node); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, @@ -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/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 75b86d5a51397..cb07212afe04d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -50,6 +50,7 @@ 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, @@ -67,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 index 5f1c27539bcc1..d9ad2a9f007ca 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -42,12 +42,11 @@ 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: { - initJerkFilteredSmootherParam(); - smoother_ = std::make_shared(jerk_filtered_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); + jerk_filtered_smoother_param_ = + std::dynamic_pointer_cast(smoother_)->getParam(); // Set Publisher for jerk filtered algorithm pub_forward_filtered_trajectory_ = @@ -61,27 +60,27 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions break; } case AlgorithmType::L2: { - initL2PseudoJerkSmootherParam(); - smoother_ = std::make_shared(l2_pseudo_jerk_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); + l2_pseudo_jerk_smoother_param_ = + std::dynamic_pointer_cast(smoother_)->getParam(); break; } case AlgorithmType::LINF: { - initLinfPseudoJerkSmootherParam(); - smoother_ = std::make_shared(linf_pseudo_jerk_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); + linf_pseudo_jerk_smoother_param_ = + std::dynamic_pointer_cast(smoother_)->getParam(); break; } case AlgorithmType::ANALYTICAL: { - initAnalyticalJerkConstrainedSmootherParam(); - smoother_ = std::make_shared( - analytical_jerk_constrained_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); + analytical_jerk_constrained_smoother_param_ = + std::dynamic_pointer_cast(smoother_)->getParam(); break; } default: throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm"); } + base_param_ = smoother_->getBaseParam(); // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); @@ -209,8 +208,6 @@ 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); @@ -270,81 +267,6 @@ 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); diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 5c49a88986b39..e862f699c47ef 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -62,15 +62,40 @@ bool applyMaxVelocity( namespace motion_velocity_smoother { -AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(const Param & smoother_param) -: smoother_param_(smoother_param) +AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) +: 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.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; } +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 & debug_trajectories) @@ -227,7 +252,7 @@ boost::optional AnalyticalJerkConstrainedSmoother::resampleTra continue; } - for (size_t j = 0; j < smoother_param_.resample.num_resample; ++j) { + for (size_t j = 0; j < static_cast(smoother_param_.resample.num_resample); ++j) { auto tp = input.at(i); tp.pose = lerpByPose(tp0.pose, tp1.pose, s); diff --git a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index 9ab84b5ecf951..64e2965e888cd 100644 --- a/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -26,9 +26,15 @@ namespace motion_velocity_smoother { -JerkFilteredSmoother::JerkFilteredSmoother(const Param & smoother_param) -: smoother_param_{smoother_param} +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); + qp_solver_.updateMaxIter(20000); qp_solver_.updateRhoInterval(0); // 0 means automatic qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 @@ -41,6 +47,11 @@ 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 & 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 index 2f4cf253b4cc7..47bdc023391b3 100644 --- a/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -24,9 +24,13 @@ namespace motion_velocity_smoother { -L2PseudoJerkSmoother::L2PseudoJerkSmoother(const Param & smoother_param) -: smoother_param_{smoother_param} +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); + qp_solver_.updateMaxIter(4000); qp_solver_.updateRhoInterval(0); // 0 means automatic qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 @@ -39,6 +43,11 @@ 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 & 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 index fceb9432787f7..c22773ef33faa 100644 --- a/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -24,9 +24,13 @@ namespace motion_velocity_smoother { -LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(const Param & smoother_param) -: smoother_param_(smoother_param) +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); + qp_solver_.updateMaxIter(20000); qp_solver_.updateRhoInterval(5000); qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 @@ -39,6 +43,11 @@ 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 & debug_trajectories) diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 77b2da1d0e80f..2ecf683fd3028 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -23,8 +23,33 @@ 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; } +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; }