Skip to content

Commit

Permalink
feat(tier4_autoware_utils): add published time debug class into utils
Browse files Browse the repository at this point in the history
Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 committed Feb 26, 2024
1 parent b86a00d commit 19ddcab
Show file tree
Hide file tree
Showing 2 changed files with 108 additions and 0 deletions.
Original file line number Diff line number Diff line change
@@ -0,0 +1,106 @@
// Copyright 2020 Tier IV, Inc.
//
// 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_optional_msgs/msg/published_time.hpp>

#include <functional>
#include <map>
#include <string>

namespace tier4_autoware_utils
{
using autoware_optional_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<std::string> hasher;
return hasher(std::string(reinterpret_cast<const char *>(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<PublishedTimePublisher> 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<bool>("use_published_time", false);
if (use_published_time) {
return std::unique_ptr<PublishedTimePublisher>(
new PublishedTimePublisher(node, end_name, qos));
} else {
return nullptr;
}
}

void publish(
const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & header_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<PublishedTime>(
static_cast<std::string>(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 = header_stamp;
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<rmw_gid_t, rclcpp::Publisher<PublishedTime>::SharedPtr, GidHash, GidEqual>
publishers_;
};
} // namespace tier4_autoware_utils

#endif // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
2 changes: 2 additions & 0 deletions common/tier4_autoware_utils/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
<depend>autoware_auto_perception_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>autoware_common_msgs</depend>
<depend>autoware_optional_msgs</depend>
<depend>builtin_interfaces</depend>
<depend>diagnostic_msgs</depend>
<depend>geometry_msgs</depend>
Expand Down

0 comments on commit 19ddcab

Please sign in to comment.