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 /* getNumSubscribers(void) //{ */
160
161 template <class TopicType>
162 unsigned int PublisherHandler_impl<TopicType>::getNumSubscribers(void)
163 {
164
165 {
166 std::scoped_lock lock(mutex_publisher_);
167
168 return publisher_->get_subscription_count();
169 }
170 }
171
172 //}
173
174 // --------------------------------------------------------------
175 // | PublisherHandler |
176 // --------------------------------------------------------------
177
178 /* operator= //{ */
179
180 template <class TopicType>
181 PublisherHandler<TopicType>& PublisherHandler<TopicType>::operator=(const PublisherHandler<TopicType>& other)
182 {
183
184 if (this == &other)
185 {
186 return *this;
187 }
188
189 if (other.impl_)
190 {
191 this->impl_ = other.impl_;
192 }
193
194 return *this;
195 }
196
197 //}
198
199 /* copy constructor //{ */
200
201 template <class TopicType>
202 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandler<TopicType>& other)
203 {
204
205 if (other.impl_)
206 {
207 this->impl_ = other.impl_;
208 }
209 }
210
211 //}
212
213 /* PublisherHandler(rclcpp::Node::SharedPtr node, const std::string& address) //{ */
214
215 template <class TopicType>
216 PublisherHandler<TopicType>::PublisherHandler(const rclcpp::Node::SharedPtr& node, const std::string& address)
217 {
218
219 PublisherHandlerOptions opts;
220
221 opts.node = node;
222
223 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(opts, address);
224 }
225
226 //}
227
228 /* PublisherHandler(const rclcpp::PublisherOptions& options, const std::string& address) //{ */
229
230 template <class TopicType>
231 PublisherHandler<TopicType>::PublisherHandler(const PublisherHandlerOptions& options, const std::string& address)
232 {
233
234 impl_ = std::make_shared<PublisherHandler_impl<TopicType>>(options, address);
235 }
236
237 //}
238
239 /* publish(const TopicType& msg) //{ */
240
241 template <class TopicType>
242 void PublisherHandler<TopicType>::publish(const TopicType& msg)
243 {
244
245 impl_->publish(msg);
246 }
247
248 //}
249
250 /* publish(const std::shared_ptr<TopicType>& msg) //{ */
251
252 template <class TopicType>
253 void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType>& msg)
254 {
255
256 impl_->publish(msg);
257 }
258
259 //}
260
261 /* /1* publish(const std::shared_ptr<TopicType const>& msg) //{ *1/ */
262
263 /* template <class TopicType> */
264 /* void PublisherHandler<TopicType>::publish(const std::shared_ptr<TopicType const>& msg) { */
265
266 /* impl_->publish(msg); */
267 /* } */
268
269 /* //} */
270
271 /* publish(const TopicType::ConstSharedPtr& msg) //{ */
272
273 template <class TopicType>
274 void PublisherHandler<TopicType>::publish(typename TopicType::ConstSharedPtr msg)
275 {
276
277 impl_->publish(msg);
278 }
279
280 //}
281
282 /* getNumSubscribers(void) //{ */
283
284 template <class TopicType>
285 unsigned int PublisherHandler<TopicType>::getNumSubscribers(void)
286 {
287
288 return impl_->getNumSubscribers();
289 }
290
291 //}
292
293} // namespace mrs_lib
294
295#endif // PUBLISHER_HANDLER_HPP
mrs_lib::PublisherHandler_impl::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:162
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:130
mrs_lib::PublisherHandler::publish
void publish(const TopicType &msg)
publish message
Definition publisher_handler.hpp:242
mrs_lib::PublisherHandler::operator=
PublisherHandler & operator=(const PublisherHandler &other)
operator=
Definition publisher_handler.hpp:181
mrs_lib::PublisherHandler::getNumSubscribers
unsigned int getNumSubscribers(void)
get number of subscribers
Definition publisher_handler.hpp:285
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