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