Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(motion_velocity_smoother): update params for dynamic parameter setting #616

Closed
wants to merge 11 commits into from
Closed

fix(motion_velocity_smoother): update params for dynamic parameter setting #616

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
bb1d669
Add constructor for setting params from node instance
mkuri Mar 24, 2022
9fdca95
Change num_resample type from double to int
mkuri Mar 30, 2022
c320afc
Remove debug std outputs
mkuri Mar 30, 2022
d3d6db4
Shorten parameter variables
mkuri Mar 30, 2022
9b0382f
Apply other smothers
mkuri Mar 30, 2022
dbef9d1
Change how to construct smoothers
mkuri Mar 30, 2022
d528ee3
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 30, 2022
61022b9
Remove extra constructor
mkuri Mar 31, 2022
b0b8042
Refactor param variables
mkuri Mar 31, 2022
b76dd81
ci(pre-commit): autofix
pre-commit-ci[bot] Mar 31, 2022
c797ad2
Update node member params for dynamic parameter settings
mkuri Apr 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 0 additions & 10 deletions ...tion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp
View file
Open in desktop
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
5 changes: 3 additions & 2 deletions ...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
View file
Open in desktop
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 All @@ -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_;
Expand Down
3 changes: 2 additions & 1 deletion ...on_velocity_smoother/include/motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp
View file
Open in desktop
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 All @@ -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_;
Expand Down
3 changes: 2 additions & 1 deletion ...n_velocity_smoother/include/motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
View file
Open in desktop
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 All @@ -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_;
Expand Down
3 changes: 2 additions & 1 deletion ...velocity_smoother/include/motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
View file
Open in desktop
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 All @@ -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_;
Expand Down
2 changes: 2 additions & 0 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Open in desktop
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 All @@ -67,6 +68,7 @@ class SmootherBase
double getMinJerk() const;

void setParam(const BaseParam & param);
BaseParam getBaseParam() const;

protected:
BaseParam base_param_;
Expand Down
104 changes: 13 additions & 91 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<JerkFilteredSmoother>(jerk_filtered_smoother_param_);
smoother_->setParam(base_param_);
smoother_ = std::make_shared<JerkFilteredSmoother>(*this);
jerk_filtered_smoother_param_ =
std::dynamic_pointer_cast<JerkFilteredSmoother>(smoother_)->getParam();

// Set Publisher for jerk filtered algorithm
pub_forward_filtered_trajectory_ =
Expand All @@ -61,27 +60,27 @@ 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);
l2_pseudo_jerk_smoother_param_ =
std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->getParam();
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);
linf_pseudo_jerk_smoother_param_ =
std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->getParam();
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);
analytical_jerk_constrained_smoother_param_ =
std::dynamic_pointer_cast<AnalyticalJerkConstrainedSmoother>(smoother_)->getParam();
break;
}
default:
throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
}
base_param_ = smoother_->getBaseParam();

// publishers, subscribers
pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
31 changes: 28 additions & 3 deletions ...rc/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<TrajectoryPoints> & debug_trajectories)
Expand Down Expand Up @@ -227,7 +252,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
15 changes: 13 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp
View file
Open in desktop
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 All @@ -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<TrajectoryPoints> & debug_trajectories)
Expand Down
13 changes: 11 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp
View file
Open in desktop
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 All @@ -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<TrajectoryPoints> & debug_trajectories)
Expand Down
13 changes: 11 additions & 2 deletions planning/motion_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp
View file
Open in desktop
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 All @@ -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<TrajectoryPoints> & debug_trajectories)
Expand Down
Loading