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): initialize params for dynamic parameter #617

Merged
merged 3 commits into from
Apr 1, 2022
Merged

fix(motion_velocity_smoother): initialize params for dynamic parameter #617

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
dbaa668
fix(motion_velocity_smoother): initialize params for dynamic parameter
mkuri Apr 1, 2022
8cb92b6
ci(pre-commit): autofix
pre-commit-ci[bot] Apr 1, 2022
4332a5f
Change parameter variables from member to local
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
6 changes: 0 additions & 6 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 @@ -124,12 +124,6 @@ class MotionVelocitySmootherNode : public rclcpp::Node
AlgorithmType algorithm_type; // Option : JerkFiltered, Linf, L2
} node_param_{};

SmootherBase::BaseParam base_param_{};
JerkFilteredSmoother::Param jerk_filtered_smoother_param_{};
L2PseudoJerkSmoother::Param l2_pseudo_jerk_smoother_param_{};
LinfPseudoJerkSmoother::Param linf_pseudo_jerk_smoother_param_{};
AnalyticalJerkConstrainedSmoother::Param analytical_jerk_constrained_smoother_param_{};

std::shared_ptr<SmootherBase> smoother_;

bool publish_debug_trajs_; // publish planned trajectories
Expand Down
1 change: 1 addition & 0 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 @@ -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
1 change: 1 addition & 0 deletions ...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 @@ -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
1 change: 1 addition & 0 deletions ...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 @@ -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
1 change: 1 addition & 0 deletions ...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 @@ -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
1 change: 1 addition & 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 @@ -68,6 +68,7 @@ class SmootherBase
double getMinJerk() const;

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

protected:
BaseParam base_param_;
Expand Down
10 changes: 5 additions & 5 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 @@ -150,7 +150,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
}

{
auto & p = base_param_;
auto p = smoother_->getBaseParam();
update_param("normal.max_acc", p.max_accel);
update_param("normal.min_acc", p.min_decel);
update_param("stop_decel", p.stop_decel);
Expand All @@ -172,7 +172,7 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter

switch (node_param_.algorithm_type) {
case AlgorithmType::JERK_FILTERED: {
auto & p = jerk_filtered_smoother_param_;
auto p = std::dynamic_pointer_cast<JerkFilteredSmoother>(smoother_)->getParam();
update_param("jerk_weight", p.jerk_weight);
update_param("over_v_weight", p.over_v_weight);
update_param("over_a_weight", p.over_a_weight);
Expand All @@ -182,23 +182,23 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter
break;
}
case AlgorithmType::L2: {
auto & p = l2_pseudo_jerk_smoother_param_;
auto p = std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->getParam();
update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
update_param("over_v_weight", p.over_v_weight);
update_param("over_a_weight", p.over_a_weight);
std::dynamic_pointer_cast<L2PseudoJerkSmoother>(smoother_)->setParam(p);
break;
}
case AlgorithmType::LINF: {
auto & p = linf_pseudo_jerk_smoother_param_;
auto p = std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->getParam();
update_param("pseudo_jerk_weight", p.pseudo_jerk_weight);
update_param("over_v_weight", p.over_v_weight);
update_param("over_a_weight", p.over_a_weight);
std::dynamic_pointer_cast<LinfPseudoJerkSmoother>(smoother_)->setParam(p);
break;
}
case AlgorithmType::ANALYTICAL: {
auto & p = analytical_jerk_constrained_smoother_param_;
auto p = std::dynamic_pointer_cast<AnalyticalJerkConstrainedSmoother>(smoother_)->getParam();
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
5 changes: 5 additions & 0 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 @@ -91,6 +91,11 @@ 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
2 changes: 2 additions & 0 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 @@ -47,6 +47,8 @@ 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
2 changes: 2 additions & 0 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 @@ -43,6 +43,8 @@ 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
2 changes: 2 additions & 0 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 @@ -43,6 +43,8 @@ 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
2 changes: 2 additions & 0 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ SmootherBase::SmootherBase(rclcpp::Node & node)

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; }
Expand Down