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