From 0d504f79392e6d05be6caefc106f95173365fd73 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 13 Jul 2024 14:27:11 +0900 Subject: [PATCH] feat(velocity_smoother): add time_keeper (#8026) Signed-off-by: Takayuki Murooka --- .../autoware/velocity_smoother/node.hpp | 5 +++ .../analytical_jerk_constrained_smoother.hpp | 6 +++- .../smoother/jerk_filtered_smoother.hpp | 5 ++- .../smoother/l2_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/linf_pseudo_jerk_smoother.hpp | 5 ++- .../smoother/smoother_base.hpp | 6 +++- .../autoware_velocity_smoother/src/node.cpp | 35 ++++++++++++++++--- .../analytical_jerk_constrained_smoother.cpp | 5 +-- .../src/smoother/jerk_filtered_smoother.cpp | 20 ++++++++++- .../src/smoother/l2_pseudo_jerk_smoother.cpp | 4 ++- .../smoother/linf_pseudo_jerk_smoother.cpp | 4 ++- .../src/smoother/smoother_base.cpp | 8 ++++- 12 files changed, 93 insertions(+), 15 deletions(-) diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 0dd18615be5ef..26918cce24549 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/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::SharedPtr debug_closest_jerk_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_closest_max_velocity_; + rclcpp::Publisher::SharedPtr + debug_processing_time_detail_; // For Jerk Filtered Algorithm Debug rclcpp::Publisher::SharedPtr pub_forward_filtered_trajectory_; @@ -275,6 +278,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; + + mutable std::shared_ptr time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother 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 42d5a520c9e44..bbc3828bb1b15 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 @@ -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 #include #include #include @@ -65,7 +67,9 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase } backward; }; - explicit AnalyticalJerkConstrainedSmoother(rclcpp::Node & node); + explicit AnalyticalJerkConstrainedSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper = + std::make_shared()); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, 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 14d0d8ab7ff84..06a6f2da7f30a 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 @@ -17,6 +17,7 @@ #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" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include 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 time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, 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 1e2918d8e313b..a2a07ec6909aa 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 @@ -17,6 +17,7 @@ #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" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include 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 time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, 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 b88cd26eb4467..7c46a53431608 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 @@ -17,6 +17,7 @@ #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" @@ -24,6 +25,7 @@ #include "boost/optional.hpp" +#include #include 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 time_keeper); bool apply( const double initial_vel, const double initial_acc, const TrajectoryPoints & input, 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 be7baf694d361..6671eaa3eabe7 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 @@ -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 +#include #include 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 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 time_keeper_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index b881b68798cdc..ca7526e9adf3b 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/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("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( + "~/debug/processing_time_detail_ms", 1); + time_keeper_ = + std::make_shared(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(*this); + smoother_ = std::make_shared(*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(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::LINF: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*this, time_keeper_); break; } case AlgorithmType::ANALYTICAL: { - smoother_ = std::make_shared(*this); + smoother_ = std::make_shared(*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 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 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. 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 7f263fdea5e36..3906222454d35 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 @@ -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 time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.resample.ds_resample = node.declare_parameter("resample.ds_resample"); 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 cab8e49a3b45d..d458c688d060c 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/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 time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.jerk_weight = node.declare_parameter("jerk_weight"); @@ -59,6 +61,8 @@ bool JerkFilteredSmoother::apply( const double v0, const double a0, const TrajectoryPoints & input, TrajectoryPoints & output, std::vector & 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::max(), std::numeric_limits::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 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); 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 b489c994b0495..f379b217187c9 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 @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +L2PseudoJerkSmoother::L2PseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); 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 cb8451dba8302..2ab3d6dd80dfc 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 @@ -25,7 +25,9 @@ namespace autoware::velocity_smoother { -LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) +LinfPseudoJerkSmoother::LinfPseudoJerkSmoother( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: SmootherBase(node, time_keeper) { auto & p = smoother_param_; p.pseudo_jerk_weight = node.declare_parameter("pseudo_jerk_weight"); diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 700cf45b7eb9d..46faf10fe4a62 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -60,7 +60,9 @@ TrajectoryPoints applyPreProcess( } } // namespace -SmootherBase::SmootherBase(rclcpp::Node & node) +SmootherBase::SmootherBase( + rclcpp::Node & node, const std::shared_ptr time_keeper) +: time_keeper_(time_keeper) { auto & p = base_param_; p.max_accel = node.declare_parameter("normal.max_acc"); @@ -130,6 +132,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate lateral acc. do nothing. } @@ -198,6 +202,8 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( const TrajectoryPoints & input, const bool use_resampling, const double input_points_interval) const { + autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (input.size() < 3) { return input; // cannot calculate the desired velocity. do nothing. }