feat(mission_planner): add processing time publisher (#9342)

* feat(mission_planner): add processing time publisher Signed-off-by: Kasunori-Nakajima <[email protected]> * delete extra line Signed-off-by: Kasunori-Nakajima <[email protected]> * update: mission_planner, route_selector, service_utils. Signed-off-by: Kasunori-Nakajima <[email protected]> * Revert "update: mission_planner, route_selector, service_utils." This reverts commit d460a63. Signed-off-by: Kasunori-Nakajima <[email protected]> * Update to show that exceptions are not handled Signed-off-by: Kasunori-Nakajima <[email protected]> * feat(mission_planner,route_selector): add processing time publisher Signed-off-by: Kasunori-Nakajima <[email protected]> --------- Signed-off-by: Kasunori-Nakajima <[email protected]> Co-authored-by: Takagi, Isamu <[email protected]> Co-authored-by: Kosuke Takeuchi <[email protected]>
autowarefoundation · Nov 25, 2024 · b782772 · b782772
1 parent 135de57
commit b782772
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 0 deletions.
diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp
@@ -83,6 +83,17 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options)
   data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); });
 
   logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
+}
+
+void MissionPlanner::publish_processing_time(
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch)
+{
+  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 MissionPlanner::publish_pose_log(const Pose & pose, const std::string & pose_type)

diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp
@@ -21,6 +21,7 @@
 #include <autoware/mission_planner/mission_planner_plugin.hpp>
 #include <autoware/route_handler/route_handler.hpp>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <pluginlib/class_loader.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -29,6 +30,7 @@
 #include <autoware_adapi_v1_msgs/srv/set_route_points.hpp>
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/reroute_availability.hpp>
 #include <tier4_planning_msgs/msg/route_state.hpp>
 #include <tier4_planning_msgs/srv/clear_route.hpp>
@@ -68,6 +70,8 @@ class MissionPlanner : public rclcpp::Node
 {
 public:
   explicit MissionPlanner(const rclcpp::NodeOptions & options);
+  void publish_processing_time(
+    autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch);
 
 private:
   ArrivalChecker arrival_checker_;
@@ -141,6 +145,7 @@ class MissionPlanner : public rclcpp::Node
   bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route);
 
   std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
 };
 
 }  // namespace autoware::mission_planner

diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp b/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp
@@ -136,6 +136,19 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options)
   initialized_ = false;
   mrm_operating_ = false;
   main_request_ = std::monostate{};
+
+  // Processing time
+  pub_processing_time_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
+}
+
+void RouteSelector::publish_processing_time(
+  autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch)
+{
+  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 RouteSelector::on_state(const RouteState::ConstSharedPtr msg)

diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp b/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp
@@ -15,9 +15,11 @@
 #ifndef MISSION_PLANNER__ROUTE_SELECTOR_HPP_
 #define MISSION_PLANNER__ROUTE_SELECTOR_HPP_
 
+#include <autoware/universe_utils/system/stop_watch.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <tier4_debug_msgs/msg/float64_stamped.hpp>
 #include <tier4_planning_msgs/msg/route_state.hpp>
 #include <tier4_planning_msgs/srv/clear_route.hpp>
 #include <tier4_planning_msgs/srv/set_lanelet_route.hpp>
@@ -64,6 +66,8 @@ class RouteSelector : public rclcpp::Node
 {
 public:
   explicit RouteSelector(const rclcpp::NodeOptions & options);
+  void publish_processing_time(
+    autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch);
 
 private:
   using WaypointRequest = SetWaypointRoute::Request::SharedPtr;
@@ -78,6 +82,7 @@ class RouteSelector : public rclcpp::Node
   rclcpp::Client<SetLaneletRoute>::SharedPtr cli_set_lanelet_route_;
   rclcpp::Subscription<RouteState>::SharedPtr sub_state_;
   rclcpp::Subscription<LaneletRoute>::SharedPtr sub_route_;
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;
 
   bool initialized_;
   bool mrm_operating_;

diff --git a/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp b/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp
@@ -15,6 +15,8 @@
 #ifndef MISSION_PLANNER__SERVICE_UTILS_HPP_
 #define MISSION_PLANNER__SERVICE_UTILS_HPP_
 
+#include <autoware/universe_utils/system/stop_watch.hpp>
+
 #include <autoware_common_msgs/msg/response_status.hpp>
 
 #include <stdexcept>
@@ -57,11 +59,13 @@ template <class T, class Req, class Res>
 std::function<void(Req, Res)> handle_exception(void (T::*callback)(Req, Res), T * instance)
 {
   return [instance, callback](Req req, Res res) {
+    autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch;
     try {
       (instance->*callback)(req, res);
     } catch (const ServiceException & error) {
       error.set(res->status);
     }
+    instance->publish_processing_time(stop_watch);
   };
 }