Fawkes API  Fawkes Development Version
pcl_thread.cpp
00001 
00002 /***************************************************************************
00003  *  pcl_thread.cpp - Thread to exchange point clouds
00004  *
00005  *  Created: Mon Nov 07 02:58:40 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 "pcl_thread.h"
00023 
00024 #include <core/threading/mutex_locker.h>
00025 #include <sensor_msgs/PointCloud2.h>
00026 
00027 using namespace fawkes;
00028 
00029 /** @class RosPointCloudThread "pcl_thread.h"
00030  * Thread to exchange point clouds between Fawkes and ROS.
00031  * @author Tim Niemueller
00032  */
00033 
00034 /** Constructor. */
00035 RosPointCloudThread::RosPointCloudThread()
00036   : Thread("RosPointCloudThread", Thread::OPMODE_WAITFORWAKEUP),
00037     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
00038 {
00039 }
00040 
00041 /** Destructor. */
00042 RosPointCloudThread::~RosPointCloudThread()
00043 {
00044 }
00045 
00046 
00047 
00048 void
00049 RosPointCloudThread::init()
00050 {
00051   //__pubman = new RosPointCloudPublisherManager(rosnode);
00052   __adapter = new RosPointCloudAdapter(pcl_manager, logger);
00053 
00054   std::vector<std::string> pcls = pcl_manager->get_pointcloud_list();
00055 
00056   std::vector<std::string>::iterator p;
00057   for (p = pcls.begin(); p != pcls.end(); ++p) {
00058 
00059     std::string topic_name = std::string("fawkes_pcls/") + *p;
00060     std::string::size_type pos = 0;
00061     while ((pos = topic_name.find("-", pos)) != std::string::npos) {
00062       topic_name.replace(pos, 1, "_");
00063     }
00064 
00065     PublisherInfo pi;
00066     pi.pub = rosnode->advertise<sensor_msgs::PointCloud2>(topic_name, 1);
00067 
00068     logger->log_info(name(), "Publishing point cloud %s at %s", p->c_str(), topic_name.c_str());
00069 
00070     std::string frame_id;
00071     unsigned int width, height;
00072     bool is_dense;
00073     RosPointCloudAdapter::V_PointFieldInfo fieldinfo;
00074     __adapter->get_info(*p, width, height, frame_id, is_dense, fieldinfo);
00075     pi.msg.header.frame_id = frame_id;
00076     pi.msg.width = width;
00077     pi.msg.height = height;
00078     pi.msg.is_dense = is_dense;
00079     pi.msg.fields.clear();
00080     pi.msg.fields.resize(fieldinfo.size());
00081     for (unsigned int i = 0; i < fieldinfo.size(); ++i) {
00082       pi.msg.fields[i].name     = fieldinfo[i].name;
00083       pi.msg.fields[i].offset   = fieldinfo[i].offset;
00084       pi.msg.fields[i].datatype = fieldinfo[i].datatype;
00085       pi.msg.fields[i].count    = fieldinfo[i].count;
00086     }
00087 
00088     __pubs[*p] = pi;
00089   }
00090 }
00091 
00092 
00093 void
00094 RosPointCloudThread::finalize()
00095 {
00096   delete __adapter;
00097 }
00098 
00099 
00100 void
00101 RosPointCloudThread::loop()
00102 {
00103   std::map<std::string, PublisherInfo>::iterator p;
00104   for (p = __pubs.begin(); p != __pubs.end(); ++p) {
00105     PublisherInfo &pi = p->second;
00106     if (pi.pub.getNumSubscribers() > 0) {
00107       unsigned int width, height;
00108       void *point_data;
00109       size_t point_size, num_points;
00110       fawkes::Time time;
00111       __adapter->get_data(p->first, width, height, time,
00112                           &point_data, point_size, num_points);
00113 
00114       if (pi.last_sent != time) {
00115         pi.last_sent = time;
00116 
00117         size_t data_size = point_size * num_points;
00118         pi.msg.data.resize(data_size);
00119         memcpy (&pi.msg.data[0], point_data, data_size);
00120 
00121         pi.msg.width             = width;
00122         pi.msg.height            = height;
00123         pi.msg.header.stamp.sec  = time.get_sec();
00124         pi.msg.header.stamp.nsec = time.get_nsec();
00125         pi.msg.point_step        = point_size;
00126         pi.msg.row_step          = point_size * pi.msg.width;
00127 
00128         pi.pub.publish(pi.msg);
00129       //} else {
00130         // logger->log_debug(name(), "No update for %s, not sending", p->first.c_str());
00131       }
00132     } else {
00133       __adapter->close(p->first);
00134     }
00135   }
00136 }