|
26 | 26 |
|
27 | 27 | namespace tier4_autoware_utils
|
28 | 28 | {
|
29 |
| -using autoware_internal_msgs::msg::PublishedTime; |
30 |
| - |
31 |
| -// Custom comparison struct for rmw_gid_t |
32 |
| -struct GidCompare |
33 |
| -{ |
34 |
| - bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const |
35 |
| - { |
36 |
| - return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; |
37 |
| - } |
38 |
| -}; |
39 | 29 |
|
40 | 30 | class PublishedTimePublisher
|
41 | 31 | {
|
42 | 32 | public:
|
43 | 33 | explicit PublishedTimePublisher(
|
44 |
| - rclcpp::Node * node, const std::string & end_name = "/debug/published_time", |
| 34 | + rclcpp::Node * node, const std::string & publisher_topic_suffix = "/debug/published_time", |
45 | 35 | const rclcpp::QoS & qos = rclcpp::QoS(1))
|
46 |
| - : node_(node), end_name_(end_name), qos_(qos) |
| 36 | + : node_(node), publisher_topic_suffix_(publisher_topic_suffix), qos_(qos) |
47 | 37 | {
|
48 | 38 | }
|
49 | 39 |
|
50 | 40 | void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
|
51 | 41 | {
|
52 | 42 | const auto & gid_key = publisher->get_gid();
|
53 |
| - const auto & topic_name = publisher->get_topic_name(); |
54 | 43 |
|
55 | 44 | // if the publisher is not in the map, create a new publisher for published time
|
56 |
| - ensure_publisher_exists(gid_key, topic_name); |
| 45 | + ensure_publisher_exists(gid_key, publisher->get_topic_name()); |
57 | 46 |
|
58 | 47 | const auto & pub_published_time_ = publishers_[gid_key];
|
59 | 48 |
|
@@ -92,14 +81,25 @@ class PublishedTimePublisher
|
92 | 81 |
|
93 | 82 | private:
|
94 | 83 | rclcpp::Node * node_;
|
95 |
| - std::string end_name_; |
| 84 | + std::string publisher_topic_suffix_; |
96 | 85 | rclcpp::QoS qos_;
|
97 | 86 |
|
| 87 | + using PublishedTime = autoware_internal_msgs::msg::PublishedTime; |
| 88 | + |
| 89 | + // Custom comparison struct for rmw_gid_t |
| 90 | + struct GidCompare |
| 91 | + { |
| 92 | + bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const |
| 93 | + { |
| 94 | + return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; |
| 95 | + } |
| 96 | + }; |
| 97 | + |
98 | 98 | // ensure that the publisher exists in publisher_ map, if not, create a new one
|
99 | 99 | void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name)
|
100 | 100 | {
|
101 | 101 | if (publishers_.find(gid_key) == publishers_.end()) {
|
102 |
| - publishers_[gid_key] = node_->create_publisher<PublishedTime>(topic_name + end_name_, qos_); |
| 102 | + publishers_[gid_key] = node_->create_publisher<PublishedTime>(topic_name + publisher_topic_suffix_, qos_); |
103 | 103 | }
|
104 | 104 | }
|
105 | 105 |
|
|
0 commit comments