feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): add processing time pub. (#9065)

* feat(costmap_generator, control_validator, scenario_selector, surround_obstacle_checker, vehicle_cmd_gate): Add: processing_time_pub

Signed-off-by: Kazunori-Nakajima <kazunori.nakajima@tier4.jp>

* fix: pre-commit

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* feat(costmap_generator): fix: No output when not Active.

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* fix: clang-format

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

* Re: fix: clang-format

Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

---------

Signed-off-by: Kazunori-Nakajima <kazunori.nakajima@tier4.jp>
Signed-off-by: Kasunori-Nakajima <kazunori.nakajima@tier4.jp>

Author: Kazunori-Nakajima

control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp

@@ -20,12 +20,14 @@
 #include "autoware_vehicle_info_utils/vehicle_info.hpp"
 #include "diagnostic_updater/diagnostic_updater.hpp"
 
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_control_validator/msg/control_validator_status.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_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 <cstdint>
 #include <memory>
@@ -143,6 +145,7 @@ class ControlValidator : public rclcpp::Node
   universe_utils::InterProcessPollingSubscriber<Trajectory>::SharedPtr sub_reference_traj_;
   rclcpp::Publisher<ControlValidatorStatus>::SharedPtr pub_status_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
 
   // system parameters
   int64_t diag_error_count_threshold_ = 0;
@@ -167,6 +170,8 @@ class ControlValidator : public rclcpp::Node
 
   Odometry::ConstSharedPtr current_kinematics_;
 
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+
   std::shared_ptr<ControlValidatorDebugMarkerPublisher> debug_pose_publisher_;
 };
 }  // namespace autoware::control_validator

control/autoware_control_validator/src/control_validator.cpp

@@ -47,6 +47,9 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options)
 
   pub_markers_ = create_publisher<visualization_msgs::msg::MarkerArray>("~/output/markers", 1);
 
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
+
   debug_pose_publisher_ = std::make_shared<ControlValidatorDebugMarkerPublisher>(this);
 
   setup_parameters();
@@ -134,6 +137,8 @@ bool ControlValidator::is_data_ready()
 
 void ControlValidator::on_predicted_trajectory(const Trajectory::ConstSharedPtr msg)
 {
+  stop_watch.tic();
+
   current_predicted_trajectory_ = msg;
   current_reference_trajectory_ = sub_reference_traj_->takeData();
   current_kinematics_ = sub_kinematics_->takeData();
@@ -162,6 +167,12 @@ void ControlValidator::publish_debug_info()
     debug_pose_publisher_->push_warning_msg(front_pose, "INVALID CONTROL");
   }
   debug_pose_publisher_->publish();
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  pub_processing_time_->publish(processing_time_msg);
 }
 
 void ControlValidator::validate(

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -77,6 +77,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   engage_pub_ = create_publisher<EngageMsg>("output/engage", durable_qos);
   pub_external_emergency_ = create_publisher<Emergency>("output/external_emergency", durable_qos);
   operation_mode_pub_ = create_publisher<OperationModeState>("output/operation_mode", durable_qos);
+  processing_time_pub_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
 
   is_filter_activated_pub_ =
     create_publisher<IsFilterActivated>("~/is_filter_activated", durable_qos);
@@ -375,6 +377,8 @@ T VehicleCmdGate::getContinuousTopic(
 
 void VehicleCmdGate::onTimer()
 {
+  stop_watch.tic();
+
   // Subscriber for auto
   const auto msg_auto_command_turn_indicator = auto_turn_indicator_cmd_sub_.takeData();
   if (msg_auto_command_turn_indicator)
@@ -573,12 +577,18 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   vehicle_cmd_emergency.emergency = (use_emergency_handling_ && is_system_emergency_);
   vehicle_cmd_emergency.stamp = filtered_control.stamp;
 
+  // ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_control);
   published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_control.stamp);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
+  processing_time_pub_->publish(processing_time_msg);
 
   // Save ControlCmd to steering angle when disengaged
   prev_control_cmd_ = filtered_control;
@@ -616,12 +626,18 @@ void VehicleCmdGate::publishEmergencyStopControlCommands()
   vehicle_cmd_emergency.stamp = stamp;
   vehicle_cmd_emergency.emergency = true;
 
+  // ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+
   // Publish topics
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(control_cmd);
   turn_indicator_cmd_pub_->publish(turn_indicator);
   hazard_light_cmd_pub_->publish(hazard_light);
   gear_cmd_pub_->publish(gear);
+  processing_time_pub_->publish(processing_time_msg);
 }
 
 void VehicleCmdGate::publishStatus()
@@ -638,12 +654,18 @@ void VehicleCmdGate::publishStatus()
   external_emergency.stamp = stamp;
   external_emergency.emergency = is_external_emergency_stop_;
 
+  // ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+
   gate_mode_pub_->publish(current_gate_mode_);
   engage_pub_->publish(autoware_engage);
   pub_external_emergency_->publish(external_emergency);
   operation_mode_pub_->publish(current_operation_mode_);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
+  processing_time_pub_->publish(processing_time_msg);
 }
 
 Control VehicleCmdGate::filterControlCommand(const Control & in)

