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 }
41
42 if (options.throttle_rate > 1e-3)
43 {
44 this->throttle_min_dt_ = 1.0 / options.throttle_rate;
45 this->throttle_ = true;
46 }
47
48 publisher_initialized_ = true;
49 }
50
51 //}
52
53 /* publish(TopicType& msg) //{ */
54
55 template <class TopicType>
56 void PublisherHandler_impl<TopicType>::publish(const TopicType& msg)
57 {
58
59 if (!publisher_initialized_)
60 {
61 return;
62 }
63
64 {
65 std::scoped_lock lock(mutex_publisher_);
66
67 rclcpp::Time now = node_->get_clock()->now();
68
69 if (throttle_min_dt_ > 0)
70 {
71
72 double passed = (now - last_time_published_).seconds();
73
74 if (passed < throttle_min_dt_)
75 {
76 return;
77 }
78 }
79
80 publisher_->publish(msg);
81
82 last_time_published_ = now;
83 }
84 }
85
86 //}
87
88 /* publish(const std::shared_ptr<TopicType>& msg) //{ */
89
90 template <class TopicType>
91 void PublisherHandler_impl<TopicType>::publish(const std::shared_ptr<TopicType>& msg)
92 {
93
94 if (!publisher_initialized_)
95 {
96 return;
97 }
98
99 {
100 std::scoped_lock lock(mutex_publisher_);
101
102 rclcpp::Time now = node_->get_clock()->now();
103
104 if (throttle_min_dt_ > 0)
105 {
106
107 double passed = (now - last_time_published_).seconds();
108
109 if (passed < throttle_min_dt_)
110 {
111 return;
112 }
113 }
114
115 publisher_->publish(msg);
116
117 last_time_published_ = now;
118 }
119 }
120
121 //}
122
123 /* publish(TopicType::ConstSharedPtr& msg) //{ */
124
125 template <class TopicType>
126 void PublisherHandler_impl<TopicType>::publish(typename TopicType::ConstSharedPtr msg)
127 {
128
129 if (!publisher_initialized_)
130 {
131 return;
132 }
133
134 {
135 std::scoped_lock lock(mutex_publisher_);
136
137 rclcpp::Time now = node_->get_clock()->now();
138
139 if (throttle_min_dt_ > 0)
140 {
141
142 double passed = (now - last_time_published_).seconds();
143
144 if (passed < throttle_min_dt_)
145 {
146 return;
147 }
148 }
149
150 publisher_->publish(msg);
151
152 last_time_published_ = now;
153 }
154 }
155
156 //}
157
158 /* getNumSubscribers(void) //{ */
159
160 template <class TopicType>
161 unsigned int PublisherHandler_impl<TopicType>::getNumSubscribers(void)
162 {
163
164 {
165 std::scoped_lock lock(mutex_publisher_);
166
167 return publisher_->get_subscription_count();
168 }
169 }
170
171 //}
172
173 // --------------------------------------------------------------
174 // | PublisherHandler |
175 // --------------------------------------------------------------
176
177 /* operator= //{ */
178
179 template <class TopicType>
180 PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other)
181 {
182
183 if (this == &other)
184 {
185 return *this;
186 }
187
188 if (other.impl_)
189 {
190 this->impl_ = other.impl_;
191 }
192
193 return *this;
194 }
195
196 //}
197
198 /* copy constructor //{ */
199
200 template <class TopicType>
201 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other)
202 {
203
204 if (other.impl_)
205 {
206 this->impl_ = other.impl_;
207 }
208 }
209
210 //}
211
212 /* PublisherHandler(rclcpp::Node::SharedPtr node, const std::string& address) //{ */
213
214 template <class TopicType>
215 PublisherHandler<TopicType>::PublisherHandler(const rclcpp::Node::SharedPtr& node, const std::string& address)
216 {
217
218 PublisherHandlerOptions opts;
219
220 opts.node = node;
221
222 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(opts, address);
223 }
224
225 //}
226
227 /* PublisherHandler(const rclcpp::PublisherOptions& options, const std::string& address) //{ */
228
229 template <class TopicType>
230 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandlerOptions& options, const std::string& address)
231 {
232
233 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(options, address);
234 }
235
236 //}
237
238 /* publish(const TopicType& msg) //{ */
239
240 template <class TopicType>
241 void PublisherHandler<TopicType>::publish(const TopicType& msg)
242 {
243
244 impl_->publish(msg);
245 }
246
247 //}
248
249 /* publish(const std::shared_ptr<TopicType>& msg) //{ */
250
251 template <class TopicType>
252 void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType>& msg)
253 {
254
255 impl_->publish(msg);
256 }
257
258 //}
259
260 /* /1* publish(const std::shared_ptr<TopicType const>& msg) //{ *1/ */
261
262 /* template <class TopicType> */
263 /* void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType const>& msg) { */
264
265 /* impl_->publish(msg); */
266 /* } */
267
268 /* //} */
269
270 /* publish(const TopicType::ConstSharedPtr& msg) //{ */
271
272 template <class TopicType>
273 void PublisherHandler<TopicType>::publish(typename TopicType::ConstSharedPtr msg)
274 {
275
276 impl_->publish(msg);
277 }
278
279 //}
280
281 /* getNumSubscribers(void) //{ */
282
283 template <class TopicType>
284 unsigned int PublisherHandler<TopicType>::getNumSubscribers(void)
285 {
286
287 return impl_->getNumSubscribers();
288 }
289
290 //}
291
292} // namespace mrs_lib
293
294#endif // PUBLISHER_HANDLER_HPP
mrs_lib::PublisherHandler_impl::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:161
mrs_lib::PublisherHandler_impl::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:56
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:130
mrs_lib::PublisherHandler::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:241
mrs_lib::PublisherHandler::operator=
PublisherHandler & operator=(const PublisherHandler &other)
operator=
Definition publisher_handler.hpp:180
mrs_lib::PublisherHandler::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:284
mrs_lib::PublisherHandler::PublisherHandler
PublisherHandler(void)
generic constructor
Definition publisher_handler.h:136
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