22 #include "laser_pointcloud_thread.h"
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>
43 :
Thread(
"LaserPointCloudThread",
Thread::OPMODE_WAITFORWAKEUP),
57 std::list<Laser360Interface *> l360ifs =
60 std::list<Laser720Interface *> l720ifs =
63 std::list<Laser1080Interface *> l1080ifs =
67 for (i = l360ifs.begin(); i != l360ifs.end(); ++i) {
68 InterfaceCloudMapping mapping;
69 mapping.id = interface_to_pcl_name((*i)->id());
71 mapping.interface_typed.as360 = *i;
72 mapping.interface = *i;
73 mapping.interface->read();
75 mapping.cloud->points.resize(360);
76 mapping.cloud->header.frame_id = (*i)->frame();
77 mapping.cloud->height = 1;
78 mapping.cloud->width = 360;
82 mappings_.push_back(mapping);
85 for (j = l720ifs.begin(); j != l720ifs.end(); ++j) {
86 InterfaceCloudMapping mapping;
87 mapping.id = interface_to_pcl_name((*j)->id());
89 mapping.interface_typed.as720 = *j;
90 mapping.interface = *j;
91 mapping.interface->read();
93 mapping.cloud->points.resize(720);
94 mapping.cloud->header.frame_id = (*j)->frame();
95 mapping.cloud->height = 1;
96 mapping.cloud->width = 720;
100 mappings_.push_back(mapping);
104 for (k = l1080ifs.begin(); k != l1080ifs.end(); ++k) {
105 InterfaceCloudMapping mapping;
106 mapping.id = interface_to_pcl_name((*k)->id());
108 mapping.interface_typed.as1080 = *k;
109 mapping.interface = *k;
110 mapping.interface->read();
112 mapping.cloud->points.resize(1080);
113 mapping.cloud->header.frame_id = (*k)->frame();
114 mapping.cloud->height = 1;
115 mapping.cloud->width = 1080;
119 mappings_.push_back(mapping);
130 for (
unsigned int i = 0; i < 360; ++i) {
131 sin_angles360[i] = sinf(
deg2rad(i));
132 cos_angles360[i] = cosf(
deg2rad(i));
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.));
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.));
151 for (m = mappings_.begin(); m != mappings_.end(); ++m) {
164 for (m = mappings_.begin(); m != mappings_.end(); ++m) {
165 m->interface->read();
166 if (!m->interface->refreshed()) {
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];
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];
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];
194 pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
201 InterfaceCloudMapping mapping;
202 mapping.id = interface_to_pcl_name(
id);
204 mapping.cloud->height = 1;
206 if (strncmp(type,
"Laser360Interface", INTERFACE_TYPE_SIZE_) == 0) {
211 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
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);
224 logger->
log_warn(name(),
"Failed to add pointcloud %s: %s", mapping.id.c_str(), e.
what());
225 blackboard->
close(lif);
229 }
else if (strncmp(type,
"Laser720Interface", INTERFACE_TYPE_SIZE_) != 0) {
234 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
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);
247 logger->
log_warn(name(),
"Failed to add pointcloud %s: %s", mapping.id.c_str(), e.
what());
248 blackboard->
close(lif);
252 }
else if (strncmp(type,
"Laser1080Interface", INTERFACE_TYPE_SIZE_) != 0) {
257 logger->
log_warn(name(),
"Failed to open %s:%s: %s", type, id, e.
what());
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);
270 logger->
log_warn(name(),
"Failed to add pointcloud %s: %s", mapping.id.c_str(), e.
what());
271 blackboard->
close(lif);
277 bbil_add_data_interface(mapping.interface);
280 logger->
log_warn(name(),
"Failed to register for %s:%s: %s", type,
id, e.
what());
282 bbil_remove_data_interface(mapping.interface);
284 blackboard->
close(mapping.interface);
285 pcl_manager->remove_pointcloud(mapping.id.c_str());
288 name(),
"Failed to deregister %s:%s during error recovery: %s", type,
id, e.
what());
293 mappings_.push_back(mapping);
300 conditional_close(interface);
307 conditional_close(interface);
311 LaserPointCloudThread::conditional_close(
Interface *interface) noexcept
318 InterfaceCloudMapping mapping;
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)));
329 if (!m->interface->has_writer() && (m->interface->num_readers() == 1)) {
331 logger->
log_info(name(),
"Last on %s, closing", m->interface->uid());
343 std::string uid = mapping.interface->uid();
345 bbil_remove_data_interface(mapping.interface);
347 blackboard->
close(mapping.interface);
348 pcl_manager->remove_pointcloud(mapping.id.c_str());
350 logger->
log_error(name(),
"Failed to unregister or close %s: %s", uid.c_str(), e.
what());
356 LaserPointCloudThread::interface_to_pcl_name(
const char *interface_id)
358 std::string rv = interface_id;
359 if (rv.compare(0, 6,
"Laser ") == 0) {
361 rv = rv.substr(strlen(
"Laser "));
365 std::string::size_type pos = 0;
366 while ((pos = rv.find(
" ", pos)) != std::string::npos) {
367 rv.replace(pos, 1,
"-");
LaserPointCloudThread()
Constructor.
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.
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.
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.
virtual void register_observer(BlackBoardInterfaceObserver *observer)
Register BB interface observer.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
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.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
virtual const char * what() const noexcept
Get primary string.
Base class for all Fawkes BlackBoard interfaces.
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.
virtual void unlock() const
Unlock list.
RefPtr< Mutex > mutex() const
Get access to the internal mutex.
virtual void log_info(const char *component, const char *format,...)
Log informational message.
virtual void log_warn(const char *component, const char *format,...)
Log warning message.
virtual void log_error(const char *component, const char *format,...)
Log error message.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
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.
Thread class encapsulation of pthreads.
A convenience class for universally unique identifiers (UUIDs).
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.