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