mrs_lib
Various reusable classes, functions and utilities for use in MRS projects
Loading...
Searching...
No Matches
publisher_handler.hpp
1// clang: TomasFormat
2#ifndef PUBLISHER_HANDLER_HPP
3#define PUBLISHER_HANDLER_HPP
4
5#include <mrs_lib/publisher_handler.h>
6
7namespace mrs_lib
8{
9
10// --------------------------------------------------------------
11// | PublisherHandler_impl |
12// --------------------------------------------------------------
13
14/* PublisherHandler_impl(void) //{ */
15
16template <class TopicType>
17PublisherHandler_impl<TopicType>::PublisherHandler_impl(void) : publisher_initialized_(false) {
18
19 last_time_published_ = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type());
20}
21
22//}
23
24/* PublisherHandler_impl(const PublisherHandlerOptions& options, const std::string& address) //{ */
25
26template <class TopicType>
27PublisherHandler_impl<TopicType>::PublisherHandler_impl(const PublisherHandlerOptions& options, const std::string& address) {
28
29 node_ = options.node;
30
31 last_time_published_ = node_->get_clock()->now();
32
33 {
34 std::scoped_lock lock(mutex_publisher_);
35
36 address_ = address;
37 publisher_ = node_->create_publisher<TopicType>(address, options.qos);
38 }
39
40 if (options.throttle_rate > 1e-3) {
41 this->throttle_min_dt_ = 1.0 / options.throttle_rate;
42 this->throttle_ = true;
43 }
44
45 publisher_initialized_ = true;
46}
47
48//}
49
50/* publish(TopicType& msg) //{ */
51
52template <class TopicType>
53void PublisherHandler_impl<TopicType>::publish(const TopicType& msg) {
54
55 if (!publisher_initialized_) {
56 return;
57 }
58
59 {
60 std::scoped_lock lock(mutex_publisher_);
61
62 rclcpp::Time now = node_->get_clock()->now();
63
64 if (throttle_min_dt_ > 0) {
65
66 double passed = (now - last_time_published_).seconds();
67
68 if (passed < throttle_min_dt_) {
69 return;
70 }
71 }
72
73 publisher_->publish(msg);
74
75 last_time_published_ = now;
76 }
77}
78
79//}
80
81/* publish(const std::shared_ptr<TopicType>& msg) //{ */
82
83template <class TopicType>
84void PublisherHandler_impl<TopicType>::publish(const std::shared_ptr<TopicType>& msg) {
85
86 if (!publisher_initialized_) {
87 return;
88 }
89
90 {
91 std::scoped_lock lock(mutex_publisher_);
92
93 rclcpp::Time now = node_->get_clock()->now();
94
95 if (throttle_min_dt_ > 0) {
96
97 double passed = (now - last_time_published_).seconds();
98
99 if (passed < throttle_min_dt_) {
100 return;
101 }
102 }
103
104 publisher_->publish(msg);
105
106 last_time_published_ = now;
107 }
108}
109
110//}
111
112/* publish(TopicType::ConstSharedPtr& msg) //{ */
113
114template <class TopicType>
115void PublisherHandler_impl<TopicType>::publish(typename TopicType::ConstSharedPtr msg) {
116
117 if (!publisher_initialized_) {
118 return;
119 }
120
121 {
122 std::scoped_lock lock(mutex_publisher_);
123
124 rclcpp::Time now = node_->get_clock()->now();
125
126 if (throttle_min_dt_ > 0) {
127
128 double passed = (now - last_time_published_).seconds();
129
130 if (passed < throttle_min_dt_) {
131 return;
132 }
133 }
134
135 publisher_->publish(msg);
136
137 last_time_published_ = now;
138 }
139}
140
141//}
142
143/* getNumSubscribers(void) //{ */
144
145template <class TopicType>
146unsigned int PublisherHandler_impl<TopicType>::getNumSubscribers(void) {
147
148 {
149 std::scoped_lock lock(mutex_publisher_);
150
151 return publisher_.getNumSubscribers();
152 }
153}
154
155//}
156
157// --------------------------------------------------------------
158// | PublisherHandler |
159// --------------------------------------------------------------
160
161/* operator= //{ */
162
163template <class TopicType>
164PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other) {
165
166 if (this == &other) {
167 return *this;
168 }
169
170 if (other.impl_) {
171 this->impl_ = other.impl_;
172 }
173
174 return *this;
175}
176
177//}
178
179/* copy constructor //{ */
180
181template <class TopicType>
182PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other) {
183
184 if (other.impl_) {
185 this->impl_ = other.impl_;
186 }
187}
188
189//}
190
191/* PublisherHandler(rclcpp::Node::SharedPtr node, const std::string& address) //{ */
192
193template <class TopicType>
194PublisherHandler<TopicType>::PublisherHandler(const rclcpp::Node::SharedPtr& node, const std::string& address) {
195
196 PublisherHandlerOptions opts;
197
198 opts.node = node;
199
200 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(opts, address);
201}
202
203//}
204
205/* PublisherHandler(const rclcpp::PublisherOptions& options, const std::string& address) //{ */
206
207template <class TopicType>
208PublisherHandler<TopicType>::PublisherHandler(const PublisherHandlerOptions& options, const std::string& address) {
209
210 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(options, address);
211}
212
213//}
214
215/* publish(const TopicType& msg) //{ */
216
217template <class TopicType>
218void PublisherHandler<TopicType>::publish(const TopicType& msg) {
219
220 impl_->publish(msg);
221}
222
223//}
224
225/* publish(const std::shared_ptr<TopicType>& msg) //{ */
226
227template <class TopicType>
228void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType>& msg) {
229
230 impl_->publish(msg);
231}
232
233//}
234
235/* /1* publish(const std::shared_ptr<TopicType const>& msg) //{ *1/ */
236
237/* template <class TopicType> */
238/* void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType const>& msg) { */
239
240/* impl_->publish(msg); */
241/* } */
242
243/* //} */
244
245/* publish(const TopicType::ConstSharedPtr& msg) //{ */
246
247template <class TopicType>
248void PublisherHandler<TopicType>::publish(typename TopicType::ConstSharedPtr msg) {
249
250 impl_->publish(msg);
251}
252
253//}
254
255/* getNumSubscribers(void) //{ */
256
257template <class TopicType>
258unsigned int PublisherHandler<TopicType>::getNumSubscribers(void) {
259
260 return impl_->getNumSubscribers();
261}
262
263//}
264
265} // namespace mrs_lib
266
267#endif // PUBLISHER_HANDLER_HPP
mrs_lib::PublisherHandler_impl::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:146
mrs_lib::PublisherHandler_impl::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:53
mrs_lib::PublisherHandler_impl::PublisherHandler_impl
PublisherHandler_impl(void)
default constructor
Definition publisher_handler.hpp:17
mrs_lib::PublisherHandler
user wrapper of the publisher handler implementation
Definition publisher_handler.h:127
mrs_lib::PublisherHandler::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:218
mrs_lib::PublisherHandler::operator=
PublisherHandler & operator=(const PublisherHandler &other)
operator=
Definition publisher_handler.hpp:164
mrs_lib::PublisherHandler::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:258
mrs_lib::PublisherHandler::PublisherHandler
PublisherHandler(void)
generic constructor
Definition publisher_handler.h:133
mrs_lib
All mrs_lib functions, classes, variables and definitions are contained in this namespace.
Definition attitude_converter.h:24
publisher_handler.h
Defines PublisherHandler and related convenience classes for upgrading the ROS publisher.
mrs_lib::PublisherHandlerOptions
Definition publisher_handler.h:23
mrs_lib::PublisherHandlerOptions::throttle_rate
double throttle_rate
when > 0, the publisher output is limited to this rate in [Hz]
Definition publisher_handler.h:34