Fawkes API
Fawkes Development Version
|
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 }