fix(dynamic_obstacle_stop): publish processing time when early return (…

…autowarefoundation#10254) Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
tier4 · Mar 11, 2025 · c31c0ee · c31c0ee
1 parent 1b37970
commit c31c0ee
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 8 deletions.
diff --git a/...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp
@@ -32,7 +32,6 @@
 #include <map>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace autoware::motion_velocity_planner
@@ -97,6 +96,14 @@ void DynamicObstacleStopModule::update_parameters(const std::vector<rclcpp::Para
   update_param(parameters, ns_ + ".ignore_unavoidable_collisions", p.ignore_unavoidable_collisions);
 }
 
+void DynamicObstacleStopModule::publish_processing_time(const double processing_time_ms)
+{
+  autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = clock_->now();
+  processing_time_msg.data = processing_time_ms;
+  processing_time_publisher_->publish(processing_time_msg);
+}
+
 VelocityPlanningResult DynamicObstacleStopModule::plan(
   [[maybe_unused]] const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> &
     raw_trajectory_points,
@@ -105,9 +112,12 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
 {
   VelocityPlanningResult result;
   debug_data_.reset_data();
-  if (smoothed_trajectory_points.size() < 2) return result;
-
   autoware_utils::StopWatch<std::chrono::microseconds> stopwatch;
+  if (smoothed_trajectory_points.size() < 2) {
+    publish_processing_time(stopwatch.toc() / 1000);
+    return result;
+  }
+
   stopwatch.tic();
   stopwatch.tic("preprocessing");
   dynamic_obstacle_stop::EgoData ego_data;
@@ -191,10 +201,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan(
   processing_times["collisions"] = collisions_duration_us / 1000;
   processing_times["Total"] = total_time_us / 1000;
   processing_diag_publisher_->publish(processing_times);
-  autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg;
-  processing_time_msg.stamp = clock_->now();
-  processing_time_msg.data = processing_times["Total"];
-  processing_time_publisher_->publish(processing_time_msg);
+  publish_processing_time(processing_times["Total"]);
   return result;
 }
 

diff --git a/...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/...utoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp
@@ -35,6 +35,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface
   void init(rclcpp::Node & node, const std::string & module_name) override;
   void publish_planning_factor() override { planning_factor_interface_->publish(); };
   void update_parameters(const std::vector<rclcpp::Parameter> & parameters) override;
+  void publish_processing_time(const double processing_time_ms);
   VelocityPlanningResult plan(
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & raw_trajectory_points,
     const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & smoothed_trajectory_points,
@@ -47,7 +48,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface
 
   inline static const std::string ns_ = "dynamic_obstacle_stop";
   std::string module_name_;
-  rclcpp::Clock::SharedPtr clock_{};
+  rclcpp::Clock::SharedPtr clock_;
 
   dynamic_obstacle_stop::PlannerParam params_;
   dynamic_obstacle_stop::ObjectStopDecisionMap object_map_;