Merge pull request autowarefoundation#937 from tier4/cp-processing-time

refactor(planning_modules): publish processing time topics

planning/behavior_path_planner/src/planner_manager.cpp

@@ -35,7 +35,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose)
   verbose_{verbose}
 {
   processing_time_.emplace("total_time", 0.0);
-  debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "behavior_planner_manager/debug");
+  debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
 }
 
 BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & data)
@@ -894,7 +894,7 @@ void PlannerManager::print() const
 void PlannerManager::publishProcessingTime() const
 {
   for (const auto & t : processing_time_) {
-    std::string name = std::string("processing_time/") + t.first;
+    std::string name = t.first + std::string("/processing_time_ms");
     debug_publisher_ptr_->publish<DebugDoubleMsg>(name, t.second);
   }
 }

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -79,7 +79,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   debug_closest_acc_ = create_publisher<Float32Stamped>("~/closest_acceleration", 1);
   debug_closest_jerk_ = create_publisher<Float32Stamped>("~/closest_jerk", 1);
   debug_closest_max_velocity_ = create_publisher<Float32Stamped>("~/closest_max_velocity", 1);
-  debug_calculation_time_ = create_publisher<Float32Stamped>("~/calculation_time", 1);
+  debug_calculation_time_ = create_publisher<Float32Stamped>("~/debug/processing_time_ms", 1);
   pub_trajectory_raw_ = create_publisher<Trajectory>("~/debug/trajectory_raw", 1);
   pub_trajectory_vel_lim_ =
     create_publisher<Trajectory>("~/debug/trajectory_external_velocity_limited", 1);

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp

@@ -45,7 +45,11 @@ struct PlannerData
 
 struct TimeKeeper
 {
-  void init() { accumulated_msg = "\n"; }
+  void init()
+  {
+    accumulated_msg = "\n";
+    accumulated_time = 0.0;
+  }
 
   template <typename T>
   TimeKeeper & operator<<(const T & msg)
@@ -71,6 +75,7 @@ struct TimeKeeper
   {
     const double elapsed_time = stop_watch_.toc(func_name);
     *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]";
+    accumulated_time = elapsed_time;
     endLine();
   }
 
@@ -80,6 +85,10 @@ struct TimeKeeper
   std::string accumulated_msg = "\n";
   std::stringstream latest_stream;
 
+  double getAccumulatedTime() const { return accumulated_time; }
+
+  double accumulated_time{0.0};
+
   tier4_autoware_utils::StopWatch<
     std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
     stop_watch_;

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp

@@ -90,7 +90,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node
   // debug publisher
   rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_markers_pub_;
-  rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_pub_;
+  rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_str_pub_;
+  rclcpp::Publisher<Float64Stamped>::SharedPtr debug_calculation_time_float_pub_;
 
   // parameter callback
   rcl_interfaces::msg::SetParametersResult onParam(

planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp

@@ -24,6 +24,7 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_msgs/msg/header.hpp"
+#include "tier4_debug_msgs/msg/float64_stamped.hpp"
 #include "tier4_debug_msgs/msg/string_stamped.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 
@@ -42,6 +43,7 @@ using nav_msgs::msg::Odometry;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 // debug
+using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_debug_msgs::msg::StringStamped;
 }  // namespace obstacle_avoidance_planner
 

planning/obstacle_avoidance_planner/src/node.cpp

@@ -45,6 +45,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string &
   return msg;
 }
 
+Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data)
+{
+  Float64Stamped msg;
+  msg.stamp = now;
+  msg.data = data;
+  return msg;
+}
+
 void setZeroVelocityAfterStopPoint(std::vector<TrajectoryPoint> & traj_points)
 {
   const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points);
@@ -92,7 +100,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n
   // debug publisher
   debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
   debug_markers_pub_ = create_publisher<MarkerArray>("~/debug/marker", 1);
-  debug_calculation_time_pub_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  debug_calculation_time_str_pub_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  debug_calculation_time_float_pub_ =
+    create_publisher<Float64Stamped>("~/debug/processing_time_ms", 1);
 
   {  // parameters
     // parameter for option
@@ -253,7 +263,9 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr)
   // publish calculation_time
   // NOTE: This function must be called after measuring onPath calculation time
   const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog());
-  debug_calculation_time_pub_->publish(calculation_time_msg);
+  debug_calculation_time_str_pub_->publish(calculation_time_msg);
+  debug_calculation_time_float_pub_->publish(
+    createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime()));
 
   const auto output_traj_msg =
     trajectory_utils::createTrajectory(path_ptr->header, full_traj_points);

planning/obstacle_cruise_planner/src/node.cpp

@@ -374,7 +374,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions &
     "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local());
 
   // debug publisher
