Skip to content

Commit

Permalink
refactor(motion_velocity_smoother): load smoother params from node in…
Browse files Browse the repository at this point in the history 
…stances (#609)

* Add constructor for setting params from node instance

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Change num_resample type from double to int

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Remove debug std outputs

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Shorten parameter variables

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Apply other smothers

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Change how to construct smoothers

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* ci(pre-commit): autofix

* Remove extra constructor

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* Refactor param variables

Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@mkuri @pre-commit-ci @taikitanaka3
2 people authored and taikitanaka3 committed Apr 11, 2022
1 parent e60ba18 commit 78e5a16
Show file tree
Hide file tree
Showing 12 changed files with 76 additions and 115 deletions.
10 changes: 0 additions & 10 deletions ...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::chrono::milliseconds> stop_watch_;
std::shared_ptr<rclcpp::Time> prev_time_;
Expand Down
4 changes: 2 additions & 2 deletions ...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
struct
{
double ds_resample;
double num_resample;
int num_resample;
double delta_yaw_threshold;
} resample;
struct
Expand All @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion ...on_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion ...n_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion ...velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
1 change: 1 addition & 0 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
95 changes: 4 additions & 91 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<JerkFilteredSmoother>(jerk_filtered_smoother_param_);
smoother_->setParam(base_param_);
smoother_ = std::make_shared<JerkFilteredSmoother>(*this);

// Set Publisher for jerk filtered algorithm
pub_forward_filtered_trajectory_ =
Expand All @@ -61,22 +58,15 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
break;
}
case AlgorithmType::L2: {
initL2PseudoJerkSmootherParam();
smoother_ = std::make_shared<L2PseudoJerkSmoother>(l2_pseudo_jerk_smoother_param_);
smoother_->setParam(base_param_);
smoother_ = std::make_shared<L2PseudoJerkSmoother>(*this);
break;
}
case AlgorithmType::LINF: {
initLinfPseudoJerkSmootherParam();
smoother_ = std::make_shared<LinfPseudoJerkSmoother>(linf_pseudo_jerk_smoother_param_);
smoother_->setParam(base_param_);
smoother_ = std::make_shared<LinfPseudoJerkSmoother>(*this);
break;
}
case AlgorithmType::ANALYTICAL: {
initAnalyticalJerkConstrainedSmootherParam();
smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(
analytical_jerk_constrained_smoother_param_);
smoother_->setParam(base_param_);
smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(*this);
break;
}
default:
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
26 changes: 23 additions & 3 deletions ...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -227,7 +247,7 @@ boost::optional<TrajectoryPoints> AnalyticalJerkConstrainedSmoother::resampleTra
continue;
}

for (size_t j = 0; j < smoother_param_.resample.num_resample; ++j) {
for (size_t j = 0; j < static_cast<size_t>(smoother_param_.resample.num_resample); ++j) {
auto tp = input.at(i);

tp.pose = lerpByPose(tp0.pose, tp1.pose, s);
Expand Down
10 changes: 8 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 6 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
8 changes: 6 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
23 changes: 23 additions & 0 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand Down

0 comments on commit 78e5a16

Please sign in to comment.