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> 30 #include <interfaces/Laser1080Interface.h> 44 :
Thread(
"LaserPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
59 std::list<Laser360Interface *> l360ifs =
62 std::list<Laser720Interface *> l720ifs =
65 std::list<Laser1080Interface *> l1080ifs =
69 for (i = l360ifs.begin(); i != l360ifs.end(); ++i) {
70 InterfaceCloudMapping mapping;
71 mapping.id = interface_to_pcl_name((*i)->id());
73 mapping.interface_typed.as360 = *i;
74 mapping.interface = *i;
75 mapping.interface->read();
77 mapping.cloud->points.resize(360);
78 mapping.cloud->header.frame_id = (*i)->frame();
79 mapping.cloud->height = 1;
80 mapping.cloud->width = 360;
84 __mappings.push_back(mapping);
87 for (j = l720ifs.begin(); j != l720ifs.end(); ++j) {
88 InterfaceCloudMapping mapping;
89 mapping.id = interface_to_pcl_name((*j)->id());
91 mapping.interface_typed.as720 = *j;
92 mapping.interface = *j;
93 mapping.interface->read();
95 mapping.cloud->points.resize(720);
96 mapping.cloud->header.frame_id = (*j)->frame();
97 mapping.cloud->height = 1;
98 mapping.cloud->width = 720;
102 __mappings.push_back(mapping);
106 for (k = l1080ifs.begin(); k != l1080ifs.end(); ++k) {
107 InterfaceCloudMapping mapping;
108 mapping.id = interface_to_pcl_name((*k)->id());
110 mapping.interface_typed.as1080 = *k;
111 mapping.interface = *k;
112 mapping.interface->read();
114 mapping.cloud->points.resize(1080);
115 mapping.cloud->header.frame_id = (*k)->frame();
116 mapping.cloud->height = 1;
117 mapping.cloud->width = 1080;
121 __mappings.push_back(mapping);
132 for (
unsigned int i = 0; i < 360; ++i) {
133 sin_angles360[i] = sinf(
deg2rad(i));
134 cos_angles360[i] = cosf(
deg2rad(i));
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.));
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.));
154 for (m = __mappings.begin(); m != __mappings.end(); ++m) {
168 for (m = __mappings.begin(); m != __mappings.end(); ++m) {
169 m->interface->read();
170 if (! m->interface->changed()) {
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];
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];
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];
198 pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
206 InterfaceCloudMapping mapping;
207 mapping.id = interface_to_pcl_name(
id);
209 mapping.cloud->height = 1;
211 if (strncmp(type,
"Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
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;
231 mapping.id.c_str(), e.
what());
236 }
else if (strncmp(type,
"Laser720Interface", __INTERFACE_TYPE_SIZE) != 0) {
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;
256 mapping.id.c_str(), e.
what());
261 }
else if (strncmp(type,
"Laser1080Interface", __INTERFACE_TYPE_SIZE) != 0) {
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;
281 mapping.id.c_str(), e.
what());
306 __mappings.push_back(mapping);
311 unsigned int instance_serial)
throw()
313 conditional_close(interface);
318 unsigned int instance_serial)
throw()
320 conditional_close(interface);
324 LaserPointCloudThread::conditional_close(
Interface *interface)
throw()
331 InterfaceCloudMapping mapping;
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)));
343 if (! m->interface->has_writer() && (m->interface->num_readers() == 1)) {
357 std::string uid = mapping.interface->uid();
365 uid.c_str(), e.
what());
372 LaserPointCloudThread::interface_to_pcl_name(
const char *interface_id)
374 std::string rv = interface_id;
375 if (rv.find(
"Laser ") == 0) {
377 rv = rv.substr(strlen(
"Laser "));
381 std::string::size_type pos = 0;
382 while ((pos = rv.find(
" ", pos)) != std::string::npos) {
383 rv.replace(pos, 1,
"-");
Laser360Interface Fawkes BlackBoard Interface.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
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.
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.
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.
Logger * logger
This is the Logger member used to access the logger.
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
LaserPointCloudThread()
Constructor.
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.
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.
Base class for exceptions in Fawkes.
char * frame() const
Get frame value.
virtual void unlock() const
Unlock list.
virtual void unregister_observer(BlackBoardInterfaceObserver *observer)
Unregister BB interface observer.
const char * name() const
Get name of thread.
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.
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.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
virtual void close(Interface *interface)=0
Close interface.