22 #include "laser_pointcloud_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <utils/math/angle.h>
26 #include <pcl_utils/utils.h>
28 #include <interfaces/Laser360Interface.h>
29 #include <interfaces/Laser720Interface.h>
43 :
Thread(
"LaserPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
58 std::list<Laser360Interface *> l360ifs =
61 std::list<Laser720Interface *> l720ifs =
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();
73 mapping.cloud->points.resize(360);
74 mapping.cloud->header.frame_id = (*i)->frame();
75 mapping.cloud->height = 1;
76 mapping.cloud->width = 360;
80 __mappings.push_back(mapping);
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();
91 mapping.cloud->points.resize(720);
92 mapping.cloud->header.frame_id = (*j)->frame();
93 mapping.cloud->height = 1;
94 mapping.cloud->width = 720;
98 __mappings.push_back(mapping);
108 for (
unsigned int i = 0; i < 360; ++i) {
109 sin_angles360[i] = sinf(
deg2rad(i));
110 cos_angles360[i] = cosf(
deg2rad(i));
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.));
126 for (m = __mappings.begin(); m != __mappings.end(); ++m) {
140 for (m = __mappings.begin(); m != __mappings.end(); ++m) {
141 m->interface->read();
142 if (! m->interface->changed()) {
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];
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];
159 pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
167 InterfaceCloudMapping mapping;
168 mapping.id = interface_to_pcl_name(
id);
170 mapping.cloud->height = 1;
172 if (strncmp(type,
"Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
178 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
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);
191 logger->
log_warn(name(),
"Failed to add pointcloud %s: %s",
192 mapping.id.c_str(), e.
what());
193 blackboard->
close(lif);
197 }
else if (strncmp(type,
"Laser720Interface", __INTERFACE_TYPE_SIZE) != 0) {
203 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type,
id, e.
what());
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);
216 logger->
log_warn(name(),
"Failed to add pointcloud %s: %s",
217 mapping.id.c_str(), e.
what());
218 blackboard->
close(lif);
224 bbil_add_data_interface(mapping.interface);
227 logger->
log_warn(name(),
"Failed to register for %s:%s: %s",
230 bbil_remove_data_interface(mapping.interface);
232 blackboard->
close(mapping.interface);
233 pcl_manager->remove_pointcloud(mapping.id.c_str());
235 logger->
log_error(name(),
"Failed to deregister %s:%s during error recovery: %s",
242 __mappings.push_back(mapping);
247 unsigned int instance_serial)
throw()
249 conditional_close(interface);
254 unsigned int instance_serial)
throw()
256 conditional_close(interface);
260 LaserPointCloudThread::conditional_close(
Interface *interface)
throw()
266 InterfaceCloudMapping mapping;
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)));
276 if (! m->interface->has_writer() && (m->interface->num_readers() == 1)) {
278 logger->
log_info(name(),
"Last on %s, closing", m->interface->uid());
290 std::string uid = mapping.interface->uid();
292 bbil_remove_data_interface(mapping.interface);
294 blackboard->
close(mapping.interface);
295 pcl_manager->remove_pointcloud(mapping.id.c_str());
297 logger->
log_error(name(),
"Failed to unregister or close %s: %s",
298 uid.c_str(), e.
what());
305 LaserPointCloudThread::interface_to_pcl_name(
const char *interface_id)
307 std::string rv = interface_id;
308 if (rv.find(
"Laser ") == 0) {
310 rv = rv.substr(strlen(
"Laser "));
314 std::string::size_type pos = 0;
315 while ((pos = rv.find(
" ", pos)) != std::string::npos) {
316 rv.replace(pos, 1,
"-");
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
void remove_pointcloud(const char *id)
Remove the point cloud.
virtual void log_error(const char *component, const char *format,...)
Log error message.
Fawkes library namespace.
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.
Thread class encapsulation of pthreads.
virtual void update_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Update BB event listener.
Base class for all Fawkes BlackBoard interfaces.
char * frame() const
Get frame value.
LaserPointCloudThread()
Constructor.
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.
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.
virtual const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
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.
Laser720Interface Fawkes BlackBoard Interface.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.