feat(motion_velocity_planner): publish processing times (#7633)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp

@@ -29,6 +29,7 @@
 #include <autoware/universe_utils/system/stop_watch.hpp>
 
 #include <algorithm>
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
@@ -48,6 +49,8 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms");
 
   using autoware::universe_utils::getOrDeclareParameter;
   auto & p = params_;
@@ -179,6 +182,12 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
   debug_data_.ego_footprints = ego_data.trajectory_footprints;
   debug_data_.obstacle_footprints = obstacle_forward_footprints;
   debug_data_.z = ego_data.pose.position.z;
+  std::map<std::string, double> processing_times;
+  processing_times["preprocessing"] = preprocessing_duration_us / 1000;
+  processing_times["footprints"] = footprints_duration_us / 1000;
+  processing_times["collisions"] = collisions_duration_us / 1000;
+  processing_times["Total"] = total_time_us / 1000;
+  processing_time_publisher_->publish(processing_times);
   return result;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp

@@ -34,6 +34,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <map>
 
 namespace autoware::motion_velocity_planner
 {
@@ -52,6 +53,8 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms");
 
   const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
   vehicle_lateral_offset_ = static_cast<double>(vehicle_info.max_lateral_offset_m);
@@ -218,6 +221,12 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan(
       safe_footprint_polygons, obstacle_masks,
       planner_data->current_odometry.pose.pose.position.z));
   }
+  std::map<std::string, double> processing_times;
+  processing_times["preprocessing"] = preprocessing_us / 1000;
+  processing_times["obstacles"] = obstacles_us / 1000;
+  processing_times["slowdowns"] = slowdowns_us / 1000;
+  processing_times["Total"] = total_us / 1000;
+  processing_time_publisher_->publish(processing_times);
   return result;
 }
 }  // namespace autoware::motion_velocity_planner

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -33,6 +33,7 @@
 
 #include <lanelet2_core/geometry/LaneletMap.h>
 
+#include <map>
 #include <memory>
 #include <string>
 #include <utility>
@@ -56,6 +57,8 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name)
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/debug_markers", 1);
   virtual_wall_publisher_ =
     node.create_publisher<visualization_msgs::msg::MarkerArray>("~/" + ns_ + "/virtual_walls", 1);
+  processing_time_publisher_ = std::make_shared<autoware::universe_utils::ProcessingTimePublisher>(
+    &node, "~/debug/" + ns_ + "/processing_time_ms");
 }
 void OutOfLaneModule::init_parameters(rclcpp::Node & node)
 {
@@ -207,7 +210,7 @@ VelocityPlanningResult OutOfLaneModule::plan(
   inputs.ego_data = ego_data;
   stopwatch.tic("filter_predicted_objects");
   inputs.objects = out_of_lane::filter_predicted_objects(planner_data, ego_data, params_);
-  const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects");
+  const auto filter_predicted_objects_us = stopwatch.toc("filter_predicted_objects");
   inputs.route_handler = planner_data->route_handler;
   inputs.lanelets = other_lanelets;
   stopwatch.tic("calculate_decisions");
@@ -288,12 +291,22 @@ VelocityPlanningResult OutOfLaneModule::plan(
     "\tcalc_slowdown_points = %2.0fus\n"
     "\tinsert_slowdown_points = %2.0fus\n",
     total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
-    calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us,
+    calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us,
     calc_slowdown_points_us, insert_slowdown_points_us);
   debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
   virtual_wall_marker_creator.add_virtual_walls(
     out_of_lane::debug::create_virtual_walls(debug_data_, params_));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
+  std::map<std::string, double> processing_times;
+  processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000;
+  processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000;
+  processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000;
+  processing_times["filter_pred_objects"] = filter_predicted_objects_us / 1000;
+  processing_times["calculate_decision"] = calculate_decisions_us / 1000;
+  processing_times["calc_slowdown_points"] = calc_slowdown_points_us / 1000;
+  processing_times["insert_slowdown_points"] = insert_slowdown_points_us / 1000;
+  processing_times["Total"] = total_time_us / 1000;
+  processing_time_publisher_->publish(processing_times);
   return result;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp

@@ -19,6 +19,7 @@
 #include "velocity_planning_result.hpp"
 
 #include <autoware/motion_utils/factor/velocity_factor_interface.hpp>
+#include <autoware/universe_utils/ros/processing_time_publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
@@ -44,6 +45,7 @@ class PluginModuleInterface
   rclcpp::Logger logger_ = rclcpp::get_logger("");
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_publisher_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_wall_publisher_;
+  std::shared_ptr<autoware::universe_utils::ProcessingTimePublisher> processing_time_publisher_;
   autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{};
 };
 

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp

@@ -17,6 +17,7 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/ros/wait_for_param.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <autoware/universe_utils/transform/transforms.hpp>
 #include <autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp>
 #include <autoware/velocity_smoother/trajectory_utils.hpp>
@@ -31,6 +32,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 
 #include <functional>
+#include <map>
 #include <memory>
 #include <vector>
 
@@ -252,9 +254,14 @@ void MotionVelocityPlannerNode::on_trajectory(
 {
   std::unique_lock<std::mutex> lk(mutex_);
 
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
+  std::map<std::string, double> processing_times;
+  stop_watch.tic("Total");
+
   if (!update_planner_data()) {
     return;
   }
+  processing_times["update_planner_data"] = stop_watch.toc(true);
 
   if (input_trajectory_msg->points.empty()) {
     RCLCPP_WARN(get_logger(), "Input trajectory message is empty");
@@ -264,19 +271,23 @@ void MotionVelocityPlannerNode::on_trajectory(
   if (has_received_map_) {
     planner_data_.route_handler = std::make_shared<route_handler::RouteHandler>(*map_ptr_);
     has_received_map_ = false;
+    processing_times["make_RouteHandler"] = stop_watch.toc(true);
   }
 
   autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points{
     input_trajectory_msg->points.begin(), input_trajectory_msg->points.end()};
 
   auto output_trajectory_msg = generate_trajectory(input_trajectory_points);
   output_trajectory_msg.header = input_trajectory_msg->header;
+  processing_times["generate_trajectory"] = stop_watch.toc(true);
 
   lk.unlock();
 
   trajectory_pub_->publish(output_trajectory_msg);
   published_time_publisher_->publish_if_subscribed(
     trajectory_pub_, output_trajectory_msg.header.stamp);
+  processing_times["Total"] = stop_watch.toc("Total");
+  processing_time_publisher_.publish(processing_times);
 }
 
 void MotionVelocityPlannerNode::insert_stop(

planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp

@@ -96,6 +96,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<autoware_adapi_v1_msgs::msg::VelocityFactorArray>::SharedPtr
     velocity_factor_publisher_;
+  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
 
   //  parameters
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr set_param_callback_;