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
…autowarefoundation#6440)

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
brkay54 authored and karishma1911 committed Jun 3, 2024
1 parent 43f04d7 commit a514ee4
Show file tree
Hide file tree
Showing 3 changed files with 116 additions and 0 deletions.
4 changes: 4 additions & 0 deletions build_depends.repos
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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_
1 change: 1 addition & 0 deletions common/tier4_autoware_utils/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down

0 comments on commit a514ee4

Please sign in to comment.