feat: add published_time publisher debug to packages

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -28,6 +28,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
   void publishProcessingTime(
     const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
   StopWatch<std::chrono::milliseconds> stop_watch_;

control/trajectory_follower_node/src/controller_node.cpp

@@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
   }
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
 Controller::LateralControllerMode Controller::getLateralControllerMode(
@@ -231,7 +233,10 @@ void Controller::callbackTimerControl()
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
 
-  // 6. publish debug marker
+  // 6. publish published time only if there are subscribers more than 1
+  published_time_publisher_->publish(control_cmd_pub_, out.stamp);
+
+  // 7. publish debug marker
   publishDebugMarker(*input_data, lat_out);
 }