feat(velocity_smoother): add time_keeper (autowarefoundation#8026)

Signed-off-by: Takayuki Murooka <[email protected]>

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp

@@ -23,6 +23,7 @@
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
 #include "autoware/universe_utils/ros/self_pose_listener.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/resample.hpp"
 #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
 #include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
@@ -260,6 +261,8 @@ class VelocitySmootherNode : public rclcpp::Node
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_jerk_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_max_velocity_;
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
+    debug_processing_time_detail_;
 
   // For Jerk Filtered Algorithm Debug
   rclcpp::Publisher<Trajectory>::SharedPtr pub_forward_filtered_trajectory_;
@@ -275,6 +278,8 @@ class VelocitySmootherNode : public rclcpp::Node
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
+
+  mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
 };
 }  // namespace autoware::velocity_smoother
 

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_  // NOLINT
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -24,6 +25,7 @@
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
@@ -65,7 +67,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
     } backward;
   };
 
-  explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node);
+  explicit AnalyticalJerkConstrainedSmoother(
+    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper =
+                           std::make_shared<autoware::universe_utils::TimeKeeper>());
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp

@@ -17,13 +17,15 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
 #include "boost/optional.hpp"
 
+#include <memory>
 #include <vector>
 
 namespace autoware::velocity_smoother
@@ -40,7 +42,8 @@ class JerkFilteredSmoother : public SmootherBase
     double jerk_filter_ds;
   };
 
-  explicit JerkFilteredSmoother(rclcpp::Node & node);
+  explicit JerkFilteredSmoother(
+    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp

@@ -17,13 +17,15 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
 #include "boost/optional.hpp"
 
+#include <memory>
 #include <vector>
 
 namespace autoware::velocity_smoother
@@ -38,7 +40,8 @@ class L2PseudoJerkSmoother : public SmootherBase
     double over_a_weight;
   };
 
-  explicit L2PseudoJerkSmoother(rclcpp::Node & node);
+  explicit L2PseudoJerkSmoother(
+    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp

@@ -17,13 +17,15 @@
 
 #include "autoware/motion_utils/trajectory/trajectory.hpp"
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/smoother/smoother_base.hpp"
 #include "osqp_interface/osqp_interface.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
 #include "boost/optional.hpp"
 
+#include <memory>
 #include <vector>
 
 namespace autoware::velocity_smoother
@@ -38,7 +40,8 @@ class LinfPseudoJerkSmoother : public SmootherBase
     double over_a_weight;
   };
 
-  explicit LinfPseudoJerkSmoother(rclcpp::Node & node);
+  explicit LinfPseudoJerkSmoother(
+    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
 
   bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,

planning/autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp

@@ -15,12 +15,14 @@
 #ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 #define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
 
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware/velocity_smoother/resample.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "autoware_planning_msgs/msg/trajectory_point.hpp"
 
 #include <limits>
+#include <memory>
 #include <vector>
 
 namespace autoware::velocity_smoother
@@ -54,7 +56,8 @@ class SmootherBase
     resampling::ResampleParam resample_param;
   };
 
-  explicit SmootherBase(rclcpp::Node & node);
+  explicit SmootherBase(
+    rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
   virtual ~SmootherBase() = default;
   virtual bool apply(
     const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
@@ -85,6 +88,7 @@ class SmootherBase
 
 protected:
   BaseParam base_param_;
+  mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
 };
 }  // namespace autoware::velocity_smoother
 

planning/autoware_velocity_smoother/src/node.cpp

@@ -46,6 +46,13 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti
   initCommonParam();
   over_stop_velocity_warn_thr_ = declare_parameter<double>("over_stop_velocity_warn_thr");
 
+  // create time_keeper and its publisher
+  // NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother.
+  debug_processing_time_detail_ = create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+    "~/debug/processing_time_detail_ms", 1);
+  time_keeper_ =
+    std::make_shared<autoware::universe_utils::TimeKeeper>(debug_processing_time_detail_);
+
   // create smoother
   setupSmoother(wheelbase_);
 
