feat(obstacle_stop_planner): output processing time (#5279)

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

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(