Fawkes API  Fawkes Development Version
pointcloud_manager.h
00001 
00002 /***************************************************************************
00003  *  pointcloud_manager.h - PointCloud manager for aspect
00004  *
00005  *  Created: Sun Nov 06 23:29:36 2011
00006  *  Copyright  2011  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. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #ifndef __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
00025 #define __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
00026 
00027 #include <aspect/aspect.h>
00028 #include <core/exception.h>
00029 #include <core/utils/refptr.h>
00030 #include <core/utils/lock_map.h>
00031 #include <core/threading/mutex_locker.h>
00032 #include <utils/time/time.h>
00033 
00034 #include <vector>
00035 #include <string>
00036 #include <stdint.h>
00037 #include <typeinfo>
00038 #include <cstring>
00039 
00040 namespace pcl {
00041   template <typename PointT>
00042     class PointCloud;
00043 }
00044 
00045 namespace fawkes {
00046 #if 0 /* just to make Emacs auto-indent happy */
00047 }
00048 #endif
00049 
00050 /** Union to pack fawkes::Time into the pcl::PointCloud timestamp. */
00051 typedef union {
00052   struct {
00053     uint64_t sec  : 44; ///< seconds part of time
00054     uint64_t usec : 20; ///< microseconds part of time
00055   } time;               ///< Access timestamp as time
00056   uint64_t timestamp;   ///< Access timestamp as number only
00057 } PointCloudTimestamp;
00058 
00059 
00060 class PointCloudManager
00061 {
00062  public:
00063   PointCloudManager();
00064   virtual ~PointCloudManager();
00065 
00066   template <typename PointT>
00067     void add_pointcloud(const char *id, RefPtr<pcl::PointCloud<PointT> > cloud);
00068 
00069   void remove_pointcloud(const char *id);
00070 
00071   template <typename PointT>
00072     const RefPtr<const pcl::PointCloud<PointT> > get_pointcloud(const char *id);
00073   bool exists_pointcloud(const char *id);
00074 
00075   /**  Check if point cloud of specified type exists.
00076    * @param id ID of point cloud to check
00077    * @return true if the point cloud exists, false otherwise
00078    */
00079   template <typename PointT>
00080   bool exists_pointcloud(const char *id);
00081 
00082   template <typename PointT>
00083     class PointCloudStorageAdapter;
00084 
00085   class StorageAdapter {
00086   public:
00087     virtual ~StorageAdapter();
00088 
00089     template <typename PointT>
00090     bool is_pointtype() const;
00091 
00092     template <typename PointT>
00093     PointCloudStorageAdapter<PointT> * as_pointtype();
00094 
00095     virtual const char * get_typename() = 0;
00096     virtual StorageAdapter * clone() const = 0;
00097     virtual size_t  point_size() const = 0;
00098     virtual unsigned int  width() const = 0;
00099     virtual unsigned int  height() const = 0;
00100     virtual size_t  num_points() const = 0;
00101     virtual void *  data_ptr() const = 0;
00102     virtual void get_time(fawkes::Time &time) const = 0;
00103   };
00104 
00105   template <typename PointT>
00106     class PointCloudStorageAdapter : public StorageAdapter
00107   {
00108    public:
00109     /** Constructor.
00110      * @param cloud cloud to encapsulate.
00111      */
00112     PointCloudStorageAdapter(RefPtr<pcl::PointCloud<PointT> > cloud)
00113       : cloud(cloud) {}
00114 
00115     /** Copy constructor.
00116      * @param p storage adapter to copy
00117      */
00118     PointCloudStorageAdapter(const PointCloudStorageAdapter<PointT> *p)
00119       : cloud(p->cloud) {}
00120 
00121     /** The point cloud. */
00122     const RefPtr<pcl::PointCloud<PointT> > cloud;
00123 
00124     virtual StorageAdapter * clone() const;
00125 
00126     virtual const char * get_typename() { return typeid(this).name(); }
00127     virtual size_t  point_size() const { return sizeof(PointT); }
00128     virtual unsigned int  width() const { return cloud->width; }
00129     virtual unsigned int  height() const { return cloud->height; }
00130     virtual size_t  num_points() const { return cloud->points.size(); }
00131     virtual void *  data_ptr() const  { return &cloud->points[0]; }
00132     virtual void get_time(fawkes::Time &time) const;
00133   };
00134 
00135   std::vector<std::string>  get_pointcloud_list() const;
00136   const fawkes::LockMap<std::string, StorageAdapter *> &  get_pointclouds() const;
00137   const StorageAdapter *  get_storage_adapter(const char *id);
00138 
00139  private:
00140   fawkes::LockMap<std::string, StorageAdapter *>  __clouds;
00141 };
00142 
00143 
00144 template <typename PointT>
00145 bool
00146 PointCloudManager::StorageAdapter::is_pointtype() const
00147 {
00148   const PointCloudStorageAdapter<PointT> *pa =
00149     dynamic_cast<const PointCloudStorageAdapter<PointT> *>(this);
00150   return (!!pa);
00151 }
00152 
00153 
00154 template <typename PointT>
00155 PointCloudManager::PointCloudStorageAdapter<PointT> *
00156 PointCloudManager::StorageAdapter::as_pointtype()
00157 {
00158   PointCloudStorageAdapter<PointT> *pa =
00159     dynamic_cast<PointCloudStorageAdapter<PointT> *>(this);
00160   if (!pa) {
00161     throw Exception("PointCloud storage adapter is not of anticipated type");
00162   }
00163   return pa;
00164 }
00165 
00166 template <typename PointT>
00167 void
00168 PointCloudManager::add_pointcloud(const char *id,
00169                                   RefPtr<pcl::PointCloud<PointT> > cloud)
00170 {
00171   fawkes::MutexLocker lock(__clouds.mutex());
00172 
00173   if (__clouds.find(id) == __clouds.end()) {
00174     __clouds[id] = new PointCloudStorageAdapter<PointT>(cloud);
00175   } else {
00176     throw Exception("Cloud %s already registered");
00177   }
00178 }
00179 
00180 template <typename PointT>
00181 const RefPtr<const pcl::PointCloud<PointT> >
00182 PointCloudManager::get_pointcloud(const char *id)
00183 {
00184   fawkes::MutexLocker lock(__clouds.mutex());
00185 
00186   if (__clouds.find(id) != __clouds.end()) {
00187     PointCloudStorageAdapter<PointT> *pa =
00188       dynamic_cast<PointCloudStorageAdapter<PointT> *>(__clouds[id]);
00189 
00190     if (!pa) {
00191       // workaround for older compilers
00192       if (strcmp(__clouds[id]->get_typename(),
00193                  typeid(PointCloudStorageAdapter<PointT> *).name()) == 0)
00194       {
00195         return static_cast<PointCloudStorageAdapter<PointT> *>(__clouds[id])->cloud;
00196       }
00197 
00198       throw Exception("The desired point cloud is of a different type");
00199     }
00200     return pa->cloud;
00201   } else {
00202     throw Exception("No point cloud with ID '%s' registered", id);
00203   }
00204 }
00205 
00206 template <typename PointT>
00207 bool
00208 PointCloudManager::exists_pointcloud(const char *id)
00209 {
00210   try {
00211     const RefPtr<const pcl::PointCloud<PointT> > p = get_pointcloud<PointT>(id);
00212     return true;
00213   } catch (Exception &e) {
00214     return false;
00215   }
00216 
00217 }
00218 
00219 template <typename PointT>
00220 PointCloudManager::StorageAdapter *
00221 PointCloudManager::PointCloudStorageAdapter<PointT>::clone() const
00222 {
00223   return new PointCloudStorageAdapter<PointT>(this);
00224 }
00225 
00226 
00227 template <typename PointT>
00228 void
00229 PointCloudManager::PointCloudStorageAdapter<PointT>::get_time(fawkes::Time &time) const
00230 {
00231 #if HAVE_ROS_PCL
00232   time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
00233 #else
00234   fawkes::PointCloudTimestamp pclts;
00235   pclts.timestamp = cloud->header.stamp;
00236   time.set_time(pclts.time.sec, time.get_usec());
00237 #endif
00238 }
00239 
00240 
00241 
00242 } // end namespace fawkes
00243 
00244 #endif