Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pcl_adapter.cpp - PCL exchange publisher manager 00004 * 00005 * Created: Tue Nov 08 00:38:34 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_adapter.h" 00023 00024 #include <pcl/point_cloud.h> 00025 #include <pcl/point_types.h> 00026 #include <pcl/ros/conversions.h> 00027 #include <logging/logger.h> 00028 #include <aspect/pointcloud/pointcloud_manager.h> 00029 00030 using namespace fawkes; 00031 00032 /// @cond INTERNALS 00033 /** ROS to PCL storage adapter. */ 00034 class RosPointCloudAdapter::StorageAdapter 00035 { 00036 public: 00037 /** Constructor. 00038 * @param a_ adapter to clone */ 00039 StorageAdapter(const PointCloudManager::StorageAdapter *a_) 00040 : a(a_->clone()) {} 00041 00042 /** Destructor. */ 00043 ~StorageAdapter() 00044 { delete a; } 00045 00046 /** PCL Point cloud storage adapter to encapsulate. */ 00047 PointCloudManager::StorageAdapter *a; 00048 }; 00049 /// @endcond 00050 00051 /** @class RosPointCloudAdapter "pcl_adapter.h" 00052 * Standalone PCL to ROS adapter class. 00053 * Currently, the standalone PCL comes with sensor_msgs and std_msgs 00054 * data types which are incompatible with the ones that come with 00055 * ROS. Hence, to use both in the same plugin, we need to confine the 00056 * two different type instances into their own modules. While the rest 00057 * of the ros-pcl plugins uses ROS-specific data types, this very 00058 * class interacts with and only with the standalone PCL and the 00059 * PointCloudManager. Interaction to the ROS parts is done by passing 00060 * internal data types so that exchange can happen without common 00061 * sensor_msgs or std_msgs data types. 00062 * @author Tim Niemueller 00063 */ 00064 00065 /** Constructor. 00066 * @param pcl_manager PCL manager 00067 * @param logger logger 00068 */ 00069 RosPointCloudAdapter::RosPointCloudAdapter(PointCloudManager *pcl_manager, 00070 Logger *logger) 00071 : __pcl_manager(pcl_manager) 00072 { 00073 } 00074 00075 00076 /** Destructor. */ 00077 RosPointCloudAdapter::~RosPointCloudAdapter() 00078 { 00079 std::map<std::string, StorageAdapter *>::iterator i; 00080 for (i = __sas.begin(); i != __sas.end(); ++i) { 00081 delete i->second; 00082 } 00083 __sas.clear(); 00084 } 00085 00086 /** Fill information of arbitrary point type. 00087 * @param p point cloud to get info from 00088 * @param width upon return contains width of point cloud 00089 * @param height upon return contains width of point cloud 00090 * @param frame_id upon return contains frame ID of the point cloud 00091 * @param is_dense upon return contains true if point cloud is dense and false otherwise 00092 * @param pfi upon return contains data type information 00093 */ 00094 template <typename PointT> 00095 static void 00096 fill_info(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &p, 00097 unsigned int &width, unsigned int &height, 00098 std::string &frame_id, bool &is_dense, 00099 RosPointCloudAdapter::V_PointFieldInfo &pfi) 00100 { 00101 width = p->width; 00102 height = p->height; 00103 frame_id = p->header.frame_id; 00104 is_dense = p->is_dense; 00105 00106 std::vector<sensor_msgs::PointField> pfields; 00107 pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type> 00108 (pcl::detail::FieldAdder<PointT>(pfields)); 00109 00110 pfi.clear(); 00111 pfi.resize(pfields.size()); 00112 for (unsigned int i = 0; i < pfields.size(); ++i) { 00113 sensor_msgs::PointField &pf = pfields[i]; 00114 pfi[i] = RosPointCloudAdapter::PointFieldInfo(pf.name, pf.offset, 00115 pf.datatype, pf.count); 00116 } 00117 } 00118 00119 /** Get info about point cloud. 00120 * @param id ID of point cloud to get info from 00121 * @param width upon return contains width of point cloud 00122 * @param height upon return contains width of point cloud 00123 * @param frame_id upon return contains frame ID of the point cloud 00124 * @param is_dense upon return contains true if point cloud is dense and false otherwise 00125 * @param pfi upon return contains data type information 00126 */ 00127 void 00128 RosPointCloudAdapter::get_info(std::string &id, 00129 unsigned int &width, unsigned int &height, 00130 std::string &frame_id, bool &is_dense, 00131 V_PointFieldInfo &pfi) 00132 { 00133 if (__sas.find(id) == __sas.end()) { 00134 __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str())); 00135 } 00136 00137 if (__pcl_manager->exists_pointcloud<pcl::PointXYZ>(id.c_str())) { 00138 const fawkes::RefPtr<const pcl::PointCloud<pcl::PointXYZ> > p = 00139 __pcl_manager->get_pointcloud<pcl::PointXYZ>(id.c_str()); 00140 fill_info(p, width, height, frame_id, is_dense, pfi); 00141 00142 } else if (__pcl_manager->exists_pointcloud<pcl::PointXYZRGB>(id.c_str())) { 00143 const fawkes::RefPtr<const pcl::PointCloud<pcl::PointXYZRGB> > p = 00144 __pcl_manager->get_pointcloud<pcl::PointXYZRGB>(id.c_str()); 00145 fill_info(p, width, height, frame_id, is_dense, pfi); 00146 00147 } else { 00148 throw Exception("PointCloud '%s' does not exist or unknown type", id.c_str()); 00149 } 00150 } 00151 00152 00153 /** Get current data of point cloud. 00154 * @param id ID of point cloud to get info from 00155 * @param width upon return contains width of point cloud 00156 * @param height upon return contains width of point cloud 00157 * @param time capture time 00158 * @param data_ptr upon return contains pointer to point cloud data 00159 * @param point_size upon return contains size of a single point 00160 * @param num_points upon return contains number of points 00161 */ 00162 void 00163 RosPointCloudAdapter::get_data(const std::string &id, 00164 unsigned int &width, unsigned int &height, fawkes::Time &time, 00165 void **data_ptr, size_t &point_size, size_t &num_points) 00166 { 00167 if (__sas.find(id) == __sas.end()) { 00168 __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str())); 00169 } 00170 00171 const PointCloudManager::StorageAdapter *sa = __sas[id]->a; 00172 width = sa->width(); 00173 height = sa->height(); 00174 *data_ptr = sa->data_ptr(); 00175 point_size = sa->point_size(); 00176 num_points = sa->num_points(); 00177 sa->get_time(time); 00178 } 00179 00180 00181 /** Close an adapter. 00182 * @param id ID of point cloud to close 00183 */ 00184 void 00185 RosPointCloudAdapter::close(const std::string &id) 00186 { 00187 if (__sas.find(id) != __sas.end()) { 00188 delete __sas[id]; 00189 __sas.erase(id); 00190 } 00191 }