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>
31 using namespace fawkes;
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();
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;
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();
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;
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);
169 mapping.cloud =
new pcl::PointCloud<pcl::PointXYZ>();
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,
"-");