control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -23,6 +23,7 @@
 #include <autoware/motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/ros/published_time_publisher.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware_vehicle_cmd_gate/msg/is_filter_activated.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
@@ -41,6 +42,7 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <tier4_control_msgs/msg/gate_mode.hpp>
 #include <tier4_debug_msgs/msg/bool_stamped.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_external_api_msgs/msg/emergency.hpp>
 #include <tier4_external_api_msgs/msg/heartbeat.hpp>
 #include <tier4_external_api_msgs/srv/engage.hpp>
@@ -114,6 +116,7 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_raw_pub_;
   rclcpp::Publisher<BoolStamped>::SharedPtr filter_activated_flag_pub_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_pub_;
   // Parameter callback
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
   rcl_interfaces::msg::SetParametersResult onParameter(
@@ -268,6 +271,9 @@ class VehicleCmdGate : public rclcpp::Node
   std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
   double stop_check_duration_;
 
+  // processing time
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+
   // debug
   MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
   void publishMarkers(const IsFilterActivated & filter_activated);

planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp

@@ -49,6 +49,8 @@
 #include "autoware_costmap_generator/points_to_costmap.hpp"
 #include "costmap_generator_node_parameters.hpp"
 
+#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/system/time_keeper.hpp>
 #include <autoware_lanelet2_extension/utility/message_conversion.hpp>
 #include <grid_map_ros/GridMapRosConverter.hpp>
@@ -58,6 +60,7 @@
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
 
 #include <grid_map_msgs/msg/grid_map.h>
@@ -91,6 +94,7 @@ class CostmapGenerator : public rclcpp::Node
   rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr pub_costmap_;
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_occupancy_grid_;
   rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr pub_processing_time_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_ms_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_points_;
   rclcpp::Subscription<autoware_perception_msgs::msg::PredictedObjects>::SharedPtr sub_objects_;
@@ -175,6 +179,9 @@ class CostmapGenerator : public rclcpp::Node
 
   /// \brief calculate cost for final output
   grid_map::Matrix generateCombinedCostmap();
+
+  /// \brief measure processing time
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 };
 }  // namespace autoware::costmap_generator
 

planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp

@@ -192,6 +192,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
   pub_processing_time_ =
     create_publisher<autoware::universe_utils::ProcessingTimeDetail>("processing_time", 1);
   time_keeper_ = std::make_shared<autoware::universe_utils::TimeKeeper>(pub_processing_time_);
+  pub_processing_time_ms_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
 
   // Timer
   const auto period_ns = rclcpp::Rate(param_->update_rate).period();
@@ -277,7 +279,13 @@ void CostmapGenerator::onScenario(const tier4_planning_msgs::msg::Scenario::Cons
 void CostmapGenerator::onTimer()
 {
   autoware::universe_utils::ScopedTimeTrack scoped_time_track(__func__, *time_keeper_);
+  stop_watch.tic();
   if (!isActive()) {
+    // Publish ProcessingTime
+    tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+    processing_time_msg.stamp = get_clock()->now();
+    processing_time_msg.data = stop_watch.toc();
+    pub_processing_time_ms_->publish(processing_time_msg);
     return;
   }
 
@@ -467,6 +475,12 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap)
   auto out_gridmap_msg = grid_map::GridMapRosConverter::toMessage(costmap);
   out_gridmap_msg->header = header;
   pub_costmap_->publish(*out_gridmap_msg);
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  pub_processing_time_ms_->publish(processing_time_msg);
 }
 }  // namespace autoware::costmap_generator
 

planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp

@@ -25,6 +25,7 @@
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
@@ -37,6 +38,7 @@
 #endif
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/polling_subscriber.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -95,6 +97,7 @@ class ScenarioSelectorNode : public rclcpp::Node
   rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr sub_parking_trajectory_;
   rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
   rclcpp::Publisher<tier4_planning_msgs::msg::Scenario>::SharedPtr pub_scenario_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
 
   // polling subscribers
   universe_utils::InterProcessPollingSubscriber<
