From 2b8d5fc654b777f33e5dbd211d48d32254b95985 Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 31 Mar 2022 17:02:06 +0900 Subject: [PATCH] refactor(motion_velocity_smoother): load smoother params from node instances (#609) * Add constructor for setting params from node instance Signed-off-by: Makoto Kurihara * Change num_resample type from double to int Signed-off-by: Makoto Kurihara * Remove debug std outputs Signed-off-by: Makoto Kurihara * Shorten parameter variables Signed-off-by: Makoto Kurihara * Apply other smothers Signed-off-by: Makoto Kurihara * Change how to construct smoothers Signed-off-by: Makoto Kurihara * ci(pre-commit): autofix * Remove extra constructor Signed-off-by: Makoto Kurihara * Refactor param variables Signed-off-by: Makoto Kurihara * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../motion_velocity_smoother_node.hpp | 10 -- .../analytical_jerk_constrained_smoother.hpp | 4 +- .../smoother/jerk_filtered_smoother.hpp | 2 +- .../smoother/l2_pseudo_jerk_smoother.hpp | 2 +- .../smoother/linf_pseudo_jerk_smoother.hpp | 2 +- .../smoother/smoother_base.hpp | 1 + .../src/motion_velocity_smoother_node.cpp | 95 +------------------ .../analytical_jerk_constrained_smoother.cpp | 26 ++++- .../src/smoother/jerk_filtered_smoother.cpp | 10 +- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 8 +- .../smoother/linf_pseudo_jerk_smoother.cpp | 8 +- .../src/smoother/smoother_base.cpp | 23 +++++ 12 files changed, 76 insertions(+), 115 deletions(-) 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..0d297403f3058 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, 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..29364319debbe 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, 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..7c338efbcd6c3 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, 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..602503cd7693f 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, 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..51cfa064049ea 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, 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..529447cb9b460 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,9 @@ 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); // Set Publisher for jerk filtered algorithm pub_forward_filtered_trajectory_ = @@ -61,22 +58,15 @@ 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); break; } case AlgorithmType::LINF: { - initLinfPseudoJerkSmootherParam(); - smoother_ = std::make_shared(linf_pseudo_jerk_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); break; } case AlgorithmType::ANALYTICAL: { - initAnalyticalJerkConstrainedSmootherParam(); - smoother_ = std::make_shared( - analytical_jerk_constrained_smoother_param_); - smoother_->setParam(base_param_); + smoother_ = std::make_shared(*this); break; } default: @@ -209,8 +199,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 +258,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..f2ccb13bd740e 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,10 +62,30 @@ 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; @@ -227,7 +247,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..5d5d8f79e1cb3 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 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..8dd5da0501e6f 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 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..7d4388bf0a926 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 diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 77b2da1d0e80f..5fa3e689e4b72 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -23,6 +23,29 @@ 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; }