Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * ir_pcl_thread.cpp - Robotino IR point cloud thread 00004 * 00005 * Created: Mon Mar 26 14:06:29 2012 00006 * Copyright 2011-2012 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include "ir_pcl_thread.h" 00024 00025 #include <interfaces/RobotinoSensorInterface.h> 00026 #include <limits> 00027 00028 using namespace fawkes; 00029 00030 /** @class RobotinoIrPclThread "ir_pcl_thread.h" 00031 * Robotino IR distances as point cloud. 00032 * This thread integrates into the Fawkes main loop at the SENSOR_PROCESS 00033 * hook and converts sensor data to a pointcloud 00034 * @author Tim Niemueller 00035 */ 00036 00037 /** Constructor. */ 00038 RobotinoIrPclThread::RobotinoIrPclThread() 00039 : Thread("RobotinoIrPclThread", Thread::OPMODE_WAITFORWAKEUP), 00040 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PREPARE) 00041 { 00042 } 00043 00044 00045 void 00046 RobotinoIrPclThread::init() 00047 { 00048 sens_if_ = blackboard->open_for_reading<RobotinoSensorInterface>("Robotino"); 00049 00050 sens_if_->read(); 00051 00052 pcl_xyz_ = new pcl::PointCloud<pcl::PointXYZ>(); 00053 pcl_xyz_->is_dense = false; 00054 pcl_xyz_->width = sens_if_->maxlenof_distance(); 00055 pcl_xyz_->height = 1; 00056 pcl_xyz_->points.resize(pcl_xyz_->width * pcl_xyz_->height); 00057 pcl_xyz_->header.frame_id = "/base_link"; 00058 00059 pcl_manager->add_pointcloud("robotino-ir", pcl_xyz_); 00060 00061 float angle_offset = (2 * M_PI) / pcl_xyz_->width; 00062 angle_sines_ = new float[pcl_xyz_->width]; 00063 angle_cosines_ = new float[pcl_xyz_->width]; 00064 for (unsigned int i = 0; i < pcl_xyz_->width; ++i) { 00065 angle_sines_[i] = sinf(angle_offset * i); 00066 angle_cosines_[i] = cosf(angle_offset * i); 00067 } 00068 } 00069 00070 00071 void 00072 RobotinoIrPclThread::finalize() 00073 { 00074 pcl_manager->remove_pointcloud("robotino-ir"); 00075 blackboard->close(sens_if_); 00076 00077 delete angle_sines_; 00078 delete angle_cosines_; 00079 } 00080 00081 void 00082 RobotinoIrPclThread::loop() 00083 { 00084 // update sensor values in interface 00085 sens_if_->read(); 00086 00087 if (sens_if_->changed()) { 00088 const Time *ct = sens_if_->timestamp(); 00089 const float *distances = sens_if_->distance(); 00090 00091 pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_xyz_; 00092 pcl.header.seq += 1; 00093 00094 pcl_utils::set_time(pcl_xyz_, *ct); 00095 00096 for (unsigned int i = 0; i < pcl_xyz_->width; ++i) { 00097 if (distances[i] == 0.) { 00098 pcl.points[i].x = pcl.points[i].y = pcl.points[i].z = 00099 std::numeric_limits<float>::quiet_NaN(); 00100 } else { 00101 pcl.points[i].x = (distances[i] + 0.2) * angle_cosines_[i]; 00102 pcl.points[i].y = (distances[i] + 0.2) * angle_sines_[i]; 00103 pcl.points[i].z = 0.025; // 2.5 cm above ground 00104 } 00105 } 00106 } 00107 }