@@ -130,6 +133,9 @@ class ScenarioSelectorNode : public rclcpp::Node
 
   static constexpr double lane_stopping_timeout_s = 5.0;
   static constexpr double empty_parking_trajectory_timeout_s = 3.0;
+
+  // processing time
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
 };
 }  // namespace autoware::scenario_selector
 #endif  // AUTOWARE__SCENARIO_SELECTOR__NODE_HPP_

planning/autoware_scenario_selector/src/node.cpp

@@ -330,6 +330,9 @@ bool ScenarioSelectorNode::isDataReady()
 
 void ScenarioSelectorNode::updateData()
 {
+  {
+    stop_watch.tic();
+  }
   {
     auto msg = sub_parking_state_->takeData();
     is_parking_completed_ = msg ? msg->data : is_parking_completed_;
@@ -370,6 +373,12 @@ void ScenarioSelectorNode::onTimer()
   }
 
   pub_scenario_->publish(scenario);
+
+  // Publish ProcessingTime
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  pub_processing_time_->publish(processing_time_msg);
 }
 
 void ScenarioSelectorNode::onLaneDrivingTrajectory(
@@ -466,6 +475,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti
     this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this));
   published_time_publisher_ =
     std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
 }
 }  // namespace autoware::scenario_selector
 

planning/autoware_surround_obstacle_checker/src/node.cpp

@@ -17,6 +17,7 @@
 #include <autoware/universe_utils/geometry/boost_polygon_utils.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/transform/transforms.hpp>
 
 #include <boost/assert.hpp>
@@ -109,6 +110,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
     "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local());
   pub_velocity_limit_ = this->create_publisher<VelocityLimit>(
     "~/output/max_velocity", rclcpp::QoS{1}.transient_local());
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
 
   using std::chrono_literals::operator""ms;
   timer_ = rclcpp::create_timer(
@@ -151,6 +154,9 @@ bool SurroundObstacleCheckerNode::getUseDynamicObject() const
 
 void SurroundObstacleCheckerNode::onTimer()
 {
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+  stop_watch.tic();
+
   odometry_ptr_ = sub_odometry_.takeData();
   pointcloud_ptr_ = sub_pointcloud_.takeData();
   object_ptr_ = sub_dynamic_objects_.takeData();
@@ -256,6 +262,11 @@ void SurroundObstacleCheckerNode::onTimer()
     no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose);
   }
 
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = stop_watch.toc();
+  pub_processing_time_->publish(processing_time_msg);
+
   pub_stop_reason_->publish(no_start_reason_diag);
   debug_ptr_->publish();
 }

planning/autoware_surround_obstacle_checker/src/node.hpp

@@ -29,6 +29,7 @@
 #include <diagnostic_msgs/msg/key_value.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
@@ -106,6 +107,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
   rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_stop_reason_;
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
 
   // parameter callback result
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;

system/autoware_processing_time_checker/config/processing_time_checker.param.yaml

@@ -2,9 +2,11 @@
   ros__parameters:
     update_rate: 10.0
     processing_time_topic_name_list:
+      - /control/control_validator/debug/processing_time_ms
       - /control/trajectory_follower/controller_node_exe/lateral/debug/processing_time_ms
       - /control/trajectory_follower/controller_node_exe/longitudinal/debug/processing_time_ms
       - /control/trajectory_follower/lane_departure_checker_node/debug/processing_time_ms
+      - /control/vehicle_cmd_gate/debug/processing_time_ms
       - /perception/object_recognition/prediction/map_based_prediction/debug/processing_time_ms
       - /perception/object_recognition/tracking/multi_object_tracker/debug/processing_time_ms
       - /planning/planning_validator/debug/processing_time_ms
@@ -33,5 +35,8 @@
       - /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/debug/processing_time_ms
       - /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/processing_time_ms
       - /planning/scenario_planning/lane_driving/motion_planning/path_optimizer/debug/processing_time_ms
+      - /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/processing_time_ms
+      - /planning/scenario_planning/parking/costmap_generator/debug/processing_time_ms
+      - /planning/scenario_planning/scenario_selector/debug/processing_time_ms
       - /planning/scenario_planning/velocity_smoother/debug/processing_time_ms
       - /simulation/shape_estimation/debug/processing_time_ms