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