feat(planning_validator): output processing time (#5276)

Signed-off-by: satoshi-ota <[email protected]>

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();