@@ -99,7 +106,7 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase)
 {
   switch (node_param_.algorithm_type) {
     case AlgorithmType::JERK_FILTERED: {
-      smoother_ = std::make_shared<JerkFilteredSmoother>(*this);
+      smoother_ = std::make_shared<JerkFilteredSmoother>(*this, time_keeper_);
 
       // Set Publisher for jerk filtered algorithm
       pub_forward_filtered_trajectory_ =
@@ -113,15 +120,15 @@ void VelocitySmootherNode::setupSmoother(const double wheelbase)
       break;
     }
     case AlgorithmType::L2: {
-      smoother_ = std::make_shared<L2PseudoJerkSmoother>(*this);
+      smoother_ = std::make_shared<L2PseudoJerkSmoother>(*this, time_keeper_);
       break;
     }
     case AlgorithmType::LINF: {
-      smoother_ = std::make_shared<LinfPseudoJerkSmoother>(*this);
+      smoother_ = std::make_shared<LinfPseudoJerkSmoother>(*this, time_keeper_);
       break;
     }
     case AlgorithmType::ANALYTICAL: {
-      smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(*this);
+      smoother_ = std::make_shared<AnalyticalJerkConstrainedSmoother>(*this, time_keeper_);
       break;
     }
     default:
@@ -309,6 +316,8 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory
 
 void VelocitySmootherNode::calcExternalVelocityLimit()
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (!external_velocity_limit_ptr_) {
     return;
   }
@@ -414,6 +423,8 @@ bool VelocitySmootherNode::checkData() const
 
 void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg)
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   RCLCPP_DEBUG(get_logger(), "========================= run start =========================");
   stop_watch_.tic();
 
@@ -502,6 +513,8 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr
 
 void VelocitySmootherNode::updateDataForExternalVelocityLimit()
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (prev_output_.empty()) {
     return;
   }
@@ -519,6 +532,8 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit()
 TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity(
   const TrajectoryPoints & traj_input) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   TrajectoryPoints output{};  // velocity is optimized by qp solver
 
   // Extract trajectory around self-position with desired forward-backward length
@@ -566,6 +581,8 @@ bool VelocitySmootherNode::smoothVelocity(
   const TrajectoryPoints & input, const size_t input_closest,
   TrajectoryPoints & traj_smoothed) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (input.empty()) {
     return false;  // cannot apply smoothing
   }
@@ -672,6 +689,8 @@ bool VelocitySmootherNode::smoothVelocity(
 void VelocitySmootherNode::insertBehindVelocity(
   const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const bool keep_closest_vel_for_behind =
     (type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN ||
      type == InitializeType::ENGAGING);
@@ -734,6 +753,8 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto
 std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::calcInitialMotion(
   const TrajectoryPoints & input_traj, const size_t input_closest) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x);
   const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x;
   const double target_vel = std::fabs(input_traj.at(input_closest).longitudinal_velocity_mps);
@@ -817,6 +838,8 @@ std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::ca
 void VelocitySmootherNode::overwriteStopPoint(
   const TrajectoryPoints & input, TrajectoryPoints & output) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input);
   if (!stop_idx) {
     return;
@@ -863,6 +886,8 @@ void VelocitySmootherNode::overwriteStopPoint(
 
 void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   if (traj.size() < 1) {
     return;
   }
@@ -902,6 +927,8 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c
 
 void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj);
   if (!stop_idx) {
     return;  // no stop point.

planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp

@@ -66,8 +66,9 @@ bool applyMaxVelocity(
 
 namespace autoware::velocity_smoother
 {
-AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node)
-: SmootherBase(node)
+AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(
+  rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper)
+: SmootherBase(node, time_keeper)
 {
   auto & p = smoother_param_;
   p.resample.ds_resample = node.declare_parameter<double>("resample.ds_resample");

planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp

@@ -29,7 +29,9 @@
 
 namespace autoware::velocity_smoother
 {
-JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node)
+JerkFilteredSmoother::JerkFilteredSmoother(
+  rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper)
+: SmootherBase(node, time_keeper)
 {
   auto & p = smoother_param_;
   p.jerk_weight = node.declare_parameter<double>("jerk_weight");
@@ -59,6 +61,8 @@ bool JerkFilteredSmoother::apply(
   const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output,
   std::vector<TrajectoryPoints> & debug_trajectories)
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   output = input;
 
   if (input.empty()) {
@@ -102,6 +106,8 @@ bool JerkFilteredSmoother::apply(
   const auto initial_traj_pose = filtered.front().pose;
 
   const auto resample = [&](const auto & trajectory) {
+    autoware::universe_utils::ScopedTimeTrack st("resample", *time_keeper_);
+
     return resampling::resampleTrajectory(
       trajectory, v0, initial_traj_pose, std::numeric_limits<double>::max(),
       std::numeric_limits<double>::max(), base_param_.resample_param);
@@ -152,6 +158,7 @@ bool JerkFilteredSmoother::apply(
     v_max_arr.at(i) = opt_resampled_trajectory.at(i).longitudinal_velocity_mps;
   }
 
+  time_keeper_->start_track("initOptimization");
   /*
    * x = [
    *      b[0], b[1], ..., b[N],               : 0~N
@@ -290,9 +297,12 @@ bool JerkFilteredSmoother::apply(
     lower_bound[constr_idx] = a0;
     ++constr_idx;
   }
+  time_keeper_->end_track("initOptimization");
 
   // execute optimization
+  time_keeper_->start_track("optimize");
   const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound);
+  time_keeper_->end_track("optimize");
   const std::vector<double> optval = std::get<0>(result);
   const int status_val = std::get<3>(result);
   if (status_val != 1) {
@@ -356,6 +366,8 @@ TrajectoryPoints JerkFilteredSmoother::forwardJerkFilter(
   const double v0, const double a0, const double a_max, const double a_start, const double j_max,
   const TrajectoryPoints & input) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   auto applyLimits = [&input, &a_start](double & v, double & a, size_t i) {
     double v_lim = input.at(i).longitudinal_velocity_mps;
     static constexpr double ep = 1.0e-5;
@@ -408,6 +420,8 @@ TrajectoryPoints JerkFilteredSmoother::backwardJerkFilter(
   const double v0, const double a0, const double a_min, const double a_stop, const double j_min,
   const TrajectoryPoints & input) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   auto input_rev = input;
   std::reverse(input_rev.begin(), input_rev.end());
   auto filtered = forwardJerkFilter(
@@ -423,6 +437,8 @@ TrajectoryPoints JerkFilteredSmoother::mergeFilteredTrajectory(
   const double v0, const double a0, const double a_min, const double j_min,
   const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   TrajectoryPoints merged;
   merged = forward_filtered;
 
@@ -475,6 +491,8 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory(
   const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold,
   const double nearest_yaw_threshold) const
 {
+  autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+
   return resampling::resampleTrajectory(
     input, current_pose, nearest_dist_threshold, nearest_yaw_threshold, base_param_.resample_param,
     smoother_param_.jerk_filter_ds);