Fawkes API  Fawkes Development Version
laser_pointcloud_thread.cpp
00001 
00002 /***************************************************************************
00003  *  laser_pointcloud_thread.cpp - Convert laser data to pointclouds
00004  *
00005  *  Created: Thu Nov 17 10:21:55 2011
00006  *  Copyright  2011  Tim Niemueller [www.niemueller.de]
00007  ****************************************************************************/
00008 
00009 /*  This program is free software; you can redistribute it and/or modify
00010  *  it under the terms of the GNU General Public License as published by
00011  *  the Free Software Foundation; either version 2 of the License, or
00012  *  (at your option) any later version.
00013  *
00014  *  This program is distributed in the hope that it will be useful,
00015  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  *  GNU Library General Public License for more details.
00018  *
00019  *  Read the full text in the LICENSE.GPL file in the doc directory.
00020  */
00021 
00022 #include "laser_pointcloud_thread.h"
00023 
00024 #include <core/threading/mutex_locker.h>
00025 #include <utils/math/angle.h>
00026 #include <pcl_utils/utils.h>
00027 
00028 #include <interfaces/Laser360Interface.h>
00029 #include <interfaces/Laser720Interface.h>
00030 
00031 using namespace fawkes;
00032 
00033 /** @class LaserPointCloudThread "tf_thread.h"
00034  * Thread to exchange transforms between Fawkes and ROS.
00035  * This threads connects to Fawkes and ROS to read and write transforms.
00036  * Transforms received on one end are republished to the other side. To
00037  * Fawkes new frames are published during the sensor hook.
00038  * @author Tim Niemueller
00039  */
00040 
00041 /** Constructor. */
00042 LaserPointCloudThread::LaserPointCloudThread()
00043   : Thread("LaserPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
00044     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE),
00045     BlackBoardInterfaceListener("LaserPointCloudThread")
00046 {
00047 }
00048 
00049 /** Destructor. */
00050 LaserPointCloudThread::~LaserPointCloudThread()
00051 {
00052 }
00053 
00054 
00055 void
00056 LaserPointCloudThread::init()
00057 {
00058   std::list<Laser360Interface *> l360ifs =
00059     blackboard->open_multiple_for_reading<Laser360Interface>("*");
00060 
00061   std::list<Laser720Interface *> l720ifs =
00062     blackboard->open_multiple_for_reading<Laser720Interface>("*");
00063 
00064   LockList<Laser360Interface *>::iterator i;
00065   for (i = l360ifs.begin(); i != l360ifs.end(); ++i) {
00066     InterfaceCloudMapping mapping;
00067     mapping.id = interface_to_pcl_name((*i)->id());
00068     mapping.is_360 = true;
00069     mapping.interface_typed.as360 = *i;
00070     mapping.interface = *i;
00071     mapping.interface->read();
00072     mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
00073     mapping.cloud->points.resize(360);
00074     mapping.cloud->header.frame_id = (*i)->frame();
00075     mapping.cloud->height = 1;
00076     mapping.cloud->width = 360;
00077     pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
00078     bbil_add_reader_interface(*i);
00079     bbil_add_writer_interface(*i);
00080     __mappings.push_back(mapping);
00081   }
00082   LockList<Laser720Interface *>::iterator j;
00083   for (j = l720ifs.begin(); j != l720ifs.end(); ++j) {
00084     InterfaceCloudMapping mapping;
00085     mapping.id = interface_to_pcl_name((*j)->id());
00086     mapping.is_360 = false;
00087     mapping.interface_typed.as720 = *j;
00088     mapping.interface = *j;
00089     mapping.interface->read();
00090     mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
00091     mapping.cloud->points.resize(720);
00092     mapping.cloud->header.frame_id = (*j)->frame();
00093     mapping.cloud->height = 1;
00094     mapping.cloud->width = 720;
00095     pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
00096     bbil_add_reader_interface(*j);
00097     bbil_add_writer_interface(*j);
00098     __mappings.push_back(mapping);
00099   }
00100 
00101   blackboard->register_listener(this);
00102 
00103   bbio_add_observed_create("Laser360Interface", "*");
00104   bbio_add_observed_create("Laser720Interface", "*");
00105   blackboard->register_observer(this);
00106 
00107   // Generate lookup tables for sin and cos
00108   for (unsigned int i = 0; i < 360; ++i) {
00109     sin_angles360[i] = sinf(deg2rad(i));
00110     cos_angles360[i] = cosf(deg2rad(i));
00111   }
00112   for (unsigned int i = 0; i < 720; ++i) {
00113     sin_angles720[i] = sinf(deg2rad((float)i / 2.));
00114     cos_angles720[i] = cosf(deg2rad((float)i / 2.));
00115   }
00116 }
00117 
00118 
00119 void
00120 LaserPointCloudThread::finalize()
00121 {
00122   blackboard->unregister_listener(this);
00123   blackboard->unregister_observer(this);
00124 
00125   LockList<InterfaceCloudMapping>::iterator m;
00126   for (m = __mappings.begin(); m != __mappings.end(); ++m) {
00127     blackboard->close(m->interface);
00128     pcl_manager->remove_pointcloud(m->id.c_str());
00129   }
00130   __mappings.clear();
00131 }
00132 
00133 
00134 void
00135 LaserPointCloudThread::loop()
00136 {
00137   MutexLocker lock(__mappings.mutex());
00138 
00139   LockList<InterfaceCloudMapping>::iterator m;
00140   for (m = __mappings.begin(); m != __mappings.end(); ++m) {
00141     m->interface->read();
00142     if (! m->interface->changed()) {
00143       continue;
00144     }
00145     if (m->is_360) {
00146       float *distances = m->interface_typed.as360->distances();
00147       for (unsigned int i = 0; i < 360; ++i) {
00148         m->cloud->points[i].x = distances[i] * cos_angles360[i];
00149         m->cloud->points[i].y = distances[i] * sin_angles360[i];
00150       }
00151     } else {
00152       float *distances = m->interface_typed.as720->distances();
00153       for (unsigned int i = 0; i < 720; ++i) {
00154         m->cloud->points[i].x = distances[i] * cos_angles720[i];
00155         m->cloud->points[i].y = distances[i] * sin_angles720[i];
00156       }
00157     }
00158 
00159     pcl_utils::set_time(m->cloud, *(m->interface->timestamp()));
00160   }
00161 }
00162 
00163 
00164 void
00165 LaserPointCloudThread::bb_interface_created(const char *type, const char *id) throw()
00166 {
00167   InterfaceCloudMapping mapping;
00168   mapping.id = interface_to_pcl_name(id);
00169   mapping.cloud = new pcl::PointCloud<pcl::PointXYZ>();
00170   mapping.cloud->height = 1;
00171 
00172   if (strncmp(type, "Laser360Interface", __INTERFACE_TYPE_SIZE) == 0) {
00173     Laser360Interface *lif;
00174     try {
00175       lif = blackboard->open_for_reading<Laser360Interface>(id);
00176     } catch (Exception &e) {
00177       // ignored
00178       logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
00179       return;
00180     }
00181 
00182     try {
00183       mapping.is_360 = true;
00184       mapping.interface_typed.as360 = lif;
00185       mapping.interface = lif;
00186       mapping.cloud->points.resize(360);
00187       mapping.cloud->header.frame_id = lif->frame();
00188       mapping.cloud->width = 360;
00189       pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
00190     } catch (Exception &e) {
00191       logger->log_warn(name(), "Failed to add pointcloud %s: %s",
00192                        mapping.id.c_str(), e.what());
00193       blackboard->close(lif);
00194       return;
00195     }
00196 
00197   } else if (strncmp(type, "Laser720Interface", __INTERFACE_TYPE_SIZE) != 0) {
00198     Laser720Interface *lif;
00199     try {
00200       lif = blackboard->open_for_reading<Laser720Interface>(id);
00201     } catch (Exception &e) {
00202       // ignored
00203       logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what());
00204       return;
00205     }
00206 
00207     try {
00208       mapping.is_360 = false;
00209       mapping.interface_typed.as720 = lif;
00210       mapping.interface = lif;
00211       mapping.cloud->points.resize(720);
00212       mapping.cloud->header.frame_id = lif->frame();
00213       mapping.cloud->width = 720;
00214       pcl_manager->add_pointcloud(mapping.id.c_str(), mapping.cloud);
00215     } catch (Exception &e) {
00216       logger->log_warn(name(), "Failed to add pointcloud %s: %s",
00217                        mapping.id.c_str(), e.what());
00218       blackboard->close(lif);
00219       return;
00220     }
00221   }
00222 
00223   try {
00224     bbil_add_data_interface(mapping.interface);
00225     blackboard->update_listener(this);
00226   } catch (Exception &e) {
00227     logger->log_warn(name(), "Failed to register for %s:%s: %s",
00228                      type, id, e.what());
00229     try {
00230       bbil_remove_data_interface(mapping.interface);
00231       blackboard->update_listener(this);
00232       blackboard->close(mapping.interface);
00233       pcl_manager->remove_pointcloud(mapping.id.c_str());
00234     } catch (Exception &e) {
00235       logger->log_error(name(), "Failed to deregister %s:%s during error recovery: %s",
00236                         type, id, e.what());
00237       
00238     }
00239     return;
00240   }
00241 
00242   __mappings.push_back(mapping);
00243 }
00244 
00245 void
00246 LaserPointCloudThread::bb_interface_writer_removed(fawkes::Interface *interface,
00247                                                    unsigned int instance_serial) throw()
00248 {
00249   conditional_close(interface);
00250 }
00251 
00252 void
00253 LaserPointCloudThread::bb_interface_reader_removed(fawkes::Interface *interface,
00254                                                    unsigned int instance_serial) throw()
00255 {
00256   conditional_close(interface);
00257 }
00258 
00259 void
00260 LaserPointCloudThread::conditional_close(Interface *interface) throw()
00261 {
00262   Laser360Interface *l360if = dynamic_cast<Laser360Interface *>(interface);
00263   Laser720Interface *l720if = dynamic_cast<Laser720Interface *>(interface);
00264 
00265   bool close = false;
00266   InterfaceCloudMapping mapping;
00267 
00268   MutexLocker lock(__mappings.mutex());
00269 
00270   fawkes::LockList<InterfaceCloudMapping>::iterator m;
00271   for (m = __mappings.begin(); m != __mappings.end(); ++m) {
00272     bool match = ((  m->is_360 && l360if && (*l360if == *m->interface_typed.as360)) ||
00273                   (! m->is_360 && l720if && (*l720if == *m->interface_typed.as720)));
00274 
00275     if (match) {
00276       if (! m->interface->has_writer() && (m->interface->num_readers() == 1)) {
00277         // It's only us
00278         logger->log_info(name(), "Last on %s, closing", m->interface->uid());
00279          close = true;
00280          mapping = *m;
00281          __mappings.erase(m);
00282         break;
00283       }
00284     }
00285   }
00286 
00287   lock.unlock();
00288 
00289   if (close) {
00290     std::string uid = mapping.interface->uid();
00291     try {
00292       bbil_remove_data_interface(mapping.interface);
00293       blackboard->update_listener(this);
00294       blackboard->close(mapping.interface);
00295       pcl_manager->remove_pointcloud(mapping.id.c_str());
00296     } catch (Exception &e) {
00297       logger->log_error(name(), "Failed to unregister or close %s: %s",
00298                         uid.c_str(), e.what());
00299     }
00300   }
00301 }
00302 
00303 
00304 std::string
00305 LaserPointCloudThread::interface_to_pcl_name(const char *interface_id)
00306 {
00307   std::string rv = interface_id;
00308   if (rv.find("Laser ") == 0) {
00309     // starts with "Laser ", remove it
00310     rv = rv.substr(strlen("Laser "));
00311   }
00312 
00313   // Replace space by dash
00314   std::string::size_type pos = 0;
00315   while ((pos = rv.find(" ", pos)) != std::string::npos) {
00316     rv.replace(pos, 1, "-");
00317   }
00318 
00319   return rv;
00320 }