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 
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 
55 void
57 {
58  std::list<Laser360Interface *> l360ifs =
60 
61  std::list<Laser720Interface *> l720ifs =
63 
65  for (i = l360ifs.begin(); i != l360ifs.end(); ++i) {
66  InterfaceCloudMapping mapping;
67  mapping.id = interface_to_pcl_name((*i)->id());
68  mapping.is_360 = true;
69  mapping.interface_typed.as360 = *i;
70  mapping.interface = *i;
71  mapping.interface->read();
72  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
73  mapping.cloud->points.resize(360);
74  mapping.cloud->header.frame_id = (*i)->frame();
75  mapping.cloud->height = 1;
76  mapping.cloud->width = 360;
77  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
80  __mappings.push_back(mapping);
81  }
83  for (j = l720ifs.begin(); j != l720ifs.end(); ++j) {
84  InterfaceCloudMapping mapping;
85  mapping.id = interface_to_pcl_name((*j)->id());
86  mapping.is_360 = false;
87  mapping.interface_typed.as720 = *j;
88  mapping.interface = *j;
89  mapping.interface->read();
90  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
91  mapping.cloud->points.resize(720);
92  mapping.cloud->header.frame_id = (*j)->frame();
93  mapping.cloud->height = 1;
94  mapping.cloud->width = 720;
95  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
98  __mappings.push_back(mapping);
99  }
100 
102 
103  bbio_add_observed_create("Laser360Interface", "*");
104  bbio_add_observed_create("Laser720Interface", "*");
106 
107  // Generate lookup tables for sin and cos
108  for (unsigned int i = 0; i < 360; ++i) {
109  sin_angles360[i] = sinf(deg2rad(i));
110  cos_angles360[i] = cosf(deg2rad(i));
111  }
112  for (unsigned int i = 0; i < 720; ++i) {
113  sin_angles720[i] = sinf(deg2rad((float)i / 2.));
114  cos_angles720[i] = cosf(deg2rad((float)i / 2.));
115  }
116 }
117 
118 
119 void
121 {
124 
126  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
127  blackboard->close(m->interface);
128  pcl_manager->remove_pointcloud(m->id.c_str());
129  }
130  __mappings.clear();
131 }
132 
133 
134 void
136 {
137  MutexLocker lock(__mappings.mutex());
138 
140  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
141  m->interface->read();
142  if (! m->interface->changed()) {
143  continue;
144  }
145  if (m->is_360) {
146  float *distances = m->interface_typed.as360->distances();
147  for (unsigned int i = 0; i < 360; ++i) {
148  m->cloud->points[i].x = distances[i] * cos_angles360[i];
149  m->cloud->points[i].y = distances[i] * sin_angles360[i];
150  }
151  } else {
152  float *distances = m->interface_typed.as720->distances();
153  for (unsigned int i = 0; i < 720; ++i) {
154  m->cloud->points[i].x = distances[i] * cos_angles720[i];
155  m->cloud->points[i].y = distances[i] * sin_angles720[i];
156  }
157  }
158 
159  pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
160  }
161 }
162 
163 
164 void
165 LaserPointCloudThread::bb_interface_created(const char *type, const char *id) throw()
166 {
167  InterfaceCloudMapping mapping;
168  mapping.id = interface_to_pcl_name(id);
169  mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
170  mapping.cloud->height = 1;
171 
172  if (strncmp(type, "Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
173  Laser360Interface *lif;
174  try {
175  lif = blackboard->open_for_reading<Laser360Interface>(id);
176  } catch (Exception &e) {
177  // ignored
178  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
179  return;
180  }
181 
182  try {
183  mapping.is_360 = true;
184  mapping.interface_typed.as360 = lif;
185  mapping.interface = lif;
186  mapping.cloud->points.resize(360);
187  mapping.cloud->header.frame_id = lif->frame();
188  mapping.cloud->width = 360;
189  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
190  } catch (Exception &e) {
191  logger->log_warn(name(), "Failed to add pointcloud %s: %s",
192  mapping.id.c_str(), e.what());
193  blackboard->close(lif);
194  return;
195  }
196 
197  } else if (strncmp(type, "Laser720Interface", __INTERFACE_TYPE_SIZE) != 0) {
198  Laser720Interface *lif;
199  try {
200  lif = blackboard->open_for_reading<Laser720Interface>(id);
201  } catch (Exception &e) {
202  // ignored
203  logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
204  return;
205  }
206 
207  try {
208  mapping.is_360 = false;
209  mapping.interface_typed.as720 = lif;
210  mapping.interface = lif;
211  mapping.cloud->points.resize(720);
212  mapping.cloud->header.frame_id = lif->frame();
213  mapping.cloud->width = 720;
214  pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
215  } catch (Exception &e) {
216  logger->log_warn(name(), "Failed to add pointcloud %s: %s",
217  mapping.id.c_str(), e.what());
218  blackboard->close(lif);
219  return;
220  }
221  }
222 
223  try {
224  bbil_add_data_interface(mapping.interface);
225  blackboard->update_listener(this);
226  } catch (Exception &e) {
227  logger->log_warn(name(), "Failed to register for %s:%s: %s",
228  type, id, e.what());
229  try {
230  bbil_remove_data_interface(mapping.interface);
231  blackboard->update_listener(this);
232  blackboard->close(mapping.interface);
233  pcl_manager->remove_pointcloud(mapping.id.c_str());
234  } catch (Exception &e) {
235  logger->log_error(name(), "Failed to deregister %s:%s during error recovery: %s",
236  type, id, e.what());
237 
238  }
239  return;
240  }
241 
242  __mappings.push_back(mapping);
243 }
244 
245 void
247  unsigned int instance_serial) throw()
248 {
249  conditional_close(interface);
250 }
251 
252 void
254  unsigned int instance_serial) throw()
255 {
256  conditional_close(interface);
257 }
258 
259 void
260 LaserPointCloudThread::conditional_close(Interface *interface) throw()
261 {
262  Laser360Interface *l360if = dynamic_cast<Laser360Interface *>(interface);
263  Laser720Interface *l720if = dynamic_cast<Laser720Interface *>(interface);
264 
265  bool close = false;
266  InterfaceCloudMapping mapping;
267 
268  MutexLocker lock(__mappings.mutex());
269 
271  for (m = __mappings.begin(); m != __mappings.end(); ++m) {
272  bool match = (( m->is_360 && l360if && (*l360if == *m->interface_typed.as360)) ||
273  (! m->is_360 && l720if && (*l720if == *m->interface_typed.as720)));
274 
275  if (match) {
276  if (! m->interface->has_writer() && (m->interface->num_readers() == 1)) {
277  // It's only us
278  logger->log_info(name(), "Last on %s, closing", m->interface->uid());
279  close = true;
280  mapping = *m;
281  __mappings.erase(m);
282  break;
283  }
284  }
285  }
286 
287  lock.unlock();
288 
289  if (close) {
290  std::string uid = mapping.interface->uid();
291  try {
292  bbil_remove_data_interface(mapping.interface);
293  blackboard->update_listener(this);
294  blackboard->close(mapping.interface);
295  pcl_manager->remove_pointcloud(mapping.id.c_str());
296  } catch (Exception &e) {
297  logger->log_error(name(), "Failed to unregister or close %s: %s",
298  uid.c_str(), e.what());
299  }
300  }
301 }
302 
303 
304 std::string
305 LaserPointCloudThread::interface_to_pcl_name(const char *interface_id)
306 {
307  std::string rv = interface_id;
308  if (rv.find("Laser ") == 0) {
309  // starts with "Laser ", remove it
310  rv = rv.substr(strlen("Laser "));
311  }
312 
313  // Replace space by dash
314  std::string::size_type pos = 0;
315  while ((pos = rv.find(" ", pos)) != std::string::npos) {
316  rv.replace(pos, 1, "-");
317  }
318 
319  return rv;
320 }
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
Definition: blackboard.cpp:211
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
Definition: lock_list.h:182
void remove_pointcloud(const char *id)
Remove the point cloud.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Definition: multi.cpp:247
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.
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:200
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:186
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
char * frame() const
Get frame value.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:174
virtual void bb_interface_writer_removed(fawkes::Interface *interface, unsigned int instance_serial)
A writing instance has been closed for a watched interface.
char * frame() const
Get frame value.
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
virtual const char * what() const
Get primary string.
Definition: exception.cpp:661
Base class for exceptions in Fawkes.
Definition: exception.h:36
List with a lock.
Definition: thread.h:40
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
Definition: multi.cpp:225
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
Definition: blackboard.cpp:224
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:203
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)=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.
virtual std::list< Interface * > open_multiple_for_reading(const char *type_pattern, const char *id_pattern="*")=0
Open multiple interfaces for reading.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
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:43
virtual void close(Interface *interface)=0
Close interface.