From a514ee41106cb1257dd17029dfc7ee010e7c59df Mon Sep 17 00:00:00 2001
From: Berkay Karaman <brkay54@gmail.com>
Date: Tue, 12 Mar 2024 23:01:41 +0300
Subject: [PATCH] feat(tier4_autoware_utils): add published time debug class
 into utils (#6440)

Signed-off-by: Berkay Karaman <brkay54@gmail.com>
---
 build_depends.repos                           |   4 +
 .../ros/published_time_publisher.hpp          | 111 ++++++++++++++++++
 common/tier4_autoware_utils/package.xml       |   1 +
 3 files changed, 116 insertions(+)
 create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp

diff --git a/build_depends.repos b/build_depends.repos
index 1193d710b91f4..f7b3f12484015 100644
--- a/build_depends.repos
+++ b/build_depends.repos
@@ -16,6 +16,10 @@ repositories:
     type: git
     url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
     version: main
+  core/autoware_internal_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_internal_msgs.git
+    version: main
   core/external/autoware_auto_msgs:
     type: git
     url: https://github.com/tier4/autoware_auto_msgs.git
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp
new file mode 100644
index 0000000000000..2b6c2997f2caf
--- /dev/null
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp
@@ -0,0 +1,111 @@
+// Copyright 2024 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_internal_msgs/msg/published_time.hpp>
+#include <std_msgs/msg/header.hpp>
+
+#include <functional>
+#include <map>
+#include <string>
+
+namespace tier4_autoware_utils
+{
+
+class PublishedTimePublisher
+{
+public:
+  explicit PublishedTimePublisher(
+    rclcpp::Node * node, const std::string & publisher_topic_suffix = "/debug/published_time",
+    const rclcpp::QoS & qos = rclcpp::QoS(1))
+  : node_(node), publisher_topic_suffix_(publisher_topic_suffix), qos_(qos)
+  {
+  }
+
+  void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
+  {
+    const auto & gid_key = publisher->get_gid();
+
+    // if the publisher is not in the map, create a new publisher for published time
+    ensure_publisher_exists(gid_key, publisher->get_topic_name());
+
+    const auto & pub_published_time_ = publishers_[gid_key];
+
+    // Check if there are any subscribers, otherwise don't do anything
+    if (pub_published_time_->get_subscription_count() > 0) {
+      PublishedTime published_time;
+
+      published_time.header.stamp = stamp;
+      published_time.published_stamp = rclcpp::Clock().now();
+
+      pub_published_time_->publish(published_time);
+    }
+  }
+
+  void publish(
+    const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header)
+  {
+    const auto & gid_key = publisher->get_gid();
+
+    // if the publisher is not in the map, create a new publisher for published time
+    ensure_publisher_exists(gid_key, publisher->get_topic_name());
+
+    const auto & pub_published_time_ = publishers_[gid_key];
+
+    // Check if there are any subscribers, otherwise don't do anything
+    if (pub_published_time_->get_subscription_count() > 0) {
+      PublishedTime published_time;
+
+      published_time.header = header;
+      published_time.published_stamp = rclcpp::Clock().now();
+
+      pub_published_time_->publish(published_time);
+    }
+  }
+
+private:
+  rclcpp::Node * node_;
+  std::string publisher_topic_suffix_;
+  rclcpp::QoS qos_;
+
+  using PublishedTime = autoware_internal_msgs::msg::PublishedTime;
+
+  // Custom comparison struct for rmw_gid_t
+  struct GidCompare
+  {
+    bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const
+    {
+      return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0;
+    }
+  };
+
+  // ensure that the publisher exists in publisher_ map, if not, create a new one
+  void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name)
+  {
+    if (publishers_.find(gid_key) == publishers_.end()) {
+      publishers_[gid_key] =
+        node_->create_publisher<PublishedTime>(topic_name + publisher_topic_suffix_, qos_);
+    }
+  }
+
+  // store them for each different publisher of the node
+  std::map<rmw_gid_t, rclcpp::Publisher<PublishedTime>::SharedPtr, GidCompare> publishers_;
+};
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml
index 04a07b88edf4e..c34d3b5fdfdd0 100644
--- a/common/tier4_autoware_utils/package.xml
+++ b/common/tier4_autoware_utils/package.xml
@@ -15,6 +15,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_internal_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>