Fawkes API  Fawkes Development Version
laser_pointcloud_thread.cpp
1 
2 /***************************************************************************
3  * laser_pointcloud_thread.cpp - Convert laser data to pointclouds
4  *
5  * Created: Thu Nov 17 10:21:55 2011
6  * Copyright 2011 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 "laser_pointcloud_thread.h"
23 
24 #include <core/threading/mutex_locker.h>
25 #include <utils/math/angle.h>
26 #include <pcl_utils/utils.h>
27 
28 #include <interfaces/Laser360Interface.h>
29 #include <interfaces/Laser720Interface.h>
30 #include <interfaces/Laser1080Interface.h>
31 
32 using namespace fawkes;
33 
34 /** @class LaserPointCloudThread "tf_thread.h"
35  * Thread to exchange transforms between Fawkes and ROS.
36  * This threads connects to Fawkes and ROS to read and write transforms.
37  * Transforms received on one end are republished to the other side. To
38  * Fawkes new frames are published during the sensor hook.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor. */
44  : Thread("LaserPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE),
46  BlackBoardInterfaceListener("LaserPointCloudThread")
47 {
48 }
49 
50 /** Destructor. */
52 {
53 }
54 
55 
56 void
58 {
59  std::list<Laser360Interface *> l360ifs =
61 
62  std::list<Laser720Interface *> l720ifs =
64 
65  std::list<Laser1080Interface *> l1080ifs =
67 
69  for (i = l360ifs.begin(); i != l360ifs.end(); ++i) {
70  InterfaceCloudMapping mapping;
71  mapping.id = interface_to_pcl_name((*i)->id());
72  mapping.size = 360;
73  mapping.interface_typed.as360 = *i;
74  mapping.interface = *i;
75  mapping.interface->read();
76  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
77  mapping.cloud->points.resize(360);
78  mapping.cloud->header.frame_id = (*i)->frame();
79  mapping.cloud->height = 1;
80  mapping.cloud->width = 360;
81  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
84  __mappings.push_back(mapping);
85  }
87  for (j = l720ifs.begin(); j != l720ifs.end(); ++j) {
88  InterfaceCloudMapping mapping;
89  mapping.id = interface_to_pcl_name((*j)->id());
90  mapping.size = 720;
91  mapping.interface_typed.as720 = *j;
92  mapping.interface = *j;
93  mapping.interface->read();
94  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
95  mapping.cloud->points.resize(720);
96  mapping.cloud->header.frame_id = (*j)->frame();
97  mapping.cloud->height = 1;
98  mapping.cloud->width = 720;
99  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
102  __mappings.push_back(mapping);
103  }
104 
106  for (k = l1080ifs.begin(); k != l1080ifs.end(); ++k) {
107  InterfaceCloudMapping mapping;
108  mapping.id = interface_to_pcl_name((*k)->id());
109  mapping.size = 1080;
110  mapping.interface_typed.as1080 = *k;
111  mapping.interface = *k;
112  mapping.interface->read();
113  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
114  mapping.cloud->points.resize(1080);
115  mapping.cloud->header.frame_id = (*k)->frame();
116  mapping.cloud->height = 1;
117  mapping.cloud->width = 1080;
118  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
121  __mappings.push_back(mapping);
122  }
123 
125 
126  bbio_add_observed_create("Laser360Interface", "*");
127  bbio_add_observed_create("Laser720Interface", "*");
128  bbio_add_observed_create("Laser1080Interface", "*");
130 
131  // Generate lookup tables for sin and cos
132  for (unsigned int i = 0; i < 360; ++i) {
133  sin_angles360[i] = sinf(deg2rad(i));
134  cos_angles360[i] = cosf(deg2rad(i));
135  }
136  for (unsigned int i = 0; i < 720; ++i) {
137  sin_angles720[i] = sinf(deg2rad((float)i / 2.));
138  cos_angles720[i] = cosf(deg2rad((float)i / 2.));
139  }
140  for (unsigned int i = 0; i < 1080; ++i) {
141  sin_angles1080[i] = sinf(deg2rad((float)i / 3.));
142  cos_angles1080[i] = cosf(deg2rad((float)i / 3.));
143  }
144 }
145 
146 
147 void
149 {
152 
154  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
155  blackboard->close(m->interface);
156  pcl_manager->remove_pointcloud(m->id.c_str());
157  }
158  __mappings.clear();
159 }
160 
161 
162 void
164 {
165  MutexLocker lock(__mappings.mutex());
166 
168  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
169  m->interface->read();
170  if (! m->interface->changed()) {
171  continue;
172  }
173  if (m->size == 360) {
174  m->cloud->header.frame_id = m->interface_typed.as360->frame();
175  float *distances = m->interface_typed.as360->distances();
176  for (unsigned int i = 0; i < 360; ++i) {
177  m->cloud->points[i].x = distances[i] * cos_angles360[i];
178  m->cloud->points[i].y = distances[i] * sin_angles360[i];
179  }
180 
181  } else if (m->size == 720) {
182  m->cloud->header.frame_id = m->interface_typed.as720->frame();
183  float *distances = m->interface_typed.as720->distances();
184  for (unsigned int i = 0; i < 720; ++i) {
185  m->cloud->points[i].x = distances[i] * cos_angles720[i];
186  m->cloud->points[i].y = distances[i] * sin_angles720[i];
187  }
188 
189  } else if (m->size == 1080) {
190  m->cloud->header.frame_id = m->interface_typed.as1080->frame();
191  float *distances = m->interface_typed.as1080->distances();
192  for (unsigned int i = 0; i < 1080; ++i) {
193  m->cloud->points[i].x = distances[i] * cos_angles1080[i];
194  m->cloud->points[i].y = distances[i] * sin_angles1080[i];
195  }
196  }
197 
198  pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
199  }
200 }
201 
202 
203 void
204 LaserPointCloudThread::bb_interface_created(const char *type, const char *id) throw()
205 {
206  InterfaceCloudMapping mapping;
207  mapping.id = interface_to_pcl_name(id);
208  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
209  mapping.cloud->height = 1;
210 
211  if (strncmp(type, "Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
212  Laser360Interface *lif;
213  try {
215  } catch (Exception &e) {
216  // ignored
217  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
218  return;
219  }
220 
221  try {
222  mapping.size = 360;
223  mapping.interface_typed.as360 = lif;
224  mapping.interface = lif;
225  mapping.cloud->points.resize(360);
226  mapping.cloud->header.frame_id = lif->frame();
227  mapping.cloud->width = 360;
228  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
229  } catch (Exception &e) {
230  logger->log_warn(name(), "Failed to add pointcloud %s: %s",
231  mapping.id.c_str(), e.what());
232  blackboard->close(lif);
233  return;
234  }
235 
236  } else if (strncmp(type, "Laser720Interface", __INTERFACE_TYPE_SIZE) != 0) {
237  Laser720Interface *lif;
238  try {
240  } catch (Exception &e) {
241  // ignored
242  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
243  return;
244  }
245 
246  try {
247  mapping.size = 720;
248  mapping.interface_typed.as720 = lif;
249  mapping.interface = lif;
250  mapping.cloud->points.resize(720);
251  mapping.cloud->header.frame_id = lif->frame();
252  mapping.cloud->width = 720;
253  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
254  } catch (Exception &e) {
255  logger->log_warn(name(), "Failed to add pointcloud %s: %s",
256  mapping.id.c_str(), e.what());
257  blackboard->close(lif);
258  return;
259  }
260 
261  } else if (strncmp(type, "Laser1080Interface", __INTERFACE_TYPE_SIZE) != 0) {
262  Laser1080Interface *lif;
263  try {
265  } catch (Exception &e) {
266  // ignored
267  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
268  return;
269  }
270 
271  try {
272  mapping.size = 1080;
273  mapping.interface_typed.as1080 = lif;
274  mapping.interface = lif;
275  mapping.cloud->points.resize(1080);
276  mapping.cloud->header.frame_id = lif->frame();
277  mapping.cloud->width = 1080;
278  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
279  } catch (Exception &e) {
280  logger->log_warn(name(), "Failed to add pointcloud %s: %s",
281  mapping.id.c_str(), e.what());
282  blackboard->close(lif);
283  return;
284  }
285  }
286 
287  try {
288  bbil_add_data_interface(mapping.interface);
290  } catch (Exception &e) {
291  logger->log_warn(name(), "Failed to register for %s:%s: %s",
292  type, id, e.what());
293  try {
294  bbil_remove_data_interface(mapping.interface);
296  blackboard->close(mapping.interface);
297  pcl_manager->remove_pointcloud(mapping.id.c_str());
298  } catch (Exception &e) {
299  logger->log_error(name(), "Failed to deregister %s:%s during error recovery: %s",
300  type, id, e.what());
301 
302  }
303  return;
304  }
305 
306  __mappings.push_back(mapping);
307 }
308 
309 void
311  unsigned int instance_serial) throw()
312 {
313  conditional_close(interface);
314 }
315 
316 void
318  unsigned int instance_serial) throw()
319 {
320  conditional_close(interface);
321 }
322 
323 void
324 LaserPointCloudThread::conditional_close(Interface *interface) throw()
325 {
326  Laser360Interface *l360if = dynamic_cast<Laser360Interface *>(interface);
327  Laser720Interface *l720if = dynamic_cast<Laser720Interface *>(interface);
328  Laser1080Interface *l1080if = dynamic_cast<Laser1080Interface *>(interface);
329 
330  bool close = false;
331  InterfaceCloudMapping mapping;
332 
333  MutexLocker lock(__mappings.mutex());
334 
336  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
337  bool match = ((m->size == 360 && l360if && (*l360if == *m->interface_typed.as360)) ||
338  (m->size == 720 && l720if && (*l720if == *m->interface_typed.as720)) ||
339  (m->size == 1080 && l1080if && (*l1080if == *m->interface_typed.as1080)));
340 
341 
342  if (match) {
343  if (! m->interface->has_writer() && (m->interface->num_readers() == 1)) {
344  // It's only us
345  logger->log_info(name(), "Last on %s, closing", m->interface->uid());
346  close = true;
347  mapping = *m;
348  __mappings.erase(m);
349  break;
350  }
351  }
352  }
353 
354  lock.unlock();
355 
356  if (close) {
357  std::string uid = mapping.interface->uid();
358  try {
359  bbil_remove_data_interface(mapping.interface);
361  blackboard->close(mapping.interface);
362  pcl_manager->remove_pointcloud(mapping.id.c_str());
363  } catch (Exception &e) {
364  logger->log_error(name(), "Failed to unregister or close %s: %s",
365  uid.c_str(), e.what());
366  }
367  }
368 }
369 
370 
371 std::string
372 LaserPointCloudThread::interface_to_pcl_name(const char *interface_id)
373 {
374  std::string rv = interface_id;
375  if (rv.find("Laser ") == 0) {
376  // starts with "Laser ", remove it
377  rv = rv.substr(strlen("Laser "));
378  }
379 
380  // Replace space by dash
381  std::string::size_type pos = 0;
382  while ((pos = rv.find(" ", pos)) != std::string::npos) {
383  rv.replace(pos, 1, "-");
384  }
385 
386  return rv;
387 }
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:230
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Laser1080Interface Fawkes BlackBoard Interface.
void remove_pointcloud(const char *id)
Remove the point cloud.
Fawkes library namespace.
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.
virtual void bb_interface_created(const char *type, const char *id)
BlackBoard interface created notification.
virtual void loop()
Code to execute in the thread.
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
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
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
Definition: lock_list.h:182
virtual void init()
Initialize 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
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
void bbio_add_observed_create(const char *type_pattern, const char *id_pattern="*")
Add interface creation type to watch list.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
Base class for exceptions in Fawkes.
Definition: exception.h:36
char * frame() const
Get frame value.
List with a lock.
Definition: thread.h:40
virtual void unlock() const
Unlock list.
Definition: lock_list.h:144
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:244
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
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.
virtual void finalize()
Finalize the thread.
void bbil_add_reader_interface(Interface *interface)
Add an interface to the reader addition/removal watch list.
virtual ~LaserPointCloudThread()
Destructor.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, unsigned int instance_serial)
A reading instance has been closed for a watched interface.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
char * frame() const
Get frame value.
Laser720Interface Fawkes BlackBoard Interface.
BlackBoard interface listener.
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.