From 784ae91217dbf9a10f9b952b9aa102323a893443 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 27 Feb 2024 17:29:23 +0300 Subject: [PATCH 1/4] feat(tier4_autoware_utils): add published time debug class into utils Signed-off-by: Berkay Karaman --- .../ros/published_time_publisher.hpp | 131 ++++++++++++++++++ common/tier4_autoware_utils/package.xml | 1 + 2 files changed, 132 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp 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..6d49aab894860 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp @@ -0,0 +1,131 @@ +// 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 + +#include +#include + +#include +#include +#include + +namespace tier4_autoware_utils +{ +using autoware_internal_msgs::msg::PublishedTime; + +struct GidHash +{ + size_t operator()(const rmw_gid_t & gid) const noexcept + { + // Hashing function that computes a hash value for the GID + std::hash hasher; + return hasher(std::string(reinterpret_cast(gid.data), RMW_GID_STORAGE_SIZE)); + } +}; + +struct GidEqual +{ + bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const noexcept + { + return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) == 0; + } +}; + +class PublishedTimePublisher +{ +public: + static std::unique_ptr create( + rclcpp::Node * node, const std::string & end_name = "/debug/published_time", + const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + const bool use_published_time = node->declare_parameter("use_published_time", false); + if (use_published_time) { + return std::unique_ptr( + new PublishedTimePublisher(node, end_name, qos)); + } else { + return nullptr; + } + } + + void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) + { + const auto & gid = publisher->get_gid(); + const auto & topic_name = publisher->get_topic_name(); + + // if the publisher is not in the map, create a new publisher for published time + if (publishers_.find(gid) == publishers_.end()) { + publishers_[gid] = node_->create_publisher( + static_cast(topic_name) + end_name_, qos_); + } + + const auto & pub_published_time_ = publishers_[gid]; + + // 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 = publisher->get_gid(); + const auto & topic_name = publisher->get_topic_name(); + + // if the publisher is not in the map, create a new publisher for published time + if (publishers_.find(gid) == publishers_.end()) { + publishers_[gid] = node_->create_publisher( + static_cast(topic_name) + end_name_, qos_); + } + + const auto & pub_published_time_ = publishers_[gid]; + + // 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: + explicit PublishedTimePublisher( + rclcpp::Node * node, const std::string & end_name, const rclcpp::QoS & qos) + : node_(node), end_name_(end_name), qos_(qos) + { + } + + rclcpp::Node * node_; + std::string end_name_; + rclcpp::QoS qos_; + + // store them for each different publisher of the node + std::unordered_map::SharedPtr, GidHash, GidEqual> + 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 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_internal_msgs builtin_interfaces diagnostic_msgs geometry_msgs From 37dd01466e1668f4d4048dd6bc81fa4d9aa3e01e Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sat, 9 Mar 2024 06:48:29 +0300 Subject: [PATCH 2/4] feat: remove create() function and convert unordered_map to map Signed-off-by: Berkay Karaman --- .../ros/published_time_publisher.hpp | 62 +++++++------------ 1 file changed, 21 insertions(+), 41 deletions(-) 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 index 6d49aab894860..98b66e0192fc8 100644 --- 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 @@ -28,52 +28,34 @@ namespace tier4_autoware_utils { using autoware_internal_msgs::msg::PublishedTime; -struct GidHash +// Custom comparison struct for rmw_gid_t +struct GidCompare { - size_t operator()(const rmw_gid_t & gid) const noexcept + bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const { - // Hashing function that computes a hash value for the GID - std::hash hasher; - return hasher(std::string(reinterpret_cast(gid.data), RMW_GID_STORAGE_SIZE)); - } -}; - -struct GidEqual -{ - bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const noexcept - { - return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) == 0; + return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; } }; class PublishedTimePublisher { public: - static std::unique_ptr create( + explicit PublishedTimePublisher( rclcpp::Node * node, const std::string & end_name = "/debug/published_time", const rclcpp::QoS & qos = rclcpp::QoS(1)) + : node_(node), end_name_(end_name), qos_(qos) { - const bool use_published_time = node->declare_parameter("use_published_time", false); - if (use_published_time) { - return std::unique_ptr( - new PublishedTimePublisher(node, end_name, qos)); - } else { - return nullptr; - } } void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) { - const auto & gid = publisher->get_gid(); + const auto & gid_key = publisher->get_gid(); const auto & topic_name = publisher->get_topic_name(); // if the publisher is not in the map, create a new publisher for published time - if (publishers_.find(gid) == publishers_.end()) { - publishers_[gid] = node_->create_publisher( - static_cast(topic_name) + end_name_, qos_); - } + ensure_publisher_exists(gid_key, topic_name); - const auto & pub_published_time_ = publishers_[gid]; + 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) { @@ -89,16 +71,13 @@ class PublishedTimePublisher void publish( const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) { - const auto & gid = publisher->get_gid(); + const auto & gid_key = publisher->get_gid(); const auto & topic_name = publisher->get_topic_name(); // if the publisher is not in the map, create a new publisher for published time - if (publishers_.find(gid) == publishers_.end()) { - publishers_[gid] = node_->create_publisher( - static_cast(topic_name) + end_name_, qos_); - } + ensure_publisher_exists(gid_key, topic_name); - const auto & pub_published_time_ = publishers_[gid]; + 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) { @@ -112,19 +91,20 @@ class PublishedTimePublisher } private: - explicit PublishedTimePublisher( - rclcpp::Node * node, const std::string & end_name, const rclcpp::QoS & qos) - : node_(node), end_name_(end_name), qos_(qos) - { - } - rclcpp::Node * node_; std::string end_name_; rclcpp::QoS qos_; + // 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(topic_name + end_name_, qos_); + } + } + // store them for each different publisher of the node - std::unordered_map::SharedPtr, GidHash, GidEqual> - publishers_; + std::map::SharedPtr, GidCompare> publishers_; }; } // namespace tier4_autoware_utils From b42b772c862b81e239d83bb0b7bd8d39ce3bf404 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 11 Mar 2024 14:43:27 +0300 Subject: [PATCH 3/4] feat: move GidCompare and namespace into class Signed-off-by: Berkay Karaman --- .../ros/published_time_publisher.hpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) 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 index 98b66e0192fc8..2b6c2997f2caf 100644 --- 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 @@ -26,34 +26,23 @@ namespace tier4_autoware_utils { -using 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; - } -}; class PublishedTimePublisher { public: explicit PublishedTimePublisher( - rclcpp::Node * node, const std::string & end_name = "/debug/published_time", + rclcpp::Node * node, const std::string & publisher_topic_suffix = "/debug/published_time", const rclcpp::QoS & qos = rclcpp::QoS(1)) - : node_(node), end_name_(end_name), qos_(qos) + : 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(); - const auto & topic_name = publisher->get_topic_name(); // if the publisher is not in the map, create a new publisher for published time - ensure_publisher_exists(gid_key, topic_name); + ensure_publisher_exists(gid_key, publisher->get_topic_name()); const auto & pub_published_time_ = publishers_[gid_key]; @@ -72,10 +61,9 @@ class PublishedTimePublisher const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) { const auto & gid_key = publisher->get_gid(); - const auto & topic_name = publisher->get_topic_name(); // if the publisher is not in the map, create a new publisher for published time - ensure_publisher_exists(gid_key, topic_name); + ensure_publisher_exists(gid_key, publisher->get_topic_name()); const auto & pub_published_time_ = publishers_[gid_key]; @@ -92,14 +80,26 @@ class PublishedTimePublisher private: rclcpp::Node * node_; - std::string end_name_; + 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(topic_name + end_name_, qos_); + publishers_[gid_key] = + node_->create_publisher(topic_name + publisher_topic_suffix_, qos_); } } From a1a595c6e509a61088030592a2e527c6d359f50b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 12 Mar 2024 14:13:31 +0300 Subject: [PATCH 4/4] add autoware_internal_msgs to build_depends.repos MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- build_depends.repos | 4 ++++ 1 file changed, 4 insertions(+) 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