perf(velocity_smoother): not resample debug_trajectories is not used (autowarefoundation#8030)

* not resample debug_trajectories if not published

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update dependant packages

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,

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<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, [[maybe_unused]] const double v0,

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<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,

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<TrajectoryPoints> & debug_trajectories) override;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) override;
 
   TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,

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<TrajectoryPoints> & debug_trajectories) = 0;
+    TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+    const bool publish_debug_trajs) = 0;
 
   virtual TrajectoryPoints resampleTrajectory(
     const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose,

planning/autoware_velocity_smoother/src/node.cpp

@@ -676,7 +676,8 @@ bool VelocitySmootherNode::smoothVelocity(
 
   std::vector<TrajectoryPoints> 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.");
   }
 
@@ -716,15 +717,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);

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<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, [[maybe_unused]] std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   RCLCPP_DEBUG(logger_, "-------------------- Start --------------------");
 

planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -57,7 +57,7 @@ JerkFilteredSmoother::Param JerkFilteredSmoother::getParam() const
 
 bool JerkFilteredSmoother::apply(
   const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
-  std::vector<TrajectoryPoints> & debug_trajectories)
+  std::vector<TrajectoryPoints> & debug_trajectories, const bool publish_debug_trajs)
 {
   autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
 
@@ -114,10 +114,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;
@@ -127,9 +129,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;
   }
 

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<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   debug_trajectories.clear();
 

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<TrajectoryPoints> & debug_trajectories)
+  TrajectoryPoints & output, std::vector<TrajectoryPoints> & debug_trajectories,
+  [[maybe_unused]] const bool publish_debug_trajs)
 {
   debug_trajectories.clear();
 

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

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(