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