Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
pointcloud_manager.h
1 
2 /***************************************************************************
3  * pointcloud_manager.h - PointCloud manager for aspect
4  *
5  * Created: Sun Nov 06 23:29:36 2011
6  * Copyright 2011 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #ifndef __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
25 #define __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
26 
27 #include <aspect/aspect.h>
28 #include <core/exception.h>
29 #include <core/utils/refptr.h>
30 #include <core/utils/lock_map.h>
31 #include <core/threading/mutex_locker.h>
32 #include <utils/time/time.h>
33 
34 #include <vector>
35 #include <string>
36 #include <stdint.h>
37 #include <typeinfo>
38 #include <cstring>
39 
40 namespace pcl {
41  template <typename PointT>
42  class PointCloud;
43 }
44 
45 namespace fawkes {
46 #if 0 /* just to make Emacs auto-indent happy */
47 }
48 #endif
49 
50 /** Union to pack fawkes::Time into the pcl::PointCloud timestamp. */
51 typedef union {
52  struct {
53  uint64_t sec : 44; ///< seconds part of time
54  uint64_t usec : 20; ///< microseconds part of time
55  } time; ///< Access timestamp as time
56  uint64_t timestamp; ///< Access timestamp as number only
58 
59 
61 {
62  public:
64  virtual ~PointCloudManager();
65 
66  template <typename PointT>
67  void add_pointcloud(const char *id, RefPtr<pcl::PointCloud<PointT> > cloud);
68 
69  void remove_pointcloud(const char *id);
70 
71  template <typename PointT>
72  const RefPtr<const pcl::PointCloud<PointT> > get_pointcloud(const char *id);
73  bool exists_pointcloud(const char *id);
74 
75  /** Check if point cloud of specified type exists.
76  * @param id ID of point cloud to check
77  * @return true if the point cloud exists, false otherwise
78  */
79  template <typename PointT>
80  bool exists_pointcloud(const char *id);
81 
82  template <typename PointT>
84 
86  public:
87  virtual ~StorageAdapter();
88 
89  template <typename PointT>
90  bool is_pointtype() const;
91 
92  template <typename PointT>
93  PointCloudStorageAdapter<PointT> * as_pointtype();
94 
95  virtual const char * get_typename() = 0;
96  virtual StorageAdapter * clone() const = 0;
97  virtual size_t point_size() const = 0;
98  virtual unsigned int width() const = 0;
99  virtual unsigned int height() const = 0;
100  virtual size_t num_points() const = 0;
101  virtual void * data_ptr() const = 0;
102  virtual void get_time(fawkes::Time &time) const = 0;
103  };
104 
105  template <typename PointT>
107  {
108  public:
109  /** Constructor.
110  * @param cloud cloud to encapsulate.
111  */
113  : cloud(cloud) {}
114 
115  /** Copy constructor.
116  * @param p storage adapter to copy
117  */
119  : cloud(p->cloud) {}
120 
121  /** The point cloud. */
123 
124  virtual StorageAdapter * clone() const;
125 
126  virtual const char * get_typename() { return typeid(this).name(); }
127  virtual size_t point_size() const { return sizeof(PointT); }
128  virtual unsigned int width() const { return cloud->width; }
129  virtual unsigned int height() const { return cloud->height; }
130  virtual size_t num_points() const { return cloud->points.size(); }
131  virtual void * data_ptr() const { return &cloud->points[0]; }
132  virtual void get_time(fawkes::Time &time) const;
133  };
134 
135  std::vector<std::string> get_pointcloud_list() const;
136  const fawkes::LockMap<std::string, StorageAdapter *> & get_pointclouds() const;
137  const StorageAdapter * get_storage_adapter(const char *id);
138 
139  private:
141 };
142 
143 
144 template <typename PointT>
145 bool
146 PointCloudManager::StorageAdapter::is_pointtype() const
147 {
149  dynamic_cast<const PointCloudStorageAdapter<PointT> *>(this);
150  return (!!pa);
151 }
152 
153 
154 template <typename PointT>
156 PointCloudManager::StorageAdapter::as_pointtype()
157 {
159  dynamic_cast<PointCloudStorageAdapter<PointT> *>(this);
160  if (!pa) {
161  throw Exception("PointCloud storage adapter is not of anticipated type");
162  }
163  return pa;
164 }
165 
166 template <typename PointT>
167 void
168 PointCloudManager::add_pointcloud(const char *id,
170 {
171  fawkes::MutexLocker lock(__clouds.mutex());
172 
173  if (__clouds.find(id) == __clouds.end()) {
174  __clouds[id] = new PointCloudStorageAdapter<PointT>(cloud);
175  } else {
176  throw Exception("Cloud %s already registered");
177  }
178 }
179 
180 template <typename PointT>
182 PointCloudManager::get_pointcloud(const char *id)
183 {
184  fawkes::MutexLocker lock(__clouds.mutex());
185 
186  if (__clouds.find(id) != __clouds.end()) {
188  dynamic_cast<PointCloudStorageAdapter<PointT> *>(__clouds[id]);
189 
190  if (!pa) {
191  // workaround for older compilers
192  if (strcmp(__clouds[id]->get_typename(),
193  typeid(PointCloudStorageAdapter<PointT> *).name()) == 0)
194  {
195  return static_cast<PointCloudStorageAdapter<PointT> *>(__clouds[id])->cloud;
196  }
197 
198  throw Exception("The desired point cloud is of a different type");
199  }
200  return pa->cloud;
201  } else {
202  throw Exception("No point cloud with ID '%s' registered", id);
203  }
204 }
205 
206 template <typename PointT>
207 bool
208 PointCloudManager::exists_pointcloud(const char *id)
209 {
210  try {
211  const RefPtr<const pcl::PointCloud<PointT> > p = get_pointcloud<PointT>(id);
212  return true;
213  } catch (Exception &e) {
214  return false;
215  }
216 
217 }
218 
219 template <typename PointT>
220 PointCloudManager::StorageAdapter *
222 {
223  return new PointCloudStorageAdapter<PointT>(this);
224 }
225 
226 
227 template <typename PointT>
228 void
230 {
231 #if HAVE_ROS_PCL
232  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
233 #else
235  pclts.timestamp = cloud->header.stamp;
236  time.set_time(pclts.time.sec, time.get_usec());
237 #endif
238 }
239 
240 
241 
242 } // end namespace fawkes
243 
244 #endif
virtual size_t point_size() const
Get size of a point.
virtual unsigned int height() const
Get height of point cloud.
virtual size_t num_points() const
Get numer of points in point cloud.
uint64_t sec
seconds part of time
Mutex locking helper.
Definition: mutex_locker.h:33
long get_usec() const
Get microseconds.
Definition: time.h:112
A class for handling time.
Definition: time.h:91
virtual unsigned int width() const
Get width of point cloud.
Map with a lock.
Definition: lock_map.h:37
PointCloudStorageAdapter(RefPtr< pcl::PointCloud< PointT > > cloud)
Constructor.
struct fawkes::PointCloudTimestamp::@0 time
Access timestamp as time.
Point Cloud manager.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Union to pack fawkes::Time into the pcl::PointCloud timestamp.
const RefPtr< pcl::PointCloud< PointT > > cloud
The point cloud.
virtual void * data_ptr() const
Get pointer on data.
RefPtr&lt;&gt; is a reference-counting shared smartpointer.
Definition: refptr.h:49
void set_time(const timeval *tv)
Sets the time.
Definition: time.cpp:262
uint64_t timestamp
Access timestamp as number only.
virtual const char * get_typename()
Get typename of storage adapter.
PointCloudStorageAdapter(const PointCloudStorageAdapter< PointT > *p)
Copy constructor.