From 29279a0f736c7c6c02cccd2c782a5a1b792fb0b2 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Thu, 12 Oct 2023 12:41:28 +0900
Subject: [PATCH] feat(planning_validator): output processing time (#5276)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../planning_validator/planning_validator.hpp        |  8 ++++++++
 .../planning_validator/src/planning_validator.cpp    | 12 ++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp
index 3b09ebe51ff6b..ef570b57773bb 100644
--- a/planning/planning_validator/include/planning_validator/planning_validator.hpp
+++ b/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
 
diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp
index 13a63a7e7d4ed..875781d47b25d 100644
--- a/planning/planning_validator/src/planning_validator.cpp
+++ b/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();