Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * pointcloud_manager.cpp - PointCloud manager for aspect 00004 * 00005 * Created: Sun Nov 06 23:49: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 #include <aspect/pointcloud/pointcloud_manager.h> 00025 00026 namespace fawkes { 00027 #if 0 /* just to make Emacs auto-indent happy */ 00028 } 00029 #endif 00030 00031 00032 /** @class PointCloudManager <aspect/pointcloud/pointcloud_manager.h> 00033 * Point Cloud manager. 00034 * This class manages a number of points clouds and acts as a hub to 00035 * distribute them. 00036 * @author Tim Niemueller 00037 * 00038 * @fn void PointCloudManager::add_pointcloud(const char *id, RefPtr<pcl::PointCloud<PointT> > cloud) 00039 * Add point cloud. 00040 * @param id ID of point cloud to add, must be unique 00041 * @param cloud refptr to point cloud 00042 * 00043 * @fn const RefPtr<const pcl::PointCloud<PointT> > PointCloudManager::get_pointcloud(const char *id) 00044 * Get point cloud. 00045 * @param id ID of point cloud to retrieve 00046 * @return point cloud 00047 * @exception Exception thrown if point cloud for given ID does not exist 00048 * 00049 */ 00050 00051 /** @class PointCloudManager::StorageAdapter <aspect/pointcloud/pointcloud_manager.h> 00052 * Adapter base class. 00053 * The adapter base class is required to store point clouds of 00054 * arbitrary point types. 00055 * @author Tim Niemueller 00056 * 00057 * @fn bool PointCloudManager::StorageAdapter::is_pointtype() const 00058 * Check if storage adapter is for specified point type. 00059 * @return true if storage adapter is for specified point type, false otherwise 00060 * 00061 * @fn template <PointT> PointCloudManager::PointCloudStorageAdapter<PointT> * PointCloudManager::StorageAdapter::as_pointtype() const 00062 * Transform to specific PointCloudStorageAdapter. 00063 * @return transformed PointCloudStorageAdapter 00064 * @exception Exception thrown if storage adapter is not of requested point type or does not exist 00065 * 00066 * @fn PointCloudManager::StorageAdapter * PointCloudManager::StorageAdapter::clone() const 00067 * Clone this storage adapter. 00068 * @return cloned copy 00069 * 00070 * @fn const char * PointCloudManager::StorageAdapter::get_typename() 00071 * Get typename of storage adapter. 00072 * @return type name 00073 * 00074 * @fn size_t PointCloudManager::StorageAdapter::point_size() const 00075 * Get size of a point. 00076 * @return size in bytes of a single point 00077 * 00078 * @fn unsigned int PointCloudManager::StorageAdapter::width() const 00079 * Get width of point cloud. 00080 * @return width of point cloud 00081 * 00082 * @fn unsigned int PointCloudManager::StorageAdapter::height() const 00083 * Get height of point cloud. 00084 * @return height of point cloud 00085 * 00086 * @fn size_t PointCloudManager::StorageAdapter::num_points() const 00087 * Get numer of points in point cloud. 00088 * @return number of points 00089 * 00090 * @fn void * PointCloudManager::StorageAdapter::data_ptr() const 00091 * Get pointer on data. 00092 * @return pointer on data 00093 * 00094 * @fn void PointCloudManager::StorageAdapter::get_time(fawkes::Time &time) const 00095 * Get last capture time. 00096 * @param time upon return contains last capture time 00097 */ 00098 00099 00100 /** Virtual empty destructor. */ 00101 PointCloudManager::StorageAdapter::~StorageAdapter() 00102 { 00103 } 00104 00105 00106 /** @class PointCloudManager::PointCloudStorageAdapter <aspect/pointcloud/pointcloud_manager.h> 00107 * Adapter class for PCL point types. 00108 * The adapter class is required to store point clouds of arbitrary point types. 00109 * @author Tim Niemueller 00110 */ 00111 00112 /** Constructor. */ 00113 PointCloudManager::PointCloudManager() 00114 { 00115 } 00116 00117 /** Destructor. */ 00118 PointCloudManager::~PointCloudManager() 00119 { 00120 LockMap<std::string, StorageAdapter *>::iterator c; 00121 for (c = __clouds.begin(); c != __clouds.end(); ++c) { 00122 delete c->second; 00123 } 00124 00125 __clouds.clear(); 00126 } 00127 00128 00129 /** Remove the point cloud. 00130 * @param id ID of point cloud to remove 00131 */ 00132 void 00133 PointCloudManager::remove_pointcloud(const char *id) 00134 { 00135 MutexLocker lock(__clouds.mutex()); 00136 00137 if (__clouds.find(id) != __clouds.end()) { 00138 delete __clouds[id]; 00139 __clouds.erase(id); 00140 } 00141 } 00142 00143 /** Check if point cloud exists 00144 * @param id ID of point cloud to check 00145 * @return true if the point cloud exists, false otherwise 00146 */ 00147 bool 00148 PointCloudManager::exists_pointcloud(const char *id) 00149 { 00150 MutexLocker lock(__clouds.mutex()); 00151 00152 return (__clouds.find(id) != __clouds.end()); 00153 } 00154 00155 00156 /** Get list of point cloud IDs. 00157 * @return list of point cloud IDs 00158 */ 00159 std::vector<std::string> 00160 PointCloudManager::get_pointcloud_list() const 00161 { 00162 MutexLocker lock(__clouds.mutex()); 00163 00164 std::vector<std::string> rv; 00165 rv.clear(); 00166 LockMap<std::string, StorageAdapter *>::const_iterator c; 00167 for (c = __clouds.begin(); c != __clouds.end(); ++c) { 00168 rv.push_back(c->first); 00169 } 00170 return rv; 00171 } 00172 00173 00174 /** Get map of point clouds. 00175 * Use with care. Do not use in ROS-enabled plugins unless you are aware 00176 * of sensor_msgs and std_msgs incompatibilities between standalone PCL 00177 * and ROS! 00178 * @return map from ID to storage adapter 00179 */ 00180 const fawkes::LockMap<std::string, PointCloudManager::StorageAdapter *> & 00181 PointCloudManager::get_pointclouds() const 00182 { 00183 return __clouds; 00184 } 00185 00186 00187 /** Get a storage adapter. 00188 * Use with care. Do not use in ROS-enabled plugins unless you are aware 00189 * of sensor_msgs and std_msgs incompatibilities between standalone PCL 00190 * and ROS! 00191 * @param id ID of point clouds whose storage adapter to retrieve 00192 * @return storage adapter for given ID 00193 * @exception Exception thrown if ID is unknown 00194 */ 00195 const PointCloudManager::StorageAdapter * 00196 PointCloudManager::get_storage_adapter(const char *id) 00197 { 00198 MutexLocker lock(__clouds.mutex()); 00199 00200 if (__clouds.find(id) == __clouds.end()) { 00201 throw Exception("PointCloud '%s' unknown", id); 00202 } 00203 return __clouds[id]; 00204 } 00205 00206 00207 00208 } // end namespace fawkes