From 246b625dd0dba4399ffdd45288ca47450cc1ce9e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 03:11:09 +0900 Subject: [PATCH 1/2] not resample debug_trajectories if not published Signed-off-by: Takayuki Murooka --- .../analytical_jerk_constrained_smoother.hpp | 3 ++- .../smoother/jerk_filtered_smoother.hpp | 3 ++- .../smoother/l2_pseudo_jerk_smoother.hpp | 3 ++- .../smoother/linf_pseudo_jerk_smoother.hpp | 3 ++- .../smoother/smoother_base.hpp | 3 ++- .../autoware_velocity_smoother/src/node.cpp | 19 ++++++++--------- .../analytical_jerk_constrained_smoother.cpp | 3 ++- .../src/smoother/jerk_filtered_smoother.cpp | 21 ++++++++++++------- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 3 ++- .../smoother/linf_pseudo_jerk_smoother.cpp | 3 ++- 10 files changed, 38 insertions(+), 26 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index bbc3828bb1b15..bfcd7db6b9c43 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -73,7 +73,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp index 06a6f2da7f30a..850e98f525ba0 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -47,7 +47,8 @@ class JerkFilteredSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, [[maybe_unused]] const double v0, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index a2a07ec6909aa..8f6bbc2236eb6 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -45,7 +45,8 @@ class L2PseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index 7c46a53431608..e27a4db10e748 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -45,7 +45,8 @@ class LinfPseudoJerkSmoother : public SmootherBase bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) override; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) override; TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp index 6671eaa3eabe7..7206a64ea32eb 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp @@ -61,7 +61,8 @@ class SmootherBase virtual ~SmootherBase() = default; virtual bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) = 0; + TrajectoryPoints & output, std::vector & debug_trajectories, + const bool publish_debug_trajs) = 0; virtual TrajectoryPoints resampleTrajectory( const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index ca7526e9adf3b..600abb19c89dd 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -629,7 +629,8 @@ bool VelocitySmootherNode::smoothVelocity( std::vector debug_trajectories; if (!smoother_->apply( - initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories)) { + initial_motion.vel, initial_motion.acc, clipped, traj_smoothed, debug_trajectories, + publish_debug_trajs_)) { RCLCPP_WARN(get_logger(), "Fail to solve optimization."); } @@ -669,15 +670,13 @@ bool VelocitySmootherNode::smoothVelocity( pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } - if (!debug_trajectories.empty()) { - for (auto & debug_trajectory : debug_trajectories) { - debug_trajectory.insert( - debug_trajectory.begin(), traj_resampled.begin(), - traj_resampled.begin() + traj_resampled_closest); - for (size_t i = 0; i < traj_resampled_closest; ++i) { - debug_trajectory.at(i).longitudinal_velocity_mps = - debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; - } + for (auto & debug_trajectory : debug_trajectories) { + debug_trajectory.insert( + debug_trajectory.begin(), traj_resampled.begin(), + traj_resampled.begin() + traj_resampled_closest); + for (size_t i = 0; i < traj_resampled_closest; ++i) { + debug_trajectory.at(i).longitudinal_velocity_mps = + debug_trajectory.at(traj_resampled_closest).longitudinal_velocity_mps; } } publishDebugTrajectories(debug_trajectories); diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 3906222454d35..43b03748133e6 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -103,7 +103,8 @@ AnalyticalJerkConstrainedSmoother::Param AnalyticalJerkConstrainedSmoother::getP bool AnalyticalJerkConstrainedSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories) + TrajectoryPoints & output, [[maybe_unused]] std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { RCLCPP_DEBUG(logger_, "-------------------- Start --------------------"); diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d458c688d060c..b49ba4bde50f2 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -59,7 +59,7 @@ JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, - std::vector & debug_trajectories) + std::vector & debug_trajectories, const bool publish_debug_trajs) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -116,10 +116,12 @@ bool JerkFilteredSmoother::apply( auto opt_resampled_trajectory = resample(filtered); // Set debug trajectories - debug_trajectories.resize(3); - debug_trajectories[0] = resample(forward_filtered); - debug_trajectories[1] = resample(backward_filtered); - debug_trajectories[2] = resample(filtered); + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = resample(forward_filtered); + debug_trajectories[1] = resample(backward_filtered); + debug_trajectories[2] = resample(filtered); + } // Ensure terminal velocity is zero opt_resampled_trajectory.back().longitudinal_velocity_mps = 0.0; @@ -129,9 +131,12 @@ bool JerkFilteredSmoother::apply( // No need to do optimization output.front().longitudinal_velocity_mps = v0; output.front().acceleration_mps2 = a0; - debug_trajectories[0] = output; - debug_trajectories[1] = output; - debug_trajectories[2] = output; + if (publish_debug_trajs) { + debug_trajectories.resize(3); + debug_trajectories[0] = output; + debug_trajectories[1] = output; + debug_trajectories[2] = output; + } return true; } diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index f379b217187c9..678c8e01bf67e 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -53,7 +53,8 @@ L2PseudoJerkSmoother::Param L2PseudoJerkSmoother::getParam() const bool L2PseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index 2ab3d6dd80dfc..60345ff0fa6f4 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -53,7 +53,8 @@ LinfPseudoJerkSmoother::Param LinfPseudoJerkSmoother::getParam() const bool LinfPseudoJerkSmoother::apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, - TrajectoryPoints & output, std::vector & debug_trajectories) + TrajectoryPoints & output, std::vector & debug_trajectories, + [[maybe_unused]] const bool publish_debug_trajs) { debug_trajectories.clear(); From c644ec3ad6a1f6988ffbfa58722080a35a63981f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 30 Jul 2024 17:59:04 +0900 Subject: [PATCH 2/2] update dependant packages Signed-off-by: Takayuki Murooka --- .../src/utilization/trajectory_utils.cpp | 2 +- .../autoware_motion_velocity_planner_node/src/node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 5900bd3531266..6815ca3ff8cb4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -76,7 +76,7 @@ bool smoothPath( TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; return false; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index f181b0c76b51e..bd9022c0ec0d2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -361,7 +361,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s autoware::motion_velocity_planner::TrajectoryPoints traj_smoothed; clipped.insert( clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } traj_smoothed.insert(