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..9af72bbcc63c2 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 @@ -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/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..d8f471d1e5a1d 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,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/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..ee2802c19aa21 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 @@ -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 index 77b2da1d0e80f..360e26adfa3cf 100644 --- 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; }