Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
89  std::list<Laser360Interface *>::iterator i360;
90  for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
91  (*i360)->read();
92  logger->log_info(name(), "Opened %s", (*i360)->uid());
96 
97  std::string topname = topic_name((*i360)->id(), "360");
98 
99  PublisherInfo pi;
100  pi.pub =
101  rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
102 
103  logger->log_info(name(), "Publishing laser scan %s at %s",
104  (*i360)->uid(), topname.c_str());
105 
106  pi.msg.header.frame_id = (*i360)->frame();
107  pi.msg.angle_min = 0;
108  pi.msg.angle_max = 2*M_PI;
109  pi.msg.angle_increment = deg2rad(1);
110  pi.msg.ranges.resize(360);
111 
112  __pubs[(*i360)->uid()] = pi;
113  }
114  std::list<Laser720Interface *>::iterator i720;
115  for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
116  logger->log_info(name(), "Opened %s", (*i720)->uid());
120 
121  std::string topname = topic_name((*i720)->id(), "720");
122 
123  PublisherInfo pi;
124  pi.pub =
125  rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
126 
127  logger->log_info(name(), "Publishing laser scan %s at %s",
128  (*i720)->uid(), topname.c_str());
129 
130  pi.msg.header.frame_id = (*i720)->frame();
131  pi.msg.angle_min = 0;
132  pi.msg.angle_max = 2*M_PI;
133  pi.msg.angle_increment = deg2rad(0.5);
134  pi.msg.ranges.resize(720);
135 
136  __pubs[(*i720)->uid()] = pi;
137  }
139 
140  bbio_add_observed_create("Laser360Interface", "*");
141  bbio_add_observed_create("Laser720Interface", "*");
143 }
144 
145 
146 void
148 {
151 
152  __sub_ls.shutdown();
153 
154  std::map<std::string, PublisherInfo>::iterator p;
155  for (p = __pubs.begin(); p != __pubs.end(); ++p) {
156  p->second.pub.shutdown();
157  }
158 
159  std::list<Laser360Interface *>::iterator i360;
160  for (i360 = __ls360_ifs.begin(); i360 != __ls360_ifs.end(); ++i360) {
161  blackboard->close(*i360);
162  }
163  __ls360_ifs.clear();
164  std::list<Laser720Interface *>::iterator i720;
165  for (i720 = __ls720_ifs.begin(); i720 != __ls720_ifs.end(); ++i720) {
166  blackboard->close(*i720);
167  }
168  __ls720_ifs.clear();
169 }
170 
171 
172 void
174 {
175  __ls_msg_queue_mutex->lock();
176  unsigned int queue = __active_queue;
177  __active_queue = 1 - __active_queue;
178  __ls_msg_queue_mutex->unlock();
179 
180  while (! __ls_msg_queues[queue].empty()) {
181  const sensor_msgs::LaserScan::ConstPtr &msg = __ls_msg_queues[queue].front();
182 
183  // Check if interface exists, open if it does not
184  std::map<std::string, std::string> *msg_header_map =
185  msg->__connection_header.get();
186  std::map<std::string, std::string>::iterator it =
187  msg_header_map->find("callerid");
188  const std::string &callerid = it->second;
189 
190  // for now we only create 360 interfaces, might add on that later
191  if (it == msg_header_map->end()) {
192  logger->log_warn(name(), "Received laser scan from ROS without caller ID,"
193  "ignoring");
194  } else {
195  bool have_interface = true;
196  if (__ls360_wifs.find(callerid) == __ls360_wifs.end()) {
197  try {
198  std::string id = std::string("ROS Laser ") + callerid;
199  Laser360Interface *ls360if =
201  __ls360_wifs[callerid] = ls360if;
202  } catch (Exception &e) {
203  logger->log_warn(name(), "Failed to open ROS laser interface for "
204  "message from node %s, exception follows",
205  callerid.c_str());
206  logger->log_warn(name(), e);
207  have_interface = false;
208  }
209  }
210 
211  if (have_interface) {
212  // update interface with laser data
213  Laser360Interface *ls360if = __ls360_wifs[it->second];
214  ls360if->set_frame(msg->header.frame_id.c_str());
215  float distances[360];
216  for (unsigned int a = 0; a < 360; ++a) {
217  float a_rad = deg2rad(a);
218  if ((a_rad < msg->angle_min) || (a_rad > msg->angle_max)) {
219  distances[a] = 0.;
220  } else {
221  // get closest ray from message
222  int idx =
223  (int)roundf((a_rad - msg->angle_min) / msg->angle_increment);
224  distances[a] = msg->ranges[idx];
225  }
226  }
227  ls360if->set_distances(distances);
228  ls360if->write();
229  }
230  }
231 
232  __ls_msg_queues[queue].pop();
233  }
234 }
235 
236 
237 void
239 {
240  Laser360Interface *ls360if = dynamic_cast<Laser360Interface *>(interface);
241  Laser720Interface *ls720if = dynamic_cast<Laser720Interface *>(interface);
242 
243  PublisherInfo &pi = __pubs[interface->uid()];
244  sensor_msgs::LaserScan &msg = pi.msg;
245 
246  if (ls360if) {
247  ls360if->read();
248 
249  const Time *time = ls360if->timestamp();
250 
251  __seq_num_mutex->lock();
252  msg.header.seq = ++__seq_num;
253  __seq_num_mutex->unlock();
254  msg.header.stamp = ros::Time(time->get_sec(), time->get_nsec());
255  msg.header.frame_id = ls360if->frame();
256 
257  msg.angle_min = 0;
258  msg.angle_max = 2*M_PI;
259  msg.angle_increment = deg2rad(1);
260  msg.range_min = 0.;
261  msg.range_max = 1000.;
262  msg.ranges.resize(360);
263  memcpy(&msg.ranges[0], ls360if->distances(), 360*sizeof(float));
264 
265  pi.pub.publish(pi.msg);
266 
267  } else if (ls720if) {
268  ls720if->read();
269 
270  const Time *time = ls720if->timestamp();
271 
272  sensor_msgs::LaserScan msg;
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 = ls720if->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(720);
285  memcpy(&msg.ranges[0], ls720if->distances(), 720*sizeof(float));
286 
287  pi.pub.publish(pi.msg);
288  }
289 
290 }
291 
292 
293 void
294 RosLaserScanThread::bb_interface_created(const char *type, const char *id) throw()
295 {
296  // Ignore ID pattern of our own interfaces
297  if (fnmatch("ROS *", id, FNM_NOESCAPE) == 0) return;
298 
299  if (strncmp(type, "Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
300 
301  Laser360Interface *ls360if;
302  try {
303  logger->log_info(name(), "Opening %s:%s", type, id);
304  ls360if = blackboard->open_for_reading<Laser360Interface>(id);
305  } catch (Exception &e) {
306  // ignored
307  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
308  return;
309  }
310 
311  try {
312  bbil_add_data_interface(ls360if);
313  bbil_add_reader_interface(ls360if);
314  bbil_add_writer_interface(ls360if);
315 
316  std::string topname = topic_name(ls360if->id(), "360");
317 
318  PublisherInfo pi;
319  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
320 
321  logger->log_info(name(), "Publishing laser scan %s at %s",
322  ls360if->uid(), topname.c_str());
323 
324  pi.msg.header.frame_id = ls360if->frame();
325  pi.msg.angle_min = 0;
326  pi.msg.angle_max = 2*M_PI;
327  pi.msg.angle_increment = deg2rad(1);
328  pi.msg.ranges.resize(360);
329 
330  __pubs[ls360if->uid()] = pi;
331 
332  blackboard->update_listener(this);
333  __ls360_ifs.push_back(ls360if);
334  } catch (Exception &e) {
335  blackboard->close(ls360if);
336  logger->log_warn(name(), "Failed to register for %s:%s: %s",
337  type, id, e.what());
338  return;
339  }
340  } else if (strncmp(type, "Laser720Interface", __INTERFACE_TYPE_SIZE) == 0) {
341 
342  Laser720Interface *ls720if;
343  try {
344  logger->log_info(name(), "Opening %s:%s", type, id);
345  ls720if = blackboard->open_for_reading<Laser720Interface>(id);
346  } catch (Exception &e) {
347  // ignored
348  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
349  return;
350  }
351 
352  try {
353  bbil_add_data_interface(ls720if);
354  bbil_add_reader_interface(ls720if);
355  bbil_add_writer_interface(ls720if);
356 
357  std::string topname = topic_name(ls720if->id(), "720");
358 
359  PublisherInfo pi;
360  pi.pub = rosnode->advertise<sensor_msgs::LaserScan>(topname, 1);
361 
362  logger->log_info(name(), "Publishing laser scan %s at %s",
363  ls720if->uid(), topname.c_str());
364 
365  pi.msg.header.frame_id = ls720if->frame();
366  pi.msg.angle_min = 0;
367  pi.msg.angle_max = 2*M_PI;
368  pi.msg.angle_increment = deg2rad(0.5);
369  pi.msg.ranges.resize(720);
370 
371  __pubs[ls720if->uid()] = pi;
372 
373  blackboard->update_listener(this);
374  __ls720_ifs.push_back(ls720if);
375  } catch (Exception &e) {
376  blackboard->close(ls720if);
377  logger->log_warn(name(), "Failed to register for %s:%s: %s",
378  type, id, e.what());
379  return;
380  }
381  }
382 }
383 
384 void
386  unsigned int instance_serial) throw()
387 {
388  conditional_close(interface);
389 }
390 
391 void
393  unsigned int instance_serial) throw()
394 {
395  conditional_close(interface);
396 }
397 
398 void
399 RosLaserScanThread::conditional_close(Interface *interface) throw()
400 {
401  // Verify it's a laser interface
402  Laser360Interface *ls360if = dynamic_cast<Laser360Interface *>(interface);
403  Laser720Interface *ls720if = dynamic_cast<Laser720Interface *>(interface);
404 
405  if (ls360if) {
406  std::list<Laser360Interface *>::iterator i;
407  for (i = __ls360_ifs.begin(); i != __ls360_ifs.end(); ++i) {
408  if (*ls360if == **i) {
409  if (! ls360if->has_writer() && (ls360if->num_readers() == 1)) {
410  // It's only us
411  logger->log_info(name(), "Last on %s, closing", ls360if->uid());
412  bbil_remove_data_interface(*i);
413  bbil_remove_reader_interface(*i);
414  bbil_remove_writer_interface(*i);
415  blackboard->update_listener(this);
416  blackboard->close(*i);
417  __ls360_ifs.erase(i);
418  break;
419  }
420  }
421  }
422  } else if (ls720if) {
423  std::list<Laser720Interface *>::iterator i;
424  for (i = __ls720_ifs.begin(); i != __ls720_ifs.end(); ++i) {
425  if (*ls720if == **i) {
426  if (! ls720if->has_writer() && (ls720if->num_readers() == 1)) {
427  // It's only us
428  logger->log_info(name(), "Last on %s, closing", ls720if->uid());
429  bbil_remove_data_interface(*i);
430  bbil_remove_reader_interface(*i);
431  bbil_remove_writer_interface(*i);
432  blackboard->update_listener(this);
433  blackboard->close(*i);
434  __ls720_ifs.erase(i);
435  break;
436  }
437  }
438  }
439  }
440 }
441 
442 
443 /** Callback function for ROS laser scan message subscription.
444  * @param msg incoming message
445  */
446 void
447 RosLaserScanThread::laser_scan_message_cb(const sensor_msgs::LaserScan::ConstPtr &msg)
448 {
449  MutexLocker lock(__ls_msg_queue_mutex);
450 
451  std::map<std::string, std::string> *msg_header_map =
452  msg->__connection_header.get();
453  std::map<std::string, std::string>::iterator it =
454  msg_header_map->find("callerid");
455 
456  if (it == msg_header_map->end()) {
457  logger->log_warn(name(), "Message received without callerid");
458  } else if (it->second != ros::this_node::getName()) {
459  __ls_msg_queues[__active_queue].push(msg);
460  }
461 }