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