24 #ifndef __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
25 #define __ASPECT_POINTCLOUD_POINTCLOUD_MANAGER_H_
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>
41 template <
typename Po
intT>
66 template <
typename Po
intT>
69 void remove_pointcloud(
const char *
id);
71 template <
typename Po
intT>
73 bool exists_pointcloud(
const char *
id);
79 template <
typename Po
intT>
80 bool exists_pointcloud(
const char *
id);
82 template <
typename Po
intT>
89 template <
typename Po
intT>
90 bool is_pointtype()
const;
92 template <
typename Po
intT>
95 virtual const char * get_typename() = 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;
105 template <
typename Po
intT>
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]; }
135 std::vector<std::string> get_pointcloud_list()
const;
137 const StorageAdapter * get_storage_adapter(
const char *
id);
144 template <
typename Po
intT>
146 PointCloudManager::StorageAdapter::is_pointtype()
const
154 template <
typename Po
intT>
156 PointCloudManager::StorageAdapter::as_pointtype()
161 throw Exception(
"PointCloud storage adapter is not of anticipated type");
166 template <
typename Po
intT>
168 PointCloudManager::add_pointcloud(
const char *
id,
173 if (__clouds.find(
id) == __clouds.end()) {
176 throw Exception(
"Cloud %s already registered");
180 template <
typename Po
intT>
182 PointCloudManager::get_pointcloud(
const char *
id)
186 if (__clouds.find(
id) != __clouds.end()) {
192 if (strcmp(__clouds[
id]->get_typename(),
198 throw Exception(
"The desired point cloud is of a different type");
202 throw Exception(
"No point cloud with ID '%s' registered",
id);
206 template <
typename Po
intT>
208 PointCloudManager::exists_pointcloud(
const char *
id)
213 }
catch (Exception &e) {
219 template <
typename Po
intT>
220 PointCloudManager::StorageAdapter *
227 template <
typename Po
intT>
232 time.
set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);