Fawkes API Fawkes Development Version
laserscan_thread.cpp
1
2/***************************************************************************
3 * laserscan_thread.cpp - Thread to exchange laser scans
4 *
5 * Created: Tue May 29 19:41:18 2012
6 * Copyright 2011-2012 Tim Niemueller [www.niemueller.de]
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "laserscan_thread.h"
23
24#include <core/threading/mutex_locker.h>
25#include <ros/this_node.h>
26#include <sensor_msgs/LaserScan.h>
27#include <utils/math/angle.h>
28
29#include <fnmatch.h>
30
31using namespace fawkes;
32
33/** @class RosLaserScanThread "pcl_thread.h"
34 * Thread to exchange point clouds between Fawkes and ROS.
35 * @author Tim Niemueller
36 */
37
38/** Constructor. */
40: Thread("RosLaserScanThread", Thread::OPMODE_WAITFORWAKEUP),
41 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
42 BlackBoardInterfaceListener("RosLaserScanThread")
43{
44 ls_msg_queue_mutex_ = new Mutex();
45 seq_num_mutex_ = new Mutex();
46}
47
48/** Destructor. */
50{
51 delete ls_msg_queue_mutex_;
52 delete seq_num_mutex_;
53}
54
55std::string
56RosLaserScanThread::topic_name(const char *if_id, const char *suffix)
57{
58 std::string topic_name = std::string("fawkes_scans/") + if_id + "_" + suffix;
59 std::string::size_type pos = 0;
60 while ((pos = topic_name.find("-", pos)) != std::string::npos) {
61 topic_name.replace(pos, 1, "_");
62 }
63 pos = 0;
64 while ((pos = topic_name.find(" ", pos)) != std::string::npos) {
65 topic_name.replace(pos, 1, "_");
66 }
67 return topic_name;
68}
69
70void
72{
73 active_queue_ = 0;
74 seq_num_ = 0;
75
76 // Must do that before registering listener because we might already
77 // get events right away
78 sub_ls_ = rosnode->subscribe("scan", 100, &RosLaserScanThread::laser_scan_message_cb, this);
79
83
84 std::list<Laser360Interface *>::iterator i360;
85 for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
86 (*i360)->read();
87 logger->log_info(name(), "Opened %s", (*i360)->uid());
91
92 std::string topname = topic_name((*i360)->id(), "360");
93
94 PublisherInfo pi;
95 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
96
97 logger->log_info(name(), "Publishing laser scan %s at %s", (*i360)->uid(), topname.c_str());
98
99 pi.msg.header.frame_id = (*i360)->frame();
100 pi.msg.angle_min = 0;
101 pi.msg.angle_max = 2 * M_PI;
102 pi.msg.angle_increment = deg2rad(1);
103 pi.msg.ranges.resize(360);
104
105 pubs_[(*i360)->uid()] = pi;
106 }
107
108 std::list<Laser720Interface *>::iterator i720;
109 for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
110 logger->log_info(name(), "Opened %s", (*i720)->uid());
114
115 std::string topname = topic_name((*i720)->id(), "720");
116
117 PublisherInfo pi;
118 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
119
120 logger->log_info(name(), "Publishing laser scan %s at %s", (*i720)->uid(), topname.c_str());
121
122 pi.msg.header.frame_id = (*i720)->frame();
123 pi.msg.angle_min = 0;
124 pi.msg.angle_max = 2 * M_PI;
125 pi.msg.angle_increment = deg2rad(0.5);
126 pi.msg.ranges.resize(720);
127
128 pubs_[(*i720)->uid()] = pi;
129 }
130
131 std::list<Laser1080Interface *>::iterator i1080;
132 for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
133 logger->log_info(name(), "Opened %s", (*i1080)->uid());
137
138 std::string topname = topic_name((*i1080)->id(), "1080");
139
140 PublisherInfo pi;
141 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
142
144 "Publishing laser scan %s at %s, frame %s",
145 (*i1080)->uid(),
146 topname.c_str(),
147 (*i1080)->frame());
148
149 pi.msg.header.frame_id = (*i1080)->frame();
150 pi.msg.angle_min = 0;
151 pi.msg.angle_max = 2 * M_PI;
152 pi.msg.angle_increment = deg2rad(1. / 3.);
153 pi.msg.ranges.resize(1080);
154
155 pubs_[(*i1080)->uid()] = pi;
156 }
157
159
160 bbio_add_observed_create("Laser360Interface", "*");
161 bbio_add_observed_create("Laser720Interface", "*");
162 bbio_add_observed_create("Laser1080Interface", "*");
164}
165
166void
168{
171
172 sub_ls_.shutdown();
173
174 std::map<std::string, PublisherInfo>::iterator p;
175 for (p = pubs_.begin(); p != pubs_.end(); ++p) {
176 p->second.pub.shutdown();
177 }
178
179 std::list<Laser360Interface *>::iterator i360;
180 for (i360 = ls360_ifs_.begin(); i360 != ls360_ifs_.end(); ++i360) {
181 blackboard->close(*i360);
182 }
183 ls360_ifs_.clear();
184 std::list<Laser720Interface *>::iterator i720;
185 for (i720 = ls720_ifs_.begin(); i720 != ls720_ifs_.end(); ++i720) {
186 blackboard->close(*i720);
187 }
188 ls720_ifs_.clear();
189 std::list<Laser1080Interface *>::iterator i1080;
190 for (i1080 = ls1080_ifs_.begin(); i1080 != ls1080_ifs_.end(); ++i1080) {
191 blackboard->close(*i1080);
192 }
193 ls1080_ifs_.clear();
194}
195
196void
198{
199 ls_msg_queue_mutex_->lock();
200 unsigned int queue = active_queue_;
201 active_queue_ = 1 - active_queue_;
202 ls_msg_queue_mutex_->unlock();
203
204 while (!ls_msg_queues_[queue].empty()) {
205 const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt = ls_msg_queues_[queue].front();
206
207 sensor_msgs::LaserScan::ConstPtr msg = msg_evt.getConstMessage();
208
209 // Check if interface exists, open if it does not
210 const std::string callerid = msg_evt.getPublisherName();
211
212 // for now we only create 360 interfaces, might add on that later
213 if (callerid.empty()) {
215 "Received laser scan from ROS without caller ID,"
216 "ignoring");
217 } else {
218 bool have_interface = true;
219 if (ls360_wifs_.find(callerid) == ls360_wifs_.end()) {
220 try {
221 std::string id = std::string("ROS Laser ") + callerid;
223 ls360_wifs_[callerid] = ls360if;
224 } catch (Exception &e) {
226 "Failed to open ROS laser interface for "
227 "message from node %s, exception follows",
228 callerid.c_str());
229 logger->log_warn(name(), e);
230 have_interface = false;
231 }
232 }
233
234 if (have_interface) {
235 // update interface with laser data
236 Laser360Interface *ls360if = ls360_wifs_[callerid];
237 ls360if->set_frame(msg->header.frame_id.c_str());
238 float distances[360];
239 for (unsigned int a = 0; a < 360; ++a) {
240 float a_rad = deg2rad(a);
241 if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
242 distances[a] = 0.;
243 } else {
244 // get closest ray from message
245 int idx = (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
246 distances[a] = msg->ranges[idx];
247 }
248 }
249 ls360if->set_distances(distances);
250 ls360if->write();
251 }
252 }
253
254 ls_msg_queues_[queue].pop();
255 }
256}
257
258void
260{
261 Laser360Interface * ls360if = dynamic_cast<Laser360Interface *>(interface);
262 Laser720Interface * ls720if = dynamic_cast<Laser720Interface *>(interface);
263 Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
264
265 PublisherInfo & pi = pubs_[interface->uid()];
266 sensor_msgs::LaserScan &msg = pi.msg;
267
268 if (ls360if) {
269 ls360if->read();
270
271 const Time *time = ls360if->timestamp();
272
273 seq_num_mutex_->lock();
274 msg.header.seq = ++seq_num_;
275 seq_num_mutex_->unlock();
276 msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
277 msg.header.frame_id = ls360if->frame();
278
279 msg.angle_min = 0;
280 msg.angle_max = 2 * M_PI;
281 msg.angle_increment = deg2rad(1);
282 msg.range_min = 0.;
283 msg.range_max = 1000.;
284 msg.ranges.resize(360);
285 memcpy(&msg.ranges[0], ls360if->distances(), 360 * sizeof(float));
286
287 pi.pub.publish(pi.msg);
288
289 } else if (ls720if) {
290 ls720if->read();
291
292 const Time *time = ls720if->timestamp();
293
294 seq_num_mutex_->lock();
295 msg.header.seq = ++seq_num_;
296 seq_num_mutex_->unlock();
297 msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
298 msg.header.frame_id = ls720if->frame();
299
300 msg.angle_min = 0;
301 msg.angle_max = 2 * M_PI;
302 msg.angle_increment = deg2rad(1. / 2.);
303 msg.range_min = 0.;
304 msg.range_max = 1000.;
305 msg.ranges.resize(720);
306 memcpy(&msg.ranges[0], ls720if->distances(), 720 * sizeof(float));
307
308 pi.pub.publish(pi.msg);
309
310 } else if (ls1080if) {
311 ls1080if->read();
312
313 const Time *time = ls1080if->timestamp();
314
315 seq_num_mutex_->lock();
316 msg.header.seq = ++seq_num_;
317 seq_num_mutex_->unlock();
318 msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
319 msg.header.frame_id = ls1080if->frame();
320
321 msg.angle_min = 0;
322 msg.angle_max = 2 * M_PI;
323 msg.angle_increment = deg2rad(1. / 3.);
324 msg.range_min = 0.;
325 msg.range_max = 1000.;
326 msg.ranges.resize(1080);
327 memcpy(&msg.ranges[0], ls1080if->distances(), 1080 * sizeof(float));
328
329 pi.pub.publish(pi.msg);
330 }
331}
332
333void
334RosLaserScanThread::bb_interface_created(const char *type, const char *id) noexcept
335{
336 // Ignore ID pattern of our own interfaces
337 if (fnmatch("ROS *", id, FNM_NOESCAPE) == 0)
338 return;
339
340 if (strncmp(type, "Laser360Interface", INTERFACE_TYPE_SIZE_) == 0) {
341 Laser360Interface *ls360if;
342 try {
343 logger->log_info(name(), "Opening %s:%s", type, id);
344 ls360if = blackboard->open_for_reading<Laser360Interface>(id);
345 } catch (Exception &e) {
346 // ignored
347 logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
348 return;
349 }
350
351 try {
352 bbil_add_data_interface(ls360if);
353 bbil_add_reader_interface(ls360if);
354 bbil_add_writer_interface(ls360if);
355
356 std::string topname = topic_name(ls360if->id(), "360");
357
358 PublisherInfo pi;
359 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
360
361 logger->log_info(name(), "Publishing laser scan %s at %s", ls360if->uid(), topname.c_str());
362
363 pi.msg.header.frame_id = ls360if->frame();
364 pi.msg.angle_min = 0;
365 pi.msg.angle_max = 2 * M_PI;
366 pi.msg.angle_increment = deg2rad(1);
367 pi.msg.ranges.resize(360);
368
369 pubs_[ls360if->uid()] = pi;
370
371 blackboard->update_listener(this);
372 ls360_ifs_.push_back(ls360if);
373 } catch (Exception &e) {
374 blackboard->close(ls360if);
375 logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
376 return;
377 }
378
379 } else if (strncmp(type, "Laser720Interface", INTERFACE_TYPE_SIZE_) == 0) {
380 Laser720Interface *ls720if;
381 try {
382 logger->log_info(name(), "Opening %s:%s", type, id);
383 ls720if = blackboard->open_for_reading<Laser720Interface>(id);
384 } catch (Exception &e) {
385 // ignored
386 logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
387 return;
388 }
389
390 try {
391 bbil_add_data_interface(ls720if);
392 bbil_add_reader_interface(ls720if);
393 bbil_add_writer_interface(ls720if);
394
395 std::string topname = topic_name(ls720if->id(), "720");
396
397 PublisherInfo pi;
398 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
399
400 logger->log_info(name(), "Publishing laser scan %s at %s", ls720if->uid(), topname.c_str());
401
402 pi.msg.header.frame_id = ls720if->frame();
403 pi.msg.angle_min = 0;
404 pi.msg.angle_max = 2 * M_PI;
405 pi.msg.angle_increment = deg2rad(0.5);
406 pi.msg.ranges.resize(720);
407
408 pubs_[ls720if->uid()] = pi;
409
410 blackboard->update_listener(this);
411 ls720_ifs_.push_back(ls720if);
412 } catch (Exception &e) {
413 blackboard->close(ls720if);
414 logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
415 return;
416 }
417
418 } else if (strncmp(type, "Laser1080Interface", INTERFACE_TYPE_SIZE_) == 0) {
419 Laser1080Interface *ls1080if;
420 try {
421 logger->log_info(name(), "Opening %s:%s", type, id);
422 ls1080if = blackboard->open_for_reading<Laser1080Interface>(id);
423 } catch (Exception &e) {
424 // ignored
425 logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
426 return;
427 }
428
429 try {
430 bbil_add_data_interface(ls1080if);
431 bbil_add_reader_interface(ls1080if);
432 bbil_add_writer_interface(ls1080if);
433
434 std::string topname = topic_name(ls1080if->id(), "1080");
435
436 PublisherInfo pi;
437 pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
438
439 logger->log_info(name(),
440 "Publishing 1080 laser scan %s at %s",
441 ls1080if->uid(),
442 topname.c_str());
443
444 pi.msg.header.frame_id = ls1080if->frame();
445 pi.msg.angle_min = 0;
446 pi.msg.angle_max = 2 * M_PI;
447 pi.msg.angle_increment = deg2rad(0.5);
448 pi.msg.ranges.resize(1080);
449
450 pubs_[ls1080if->uid()] = pi;
451
452 blackboard->update_listener(this);
453 ls1080_ifs_.push_back(ls1080if);
454 } catch (Exception &e) {
455 blackboard->close(ls1080if);
456 logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what());
457 return;
458 }
459 }
460}
461
462void
464 fawkes::Uuid instance_serial) noexcept
465{
466 conditional_close(interface);
467}
468
469void
471 fawkes::Uuid instance_serial) noexcept
472{
473 conditional_close(interface);
474}
475
476void
477RosLaserScanThread::conditional_close(Interface *interface) noexcept
478{
479 // Verify it's a laser interface
480 Laser360Interface * ls360if = dynamic_cast<Laser360Interface *>(interface);
481 Laser720Interface * ls720if = dynamic_cast<Laser720Interface *>(interface);
482 Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
483
484 if (ls360if) {
485 std::list<Laser360Interface *>::iterator i;
486 for (i = ls360_ifs_.begin(); i != ls360_ifs_.end(); ++i) {
487 if (*ls360if == **i) {
488 if (!ls360if->has_writer() && (ls360if->num_readers() == 1)) {
489 // It's only us
490 logger->log_info(name(), "Last on %s, closing", ls360if->uid());
491 bbil_remove_data_interface(*i);
492 bbil_remove_reader_interface(*i);
493 bbil_remove_writer_interface(*i);
494 blackboard->update_listener(this);
495 blackboard->close(*i);
496 ls360_ifs_.erase(i);
497 break;
498 }
499 }
500 }
501 } else if (ls720if) {
502 std::list<Laser720Interface *>::iterator i;
503 for (i = ls720_ifs_.begin(); i != ls720_ifs_.end(); ++i) {
504 if (*ls720if == **i) {
505 if (!ls720if->has_writer() && (ls720if->num_readers() == 1)) {
506 // It's only us
507 logger->log_info(name(), "Last on %s, closing", ls720if->uid());
508 bbil_remove_data_interface(*i);
509 bbil_remove_reader_interface(*i);
510 bbil_remove_writer_interface(*i);
511 blackboard->update_listener(this);
512 blackboard->close(*i);
513 ls720_ifs_.erase(i);
514 break;
515 }
516 }
517 }
518
519 } else if (ls1080if) {
520 std::list<Laser1080Interface *>::iterator i;
521 for (i = ls1080_ifs_.begin(); i != ls1080_ifs_.end(); ++i) {
522 if (*ls1080if == **i) {
523 if (!ls1080if->has_writer() && (ls1080if->num_readers() == 1)) {
524 // It's only us
525 logger->log_info(name(), "Last on %s, closing", ls1080if->uid());
526 bbil_remove_data_interface(*i);
527 bbil_remove_reader_interface(*i);
528 bbil_remove_writer_interface(*i);
529 blackboard->update_listener(this);
530 blackboard->close(*i);
531 ls1080_ifs_.erase(i);
532 break;
533 }
534 }
535 }
536 }
537}
538
539/** Callback function for ROS laser scan message subscription.
540 * @param msg incoming message
541 */
542void
543RosLaserScanThread::laser_scan_message_cb(
544 const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt)
545{
546 MutexLocker lock(ls_msg_queue_mutex_);
547 ls_msg_queues_[active_queue_].push(msg_evt);
548}
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
virtual void finalize()
Finalize the thread.
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
virtual void loop()
Code to execute in the thread.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual ~RosLaserScanThread()
Destructor.
virtual void init()
Initialize the thread.
RosLaserScanThread()
Constructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*") noexcept
Add interface creation type to watch list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:240
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:197
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:225
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*", const char *owner=NULL)=0
Open multiple interfaces for reading.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:714
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:686
unsigned int num_readers() const
Get the number of readers.
Definition: interface.cpp:876
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Laser1080Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
void set_frame(const char *new_frame)
Set frame value.
char * frame() const
Get frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Laser720Interface Fawkes BlackBoard Interface.
char * frame() const
Get frame value.
float * distances() const
Get distances value.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:216
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
long get_sec() const
Get seconds.
Definition: time.h:117
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36