feat(planning_evaluator): add processing time pub (#9334)

Signed-off-by: Kasunori-Nakajima <[email protected]>

evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp

@@ -23,6 +23,7 @@
 
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp"
@@ -31,6 +32,7 @@
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_metric_msgs/msg/metric.hpp>
 #include <tier4_metric_msgs/msg/metric_array.hpp>
 
@@ -142,6 +144,7 @@ class PlanningEvaluatorNode : public rclcpp::Node
   autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped> accel_sub_{
     this, "~/input/acceleration"};
 
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_pub_;
   rclcpp::Publisher<MetricArrayMsg>::SharedPtr metrics_pub_;
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;

evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp

@@ -53,6 +53,9 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op
   output_file_str_ = declare_parameter<std::string>("output_file");
   ego_frame_str_ = declare_parameter<std::string>("ego_frame");
 
+  processing_time_pub_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
+
   // List of metrics to calculate and publish
   metrics_pub_ = create_publisher<MetricArrayMsg>("~/metrics", 1);
   for (const std::string & selected_metric :
@@ -222,6 +225,8 @@ void PlanningEvaluatorNode::AddMetricMsg(const Metric & metric, const Stat<doubl
 
 void PlanningEvaluatorNode::onTimer()
 {
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+
   const auto ego_state_ptr = odometry_sub_.takeData();
   onOdometry(ego_state_ptr);
   {
@@ -247,6 +252,12 @@ void PlanningEvaluatorNode::onTimer()
   metrics_msg_.stamp = now();
   metrics_pub_->publish(metrics_msg_);
   metrics_msg_ = MetricArrayMsg{};
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  processing_time_pub_->publish(processing_time_msg);
 }
 
 void PlanningEvaluatorNode::onTrajectory(