feat(lane_departure_checker): add processing time publisher (Float64)

Signed-off-by: Maxime CLEMENT <[email protected]>
autowarefoundation · Jun 25, 2024 · 64eb90f · 64eb90f
1 parent fb898eb
commit 64eb90f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 2 deletions.
diff --git a/...departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/...departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp
@@ -100,7 +100,9 @@ class LaneDepartureCheckerNode : public rclcpp::Node
 
   // Publisher
   autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"};
-  autoware::universe_utils::ProcessingTimePublisher processing_time_publisher_{this};
+  autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{
+    this, "~/debug/processing_time_ms_diag"};
+  rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr processing_time_publisher_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;

diff --git a/...re_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/...re_lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp
@@ -170,6 +170,8 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o
   lane_departure_checker_->setParam(param_, vehicle_info);
 
   // Publisher
+  processing_time_publisher_ =
+    this->create_publisher<tier4_debug_msgs::msg::Float64Stamped>("~/debug/processing_time_ms", 1);
   // Nothing
 
   // Diagnostic Updater
@@ -347,7 +349,11 @@ void LaneDepartureCheckerNode::onTimer()
   }
 
   processing_time_map["Total"] = stop_watch.toc("Total");
-  processing_time_publisher_.publish(processing_time_map);
+  processing_diag_publisher_.publish(processing_time_map);
+  tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
+  processing_time_msg.stamp = get_clock()->now();
+  processing_time_msg.data = processing_time_map["Total"];
+  processing_time_publisher_->publish(processing_time_msg);
 }
 
 rcl_interfaces::msg::SetParametersResult LaneDepartureCheckerNode::onParameter(