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