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
31using 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
54void
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
144void
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
158void
160{
161 MutexLocker lock(mappings_.mutex());
162
164 for (m = mappings_.begin(); m != mappings_.end(); ++m) {
165 m->interface->read();
166 if (!m->interface->refreshed()) {
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
198void
199LaserPointCloudThread::bb_interface_created(const char *type, const char *id) noexcept
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) {
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) {
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) {
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
296void
298 fawkes::Uuid instance_serial) noexcept
299{
300 conditional_close(interface);
301}
302
303void
305 fawkes::Uuid instance_serial) noexcept
306{
307 conditional_close(interface);
308}
309
310void
311LaserPointCloudThread::conditional_close(Interface *interface) noexcept
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
355std::string
356LaserPointCloudThread::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 loop()
Code to execute in the thread.
virtual void bb_interface_created(const char *type, const char *id) noexcept
BlackBoard interface created notification.
virtual void finalize()
Finalize the thread.
virtual void bb_interface_writer_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A writing instance has been closed for a watched interface.
virtual ~LaserPointCloudThread()
Destructor.
virtual void bb_interface_reader_removed(fawkes::Interface *interface, fawkes::Uuid instance_serial) noexcept
A reading instance has been closed for a watched interface.
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="*") noexcept
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 noexcept
Get primary string.
Definition: exception.cpp:639
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
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
A convenience class for universally unique identifiers (UUIDs).
Definition: uuid.h:29
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36