-  debug_calculation_time_pub_ = create_publisher<Float32Stamped>("~/debug/calculation_time", 1);
+  debug_calculation_time_pub_ = create_publisher<Float32Stamped>("~/debug/processing_time_ms", 1);
   debug_cruise_wall_marker_pub_ = create_publisher<MarkerArray>("~/debug/cruise/virtual_wall", 1);
   debug_stop_wall_marker_pub_ = create_publisher<MarkerArray>("~/virtual_wall", 1);
   debug_slow_down_wall_marker_pub_ =

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

@@ -19,6 +19,7 @@
 #include "obstacle_stop_planner/debug_marker.hpp"
 #include "obstacle_stop_planner/planner_data.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <motion_utils/trajectory/tmp_conversion.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
@@ -37,6 +38,7 @@
 #include <tier4_debug_msgs/msg/bool_stamped.hpp>
 #include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/expand_stop_range.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
@@ -78,9 +80,11 @@ using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
+using tier4_autoware_utils::StopWatch;
 using tier4_debug_msgs::msg::BoolStamped;
 using tier4_debug_msgs::msg::Float32MultiArrayStamped;
 using tier4_debug_msgs::msg::Float32Stamped;
+using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_planning_msgs::msg::ExpandStopRange;
 using tier4_planning_msgs::msg::VelocityLimit;
 using tier4_planning_msgs::msg::VelocityLimitClearCommand;
@@ -143,6 +147,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_collision_pointcloud_debug_;
 
+  rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_ms_;
+
   std::unique_ptr<AdaptiveCruiseController> acc_controller_;
   std::shared_ptr<ObstacleStopPlannerDebugNode> debug_ptr_;
   boost::optional<SlowDownSection> latest_slow_down_section_{boost::none};
@@ -166,6 +172,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node
   StopParam stop_param_;
   SlowDownParam slow_down_param_;
 
+  StopWatch<std::chrono::milliseconds> stop_watch_;
+
   // mutex for vehicle_info_, stop_param_, current_acc_, obstacle_ros_pointcloud_ptr_
   // NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used
   //   (current_velocity_ptr_)

planning/obstacle_stop_planner/src/node.cpp

@@ -209,6 +209,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod
   pub_collision_pointcloud_debug_ =
     this->create_publisher<PointCloud2>("~/debug/collision_pointcloud", 1);
 
+  pub_processing_time_ms_ = this->create_publisher<Float64Stamped>("~/debug/processing_time_ms", 1);
+
   // Subscribers
   if (!node_param_.use_predicted_objects) {
     // No need to point cloud while using predicted objects
@@ -274,6 +276,8 @@ void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr inp
 
 void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg)
 {
+  stop_watch_.tic(__func__);
+
   mutex_.lock();
   // NOTE: these variables must not be referenced for multithreading
   const auto vehicle_info = vehicle_info_;
@@ -376,6 +380,11 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m
 
   trajectory.header = input_msg->header;
   pub_trajectory_->publish(trajectory);
+
+  Float64Stamped processing_time_ms;
+  processing_time_ms.stamp = now();
+  processing_time_ms.data = stop_watch_.toc(__func__);
+  pub_processing_time_ms_->publish(processing_time_ms);
 }
 
 void ObstacleStopPlannerNode::searchObstacle(

planning/path_smoother/include/path_smoother/common_structs.hpp

@@ -44,7 +44,11 @@ struct PlannerData
 
 struct TimeKeeper
 {
-  void init() { accumulated_msg = "\n"; }
+  void init()
+  {
+    accumulated_msg = "\n";
+    accumulated_time = 0.0;
+  }
 
   template <typename T>
   TimeKeeper & operator<<(const T & msg)
@@ -66,6 +70,7 @@ struct TimeKeeper
   {
     const double elapsed_time = stop_watch_.toc(func_name);
     *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]";
+    accumulated_time = elapsed_time;
     endLine();
   }
 
@@ -74,6 +79,10 @@ struct TimeKeeper
   std::string accumulated_msg = "\n";
   std::stringstream latest_stream;
 
+  double getAccumulatedTime() const { return accumulated_time; }
+
+  double accumulated_time{0.0};
+
   tier4_autoware_utils::StopWatch<
     std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock>
     stop_watch_;

planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp

@@ -82,7 +82,8 @@ class ElasticBandSmoother : public rclcpp::Node
 
   // debug publisher
   rclcpp::Publisher<Trajectory>::SharedPtr debug_extended_traj_pub_;
-  rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_pub_;
+  rclcpp::Publisher<StringStamped>::SharedPtr debug_calculation_time_str_pub_;
+  rclcpp::Publisher<Float64Stamped>::SharedPtr debug_calculation_time_float_pub_;
 
   // parameter callback
   rcl_interfaces::msg::SetParametersResult onParam(

planning/path_smoother/include/path_smoother/type_alias.hpp

@@ -22,6 +22,7 @@
 #include "geometry_msgs/msg/point.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_msgs/msg/header.hpp"
+#include "tier4_debug_msgs/msg/float64_stamped.hpp"
 #include "tier4_debug_msgs/msg/string_stamped.hpp"
 
 namespace path_smoother
@@ -36,6 +37,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 // navigation
 using nav_msgs::msg::Odometry;
 // debug
+using tier4_debug_msgs::msg::Float64Stamped;
 using tier4_debug_msgs::msg::StringStamped;
 }  // namespace path_smoother
 

planning/path_smoother/src/elastic_band_smoother.cpp

@@ -43,6 +43,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string &
   return msg;
 }
 
+Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data)
+{
+  Float64Stamped msg;
+  msg.stamp = now;
+  msg.data = data;
+  return msg;
+}
+
 void setZeroVelocityAfterStopPoint(std::vector<TrajectoryPoint> & traj_points)
 {
   const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points);
@@ -75,7 +83,9 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option
 
   // debug publisher
   debug_extended_traj_pub_ = create_publisher<Trajectory>("~/debug/extended_traj", 1);
-  debug_calculation_time_pub_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  debug_calculation_time_str_pub_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  debug_calculation_time_float_pub_ =
+    create_publisher<Float64Stamped>("~/debug/processing_time_ms", 1);
 
   {  // parameters
     // parameters for ego nearest search
@@ -205,7 +215,9 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr)
   // publish calculation_time
   // NOTE: This function must be called after measuring onPath calculation time
   const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog());
-  debug_calculation_time_pub_->publish(calculation_time_msg);
+  debug_calculation_time_str_pub_->publish(calculation_time_msg);
+  debug_calculation_time_float_pub_->publish(
+    createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime()));
 
   const auto output_traj_msg =
     trajectory_utils::createTrajectory(path_ptr->header, full_traj_points);

planning/planning_validator/include/planning_validator/planning_validator.hpp

@@ -18,6 +18,7 @@
 #include "planning_validator/debug_marker.hpp"
 #include "planning_validator/msg/planning_validator_status.hpp"
 #include "tier4_autoware_utils/ros/logger_level_configure.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
@@ -26,6 +27,7 @@
 #include <autoware_auto_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 
 #include <memory>
 #include <string>
@@ -38,6 +40,8 @@ using diagnostic_updater::DiagnosticStatusWrapper;
 using diagnostic_updater::Updater;
 using nav_msgs::msg::Odometry;
 using planning_validator::msg::PlanningValidatorStatus;
+using tier4_autoware_utils::StopWatch;
+using tier4_debug_msgs::msg::Float64Stamped;
 
 struct ValidationParams
 {
@@ -81,6 +85,7 @@ class PlanningValidator : public rclcpp::Node
 
   void validate(const Trajectory & trajectory);
 
+  void publishProcessingTime(const double processing_time_ms);
   void publishTrajectory();
   void publishDebugInfo();
   void displayStatus();
@@ -91,6 +96,7 @@ class PlanningValidator : public rclcpp::Node
   rclcpp::Subscription<Trajectory>::SharedPtr sub_traj_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_traj_;
   rclcpp::Publisher<PlanningValidatorStatus>::SharedPtr pub_status_;
+  rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_ms_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
 
   // system parameters
@@ -120,6 +126,8 @@ class PlanningValidator : public rclcpp::Node
   std::shared_ptr<PlanningValidatorDebugMarkerPublisher> debug_pose_publisher_;
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
+
+  StopWatch<std::chrono::milliseconds> stop_watch_;
 };
 }  // namespace planning_validator
 

planning/planning_validator/src/planning_validator.cpp

@@ -41,6 +41,7 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options)
   pub_traj_ = create_publisher<Trajectory>("~/output/trajectory", 1);
   pub_status_ = create_publisher<PlanningValidatorStatus>("~/output/validation_status", 1);
   pub_markers_ = create_publisher<visualization_msgs::msg::MarkerArray>("~/output/markers", 1);
+  pub_processing_time_ms_ = create_publisher<Float64Stamped>("~/debug/processing_time_ms", 1);
 
   debug_pose_publisher_ = std::make_shared<PlanningValidatorDebugMarkerPublisher>(this);
 
@@ -167,6 +168,8 @@ bool PlanningValidator::isDataReady()
 
 void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg)
 {
+  stop_watch_.tic(__func__);
+
   current_trajectory_ = msg;
 
   if (!isDataReady()) return;
@@ -180,6 +183,7 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg)
   publishTrajectory();
 
   // for debug
+  publishProcessingTime(stop_watch_.toc(__func__));
   publishDebugInfo();
   displayStatus();
 }
@@ -222,6 +226,14 @@ void PlanningValidator::publishTrajectory()
   return;
 }
 
+void PlanningValidator::publishProcessingTime(const double processing_time_ms)
+{
+  Float64Stamped msg{};
+  msg.stamp = this->now();
+  msg.data = processing_time_ms;
+  pub_processing_time_ms_->publish(msg);
+}
+
 void PlanningValidator::publishDebugInfo()
 {
   validation_status_.stamp = get_clock()->now();