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
16 template <class TopicType>
17 PublisherHandler_impl<TopicType>::PublisherHandler_impl(void) : publisher_initialized_(false)
18 {
19
20 last_time_published_ = rclcpp::Time(0, 0, node_->get_clock()->get_clock_type());
21 }
22
23 //}
24
25 /* PublisherHandler_impl(const PublisherHandlerOptions& options, const std::string& address) //{ */
26
27 template <class TopicType>
28 PublisherHandler_impl<TopicType>::PublisherHandler_impl(const PublisherHandlerOptions& options, const std::string& address)
29 {
30
31 node_ = options.node;
32
33 last_time_published_ = node_->get_clock()->now();
34
35 {
36 std::scoped_lock lock(mutex_publisher_);
37
38 address_ = address;
39 publisher_ = node_->create_publisher<TopicType>(address, options.qos);
40 RCLCPP_INFO(node_->get_logger(), " Created publisher on topic '%s' -> '%s'", address.c_str(), publisher_->get_topic_name());
41 }
42
43 if (options.throttle_rate > 1e-3)
44 {
45 this->throttle_min_dt_ = 1.0 / options.throttle_rate;
46 this->throttle_ = true;
47 }
48
49 publisher_initialized_ = true;
50 }
51
52 //}
53
54 /* publish(TopicType& msg) //{ */
55
56 template <class TopicType>
57 void PublisherHandler_impl<TopicType>::publish(const TopicType& msg)
58 {
59
60 if (!publisher_initialized_)
61 {
62 return;
63 }
64
65 {
66 std::scoped_lock lock(mutex_publisher_);
67
68 rclcpp::Time now = node_->get_clock()->now();
69
70 if (throttle_min_dt_ > 0)
71 {
72
73 double passed = (now - last_time_published_).seconds();
74
75 if (passed < throttle_min_dt_)
76 {
77 return;
78 }
79 }
80
81 publisher_->publish(msg);
82
83 last_time_published_ = now;
84 }
85 }
86
87 //}
88
89 /* publish(const std::shared_ptr<TopicType>& msg) //{ */
90
91 template <class TopicType>
92 void PublisherHandler_impl<TopicType>::publish(const std::shared_ptr<TopicType>& msg)
93 {
94
95 if (!publisher_initialized_)
96 {
97 return;
98 }
99
100 {
101 std::scoped_lock lock(mutex_publisher_);
102
103 rclcpp::Time now = node_->get_clock()->now();
104
105 if (throttle_min_dt_ > 0)
106 {
107
108 double passed = (now - last_time_published_).seconds();
109
110 if (passed < throttle_min_dt_)
111 {
112 return;
113 }
114 }
115
116 publisher_->publish(msg);
117
118 last_time_published_ = now;
119 }
120 }
121
122 //}
123
124 /* publish(TopicType::ConstSharedPtr& msg) //{ */
125
126 template <class TopicType>
127 void PublisherHandler_impl<TopicType>::publish(typename TopicType::ConstSharedPtr msg)
128 {
129
130 if (!publisher_initialized_)
131 {
132 return;
133 }
134
135 {
136 std::scoped_lock lock(mutex_publisher_);
137
138 rclcpp::Time now = node_->get_clock()->now();
139
140 if (throttle_min_dt_ > 0)
141 {
142
143 double passed = (now - last_time_published_).seconds();
144
145 if (passed < throttle_min_dt_)
146 {
147 return;
148 }
149 }
150
151 publisher_->publish(msg);
152
153 last_time_published_ = now;
154 }
155 }
156
157 //}
158
159 /* publish(std::unique_ptr<TopicType> msg) //{ */
160
161 template <class TopicType>
162 void PublisherHandler_impl<TopicType>::publish(std::unique_ptr<TopicType> msg)
163 {
164 if (!publisher_initialized_)
165 {
166 return;
167 }
168
169 {
170 std::scoped_lock lock(mutex_publisher_);
171
172 rclcpp::Time now = node_->get_clock()->now();
173
174 if (throttle_min_dt_ > 0)
175 {
176
177 double passed = (now - last_time_published_).seconds();
178
179 if (passed < throttle_min_dt_)
180 {
181 return;
182 }
183 }
184
185 publisher_->publish(std::move(msg));
186
187 last_time_published_ = now;
188 }
189 }
190
191 //}
192
193 /* getNumSubscribers(void) //{ */
194
195 template <class TopicType>
196 unsigned int PublisherHandler_impl<TopicType>::getNumSubscribers(void)
197 {
198
199 {
200 std::scoped_lock lock(mutex_publisher_);
201
202 return publisher_->get_subscription_count();
203 }
204 }
205
206 //}
207
208 // --------------------------------------------------------------
209 // | PublisherHandler |
210 // --------------------------------------------------------------
211
212 /* operator= //{ */
213
214 template <class TopicType>
215 PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other)
216 {
217
218 if (this == &other)
219 {
220 return *this;
221 }
222
223 if (other.impl_)
224 {
225 this->impl_ = other.impl_;
226 }
227
228 return *this;
229 }
230
231 //}
232
233 /* copy constructor //{ */
234
235 template <class TopicType>
236 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other)
237 {
238
239 if (other.impl_)
240 {
241 this->impl_ = other.impl_;
242 }
243 }
244
245 //}
246
247 /* PublisherHandler(rclcpp::Node::SharedPtr node, const std::string& address) //{ */
248
249 template <class TopicType>
250 PublisherHandler<TopicType>::PublisherHandler(const rclcpp::Node::SharedPtr& node, const std::string& address)
251 {
252
253 PublisherHandlerOptions opts;
254
255 opts.node = node;
256
257 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(opts, address);
258 }
259
260 //}
261
262 /* PublisherHandler(const rclcpp::PublisherOptions& options, const std::string& address) //{ */
263
264 template <class TopicType>
265 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandlerOptions& options, const std::string& address)
266 {
267
268 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(options, address);
269 }
270
271 //}
272
273 /* publish(const TopicType& msg) //{ */
274
275 template <class TopicType>
276 void PublisherHandler<TopicType>::publish(const TopicType& msg)
277 {
278
279 impl_->publish(msg);
280 }
281
282 //}
283
284 /* publish(const std::shared_ptr<TopicType>& msg) //{ */
285
286 template <class TopicType>
287 void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType>& msg)
288 {
289
290 impl_->publish(msg);
291 }
292
293 //}
294
295
296 /* /1* publish(const std::shared_ptr<TopicType const>& msg) //{ *1/ */
297
298 /* template <class TopicType> */
299 /* void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType const>& msg) { */
300
301 /* impl_->publish(msg); */
302 /* } */
303
304 /* //} */
305
306 /* publish(const TopicType::ConstSharedPtr& msg) //{ */
307
308 template <class TopicType>
309 void PublisherHandler<TopicType>::publish(typename TopicType::ConstSharedPtr msg)
310 {
311
312 impl_->publish(msg);
313 }
314
315 //}
316
317
318 /* publish(std::unique_ptr<TopicType> msg) //{ */
319
320 template <class TopicType>
321 void PublisherHandler<TopicType>::publish(std::unique_ptr<TopicType> msg)
322 {
323
324 impl_->publish(std::move(msg));
325 }
326
327 //}
328
329 /* getNumSubscribers(void) //{ */
330
331 template <class TopicType>
332 unsigned int PublisherHandler<TopicType>::getNumSubscribers(void)
333 {
334
335 return impl_->getNumSubscribers();
336 }
337
338 //}
339
340} // namespace mrs_lib
341
342#endif // PUBLISHER_HANDLER_HPP
mrs_lib::PublisherHandler_impl::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:196
mrs_lib::PublisherHandler_impl::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:57
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:137
mrs_lib::PublisherHandler::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:276
mrs_lib::PublisherHandler::operator=
PublisherHandler & operator=(const PublisherHandler &other)
operator=
Definition publisher_handler.hpp:215
mrs_lib::PublisherHandler::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:332
mrs_lib::PublisherHandler::PublisherHandler
PublisherHandler(void)
generic constructor
Definition publisher_handler.h:143
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:35