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 <utils/math/angle.h>
26 
27 #include <ros/this_node.h>
28 #include <sensor_msgs/LaserScan.h>
29 
30 #include <fnmatch.h>
31 
32 using namespace fawkes;
33 
34 /** @class RosLaserScanThread "pcl_thread.h"
35  * Thread to exchange point clouds between Fawkes and ROS.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor. */
41  : Thread("RosLaserScanThread", Thread::OPMODE_WAITFORWAKEUP),
42  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
43  BlackBoardInterfaceListener("RosLaserScanThread")
44 {
45  __ls_msg_queue_mutex = new Mutex();
46  __seq_num_mutex = new Mutex();
47 }
48 
49 /** Destructor. */
51 {
52  delete __ls_msg_queue_mutex;
53  delete __seq_num_mutex;
54 }
55 
56 
57 std::string
58 RosLaserScanThread::topic_name(const char *if_id, const char *suffix)
59 {
60  std::string topic_name = std::string("fawkes_scans/") + if_id + "_" + suffix;
61  std::string::size_type pos = 0;
62  while ((pos = topic_name.find("-", pos)) != std::string::npos) {
63  topic_name.replace(pos, 1, "_");
64  }
65  pos = 0;
66  while ((pos = topic_name.find(" ", pos)) != std::string::npos) {
67  topic_name.replace(pos, 1, "_");
68  }
69  return topic_name;
70 }
71 
72 
73 void
75 {
76  __active_queue = 0;
77  __seq_num = 0;
78 
79  // Must do that before registering listener because we might already
80  // get events right away
81  __sub_ls = rosnode->subscribe("scan", 100,
82  &RosLaserScanThread::laser_scan_message_cb, this);
83 
84  __ls360_ifs =
86  __ls720_ifs =
88  __ls1080_ifs =
90 
91  std::list<Laser360Interface *>::iterator i360;
92  for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
93  (*i360)->read();
94  logger->log_info(name(), "Opened %s", (*i360)->uid());
98 
99  std::string topname = topic_name((*i360)->id(), "360");
100 
101  PublisherInfo pi;
102  pi.pub =
103  rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
104 
105  logger->log_info(name(), "Publishing laser scan %s at %s",
106  (*i360)->uid(), topname.c_str());
107 
108  pi.msg.header.frame_id = (*i360)->frame();
109  pi.msg.angle_min = 0;
110  pi.msg.angle_max = 2*M_PI;
111  pi.msg.angle_increment = deg2rad(1);
112  pi.msg.ranges.resize(360);
113 
114  __pubs[(*i360)->uid()] = pi;
115  }
116 
117  std::list<Laser720Interface *>::iterator i720;
118  for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
119  logger->log_info(name(), "Opened %s", (*i720)->uid());
123 
124  std::string topname = topic_name((*i720)->id(), "720");
125 
126  PublisherInfo pi;
127  pi.pub =
128  rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
129 
130  logger->log_info(name(), "Publishing laser scan %s at %s",
131  (*i720)->uid(), topname.c_str());
132 
133  pi.msg.header.frame_id = (*i720)->frame();
134  pi.msg.angle_min = 0;
135  pi.msg.angle_max = 2*M_PI;
136  pi.msg.angle_increment = deg2rad(0.5);
137  pi.msg.ranges.resize(720);
138 
139  __pubs[(*i720)->uid()] = pi;
140  }
141 
142  std::list<Laser1080Interface *>::iterator i1080;
143  for (i1080 = __ls1080_ifs.begin(); i1080 != __ls1080_ifs.end(); ++i1080) {
144  logger->log_info(name(), "Opened %s", (*i1080)->uid());
145  bbil_add_data_interface(*i1080);
148 
149  std::string topname = topic_name((*i1080)->id(), "1080");
150 
151  PublisherInfo pi;
152  pi.pub =
153  rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
154 
155  logger->log_info(name(), "Publishing laser scan %s at %s, frame %s",
156  (*i1080)->uid(), topname.c_str(), (*i1080)->frame());
157 
158  pi.msg.header.frame_id = (*i1080)->frame();
159  pi.msg.angle_min = 0;
160  pi.msg.angle_max = 2*M_PI;
161  pi.msg.angle_increment = deg2rad(1. / 3.);
162  pi.msg.ranges.resize(1080);
163 
164  __pubs[(*i1080)->uid()] = pi;
165  }
166 
168 
169  bbio_add_observed_create("Laser360Interface", "*");
170  bbio_add_observed_create("Laser720Interface", "*");
171  bbio_add_observed_create("Laser1080Interface", "*");
173 }
174 
175 
176 void
178 {
181 
182  __sub_ls.shutdown();
183 
184  std::map<std::string, PublisherInfo>::iterator p;
185  for (p = __pubs.begin(); p != __pubs.end(); ++p) {
186  p->second.pub.shutdown();
187  }
188 
189  std::list<Laser360Interface *>::iterator i360;
190  for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
191  blackboard->close(*i360);
192  }
193  __ls360_ifs.clear();
194  std::list<Laser720Interface *>::iterator i720;
195  for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
196  blackboard->close(*i720);
197  }
198  __ls720_ifs.clear();
199  std::list<Laser1080Interface *>::iterator i1080;
200  for (i1080 = __ls1080_ifs.begin(); i1080 != __ls1080_ifs.end(); ++i1080) {
201  blackboard->close(*i1080);
202  }
203  __ls1080_ifs.clear();
204 }
205 
206 
207 void
209 {
210  __ls_msg_queue_mutex->lock();
211  unsigned int queue = __active_queue;
212  __active_queue = 1 - __active_queue;
213  __ls_msg_queue_mutex->unlock();
214 
215  while (! __ls_msg_queues[queue].empty()) {
216  const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt =
217  __ls_msg_queues[queue].front();
218 
219  sensor_msgs::LaserScan::ConstPtr msg = msg_evt.getConstMessage();
220 
221  // Check if interface exists, open if it does not
222  const std::string callerid = msg_evt.getPublisherName();
223 
224  // for now we only create 360 interfaces, might add on that later
225  if (callerid.empty()) {
226  logger->log_warn(name(), "Received laser scan from ROS without caller ID,"
227  "ignoring");
228  } else {
229  bool have_interface = true;
230  if (__ls360_wifs.find(callerid) == __ls360_wifs.end()) {
231  try {
232  std::string id = std::string("ROS Laser ") + callerid;
233  Laser360Interface *ls360if =
235  __ls360_wifs[callerid] = ls360if;
236  } catch (Exception &e) {
237  logger->log_warn(name(), "Failed to open ROS laser interface for "
238  "message from node %s, exception follows",
239  callerid.c_str());
240  logger->log_warn(name(), e);
241  have_interface = false;
242  }
243  }
244 
245  if (have_interface) {
246  // update interface with laser data
247  Laser360Interface *ls360if = __ls360_wifs[callerid];
248  ls360if->set_frame(msg->header.frame_id.c_str());
249  float distances[360];
250  for (unsigned int a = 0; a < 360; ++a) {
251  float a_rad = deg2rad(a);
252  if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
253  distances[a] = 0.;
254  } else {
255  // get closest ray from message
256  int idx =
257  (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
258  distances[a] = msg->ranges[idx];
259  }
260  }
261  ls360if->set_distances(distances);
262  ls360if->write();
263  }
264  }
265 
266  __ls_msg_queues[queue].pop();
267  }
268 }
269 
270 
271 void
273 {
274  Laser360Interface *ls360if = dynamic_cast<Laser360Interface *>(interface);
275  Laser720Interface *ls720if = dynamic_cast<Laser720Interface *>(interface);
276  Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
277 
278  PublisherInfo &pi = __pubs[interface->uid()];
279  sensor_msgs::LaserScan &msg = pi.msg;
280 
281  if (ls360if) {
282  ls360if->read();
283 
284  const Time *time = ls360if->timestamp();
285 
286  __seq_num_mutex->lock();
287  msg.header.seq = ++__seq_num;
288  __seq_num_mutex->unlock();
289  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
290  msg.header.frame_id = ls360if->frame();
291 
292  msg.angle_min = 0;
293  msg.angle_max = 2*M_PI;
294  msg.angle_increment = deg2rad(1);
295  msg.range_min = 0.;
296  msg.range_max = 1000.;
297  msg.ranges.resize(360);
298  memcpy(&msg.ranges[0], ls360if->distances(), 360*sizeof(float));
299 
300  pi.pub.publish(pi.msg);
301 
302  } else if (ls720if) {
303  ls720if->read();
304 
305  const Time *time = ls720if->timestamp();
306 
307  __seq_num_mutex->lock();
308  msg.header.seq = ++__seq_num;
309  __seq_num_mutex->unlock();
310  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
311  msg.header.frame_id = ls720if->frame();
312 
313  msg.angle_min = 0;
314  msg.angle_max = 2*M_PI;
315  msg.angle_increment = deg2rad(1./2.);
316  msg.range_min = 0.;
317  msg.range_max = 1000.;
318  msg.ranges.resize(720);
319  memcpy(&msg.ranges[0], ls720if->distances(), 720*sizeof(float));
320 
321  pi.pub.publish(pi.msg);
322 
323  } else if (ls1080if) {
324  ls1080if->read();
325 
326  const Time *time = ls1080if->timestamp();
327 
328  __seq_num_mutex->lock();
329  msg.header.seq = ++__seq_num;
330  __seq_num_mutex->unlock();
331  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
332  msg.header.frame_id = ls1080if->frame();
333 
334  msg.angle_min = 0;
335  msg.angle_max = 2*M_PI;
336  msg.angle_increment = deg2rad(1./3.);
337  msg.range_min = 0.;
338  msg.range_max = 1000.;
339  msg.ranges.resize(1080);
340  memcpy(&msg.ranges[0], ls1080if->distances(), 1080*sizeof(float));
341 
342  pi.pub.publish(pi.msg);
343  }
344 
345 }
346 
347 
348 void
349 RosLaserScanThread::bb_interface_created(const char *type, const char *id) throw()
350 {
351  // Ignore ID pattern of our own interfaces
352  if (fnmatch("ROS *", id, FNM_NOESCAPE) == 0) return;
353 
354  if (strncmp(type, "Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
355 
356  Laser360Interface *ls360if;
357  try {
358  logger->log_info(name(), "Opening %s:%s", type, id);
360  } catch (Exception &e) {
361  // ignored
362  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
363  return;
364  }
365 
366  try {
367  bbil_add_data_interface(ls360if);
368  bbil_add_reader_interface(ls360if);
369  bbil_add_writer_interface(ls360if);
370 
371  std::string topname = topic_name(ls360if->id(), "360");
372 
373  PublisherInfo pi;
374  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
375 
376  logger->log_info(name(), "Publishing laser scan %s at %s",
377  ls360if->uid(), topname.c_str());
378 
379  pi.msg.header.frame_id = ls360if->frame();
380  pi.msg.angle_min = 0;
381  pi.msg.angle_max = 2*M_PI;
382  pi.msg.angle_increment = deg2rad(1);
383  pi.msg.ranges.resize(360);
384 
385  __pubs[ls360if->uid()] = pi;
386 
388  __ls360_ifs.push_back(ls360if);
389  } catch (Exception &e) {
390  blackboard->close(ls360if);
391  logger->log_warn(name(), "Failed to register for %s:%s: %s",
392  type, id, e.what());
393  return;
394  }
395 
396  } else if (strncmp(type, "Laser720Interface", __INTERFACE_TYPE_SIZE) == 0) {
397  Laser720Interface *ls720if;
398  try {
399  logger->log_info(name(), "Opening %s:%s", type, id);
401  } catch (Exception &e) {
402  // ignored
403  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
404  return;
405  }
406 
407  try {
408  bbil_add_data_interface(ls720if);
409  bbil_add_reader_interface(ls720if);
410  bbil_add_writer_interface(ls720if);
411 
412  std::string topname = topic_name(ls720if->id(), "720");
413 
414  PublisherInfo pi;
415  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
416 
417  logger->log_info(name(), "Publishing laser scan %s at %s",
418  ls720if->uid(), topname.c_str());
419 
420  pi.msg.header.frame_id = ls720if->frame();
421  pi.msg.angle_min = 0;
422  pi.msg.angle_max = 2*M_PI;
423  pi.msg.angle_increment = deg2rad(0.5);
424  pi.msg.ranges.resize(720);
425 
426  __pubs[ls720if->uid()] = pi;
427 
429  __ls720_ifs.push_back(ls720if);
430  } catch (Exception &e) {
431  blackboard->close(ls720if);
432  logger->log_warn(name(), "Failed to register for %s:%s: %s",
433  type, id, e.what());
434  return;
435  }
436 
437  } else if (strncmp(type, "Laser1080Interface", __INTERFACE_TYPE_SIZE) == 0) {
438  Laser1080Interface *ls1080if;
439  try {
440  logger->log_info(name(), "Opening %s:%s", type, id);
442  } catch (Exception &e) {
443  // ignored
444  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
445  return;
446  }
447 
448  try {
449  bbil_add_data_interface(ls1080if);
450  bbil_add_reader_interface(ls1080if);
451  bbil_add_writer_interface(ls1080if);
452 
453  std::string topname = topic_name(ls1080if->id(), "1080");
454 
455  PublisherInfo pi;
456  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
457 
458  logger->log_info(name(), "Publishing 1080 laser scan %s at %s",
459  ls1080if->uid(), topname.c_str());
460 
461  pi.msg.header.frame_id = ls1080if->frame();
462  pi.msg.angle_min = 0;
463  pi.msg.angle_max = 2*M_PI;
464  pi.msg.angle_increment = deg2rad(0.5);
465  pi.msg.ranges.resize(1080);
466 
467  __pubs[ls1080if->uid()] = pi;
468 
470  __ls1080_ifs.push_back(ls1080if);
471  } catch (Exception &e) {
472  blackboard->close(ls1080if);
473  logger->log_warn(name(), "Failed to register for %s:%s: %s",
474  type, id, e.what());
475  return;
476  }
477  }
478 }
479 
480 void
482  unsigned int instance_serial) throw()
483 {
484  conditional_close(interface);
485 }
486 
487 void
489  unsigned int instance_serial) throw()
490 {
491  conditional_close(interface);
492 }
493 
494 void
495 RosLaserScanThread::conditional_close(Interface *interface) throw()
496 {
497  // Verify it's a laser interface
498  Laser360Interface *ls360if = dynamic_cast<Laser360Interface *>(interface);
499  Laser720Interface *ls720if = dynamic_cast<Laser720Interface *>(interface);
500  Laser1080Interface *ls1080if = dynamic_cast<Laser1080Interface *>(interface);
501 
502  if (ls360if) {
503  std::list<Laser360Interface *>::iterator i;
504  for (i = __ls360_ifs.begin(); i != __ls360_ifs.end(); ++i) {
505  if (*ls360if == **i) {
506  if (! ls360if->has_writer() && (ls360if->num_readers() == 1)) {
507  // It's only us
508  logger->log_info(name(), "Last on %s, closing", ls360if->uid());
513  blackboard->close(*i);
514  __ls360_ifs.erase(i);
515  break;
516  }
517  }
518  }
519  } else if (ls720if) {
520  std::list<Laser720Interface *>::iterator i;
521  for (i = __ls720_ifs.begin(); i != __ls720_ifs.end(); ++i) {
522  if (*ls720if == **i) {
523  if (! ls720if->has_writer() && (ls720if->num_readers() == 1)) {
524  // It's only us
525  logger->log_info(name(), "Last on %s, closing", ls720if->uid());
530  blackboard->close(*i);
531  __ls720_ifs.erase(i);
532  break;
533  }
534  }
535  }
536 
537  } else if (ls1080if) {
538  std::list<Laser1080Interface *>::iterator i;
539  for (i = __ls1080_ifs.begin(); i != __ls1080_ifs.end(); ++i) {
540  if (*ls1080if == **i) {
541  if (! ls1080if->has_writer() && (ls1080if->num_readers() == 1)) {
542  // It's only us
543  logger->log_info(name(), "Last on %s, closing", ls1080if->uid());
548  blackboard->close(*i);
549  __ls1080_ifs.erase(i);
550  break;
551  }
552  }
553  }
554  }
555 }
556 
557 
558 /** Callback function for ROS laser scan message subscription.
559  * @param msg incoming message
560  */
561 void
562 RosLaserScanThread::laser_scan_message_cb(const ros::MessageEvent<sensor_msgs::LaserScan const> &msg_evt)
563 {
564  MutexLocker lock(__ls_msg_queue_mutex);
565  __ls_msg_queues[__active_queue].push(msg_evt);
566 }
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:230
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Laser1080Interface Fawkes BlackBoard Interface.
Fawkes library namespace.
void unlock()
Unlock the mutex.
Definition: mutex.cpp:135
Mutex locking helper.
Definition: mutex_locker.h:33
void bbil_add_writer_interface(Interface *interface)
Add an interface to the writer addition/removal watch list.
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
virtual void finalize()
Finalize the thread.
A class for handling time.
Definition: time.h:91
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Definition: blackboard.cpp:203
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void bbil_remove_writer_interface(Interface *interface)
Remove an interface to the writer addition/removal watch list.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual void loop()
Code to execute in the thread.
char * frame() const
Get frame value.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
char * frame() const
Get frame value.
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:244
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
const char * name() const
Get name of thread.
Definition: thread.h:95
const char * uid() const
Get unique identifier of interface.
Definition: interface.cpp:687
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
float * distances() const
Get distances value.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:718
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.
void bbil_remove_data_interface(Interface *interface)
Remove an interface to the data modification watch list.
long get_sec() const
Get seconds.
Definition: time.h:110
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual ~RosLaserScanThread()
Destructor.
virtual void init()
Initialize the thread.
float * distances() const
Get distances value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
RosLaserScanThread()
Constructor.
void lock()
Lock this mutex.
Definition: mutex.cpp:89
unsigned int num_readers() const
Get the number of readers.
Definition: interface.cpp:863
Mutex mutual exclusion lock.
Definition: mutex.h:32
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
char * frame() const
Get frame value.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
Laser720Interface Fawkes BlackBoard Interface.
void bbil_remove_reader_interface(Interface *interface)
Remove an interface to the reader addition/removal watch list.
BlackBoard interface listener.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.