Fawkes API  Fawkes Development Version
pcl_adapter.cpp
1 
2 /***************************************************************************
3  * pcl_adapter.cpp - PCL exchange publisher manager
4  *
5  * Created: Tue Nov 08 00:38:34 2011
6  * Copyright 2011-2014 Tim Niemueller [www.niemueller.de]
7  * 2012 Bastian Klingen
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.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "pcl_adapter.h"
24 
25 #include <pcl/point_cloud.h>
26 #include <pcl/point_types.h>
27 #if PCL_VERSION_COMPARE(>=,1,7,0)
28 # include <pcl/PCLPointField.h>
29 #endif
30 #include <logging/logger.h>
31 #include <pcl_utils/pointcloud_manager.h>
32 
33 using namespace fawkes;
34 
35 /// @cond INTERNALS
36 /** ROS to PCL storage adapter. */
37 class PointCloudAdapter::StorageAdapter
38 {
39  public:
40  /** Constructor.
41  * @param a_ adapter to clone */
42  StorageAdapter(const pcl_utils::StorageAdapter *a_)
43  : a(a_->clone()) {}
44 
45  /** Destructor. */
46  ~StorageAdapter()
47  { delete a; }
48 
49  /** PCL Point cloud storage adapter to encapsulate. */
51 };
52 /// @endcond
53 
54 /** @class PointCloudAdapter <pcl_utils/pcl_adapter.h>
55  * Point cloud adapter class.
56  * Currently, the standalone PCL comes with sensor_msgs and std_msgs
57  * data types which are incompatible with the ones that come with
58  * ROS. Hence, to use both in the same plugin, we need to confine the
59  * two different type instances into their own modules. While the rest
60  * of the ros-pcl plugins uses ROS-specific data types, this very
61  * class interacts with and only with the standalone PCL and the
62  * PointCloudManager. Interaction to the ROS parts is done by passing
63  * internal data types so that exchange can happen without common
64  * sensor_msgs or std_msgs data types.
65  * @author Tim Niemueller
66  */
67 
68 /** Constructor.
69  * @param pcl_manager PCL manager
70  * @param logger logger
71  */
73  Logger *logger)
74  : __pcl_manager(pcl_manager)
75 {
76 }
77 
78 
79 /** Destructor. */
81 {
82  std::map<std::string, StorageAdapter *>::iterator i;
83  for (i = __sas.begin(); i != __sas.end(); ++i) {
84  delete i->second;
85  }
86  __sas.clear();
87 }
88 
89 /** Fill information of arbitrary point type.
90  * @param p point cloud to get info from
91  * @param width upon return contains width of point cloud
92  * @param height upon return contains width of point cloud
93  * @param frame_id upon return contains frame ID of the point cloud
94  * @param is_dense upon return contains true if point cloud is dense and false otherwise
95  * @param pfi upon return contains data type information
96  */
97 template <typename PointT>
98 static void
99 fill_info(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &p,
100  unsigned int &width, unsigned int &height,
101  std::string &frame_id, bool &is_dense,
103 {
104  width = p->width;
105  height = p->height;
106  frame_id = p->header.frame_id;
107  is_dense = p->is_dense;
108 
109 #if PCL_VERSION_COMPARE(>=,1,7,0)
110  std::vector<pcl::PCLPointField> pfields;
111 #else
112  std::vector<sensor_msgs::PointField> pfields;
113 #endif
114  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>
115  (pcl::detail::FieldAdder<PointT>(pfields));
116 
117  pfi.clear();
118  pfi.resize(pfields.size());
119  for (unsigned int i = 0; i < pfields.size(); ++i) {
120 #if PCL_VERSION_COMPARE(>=,1,7,0)
121  pcl::PCLPointField &pf = pfields[i];
122 #else
123  sensor_msgs::PointField &pf = pfields[i];
124 #endif
125  pfi[i] = PointCloudAdapter::PointFieldInfo(pf.name, pf.offset,
126  pf.datatype, pf.count);
127  }
128 }
129 
130 /** Get info about point cloud.
131  * @param id ID of point cloud to get info from
132  * @param width upon return contains width of point cloud
133  * @param height upon return contains width of point cloud
134  * @param frame_id upon return contains frame ID of the point cloud
135  * @param is_dense upon return contains true if point cloud is dense and false otherwise
136  * @param pfi upon return contains data type information
137  */
138 void
139 PointCloudAdapter::get_info(const std::string &id,
140  unsigned int &width, unsigned int &height,
141  std::string &frame_id, bool &is_dense,
142  V_PointFieldInfo &pfi)
143 {
144  if (__sas.find(id) == __sas.end()) {
145  __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str()));
146  }
147 
148  if (__pcl_manager->exists_pointcloud<pcl::PointXYZ>(id.c_str())) {
150  __pcl_manager->get_pointcloud<pcl::PointXYZ>(id.c_str());
151  fill_info(p, width, height, frame_id, is_dense, pfi);
152 
153  } else if (__pcl_manager->exists_pointcloud<pcl::PointXYZRGB>(id.c_str())) {
155  __pcl_manager->get_pointcloud<pcl::PointXYZRGB>(id.c_str());
156  fill_info(p, width, height, frame_id, is_dense, pfi);
157 
158  } else if (__pcl_manager->exists_pointcloud<pcl::PointXYZL>(id.c_str())) {
160  __pcl_manager->get_pointcloud<pcl::PointXYZL>(id.c_str());
161  fill_info(p, width, height, frame_id, is_dense, pfi);
162 
163  } else {
164  throw Exception("PointCloud '%s' does not exist or unknown type", id.c_str());
165  }
166 }
167 
168 
169 /** Get current data of point cloud.
170  * @param id ID of point cloud to get info from
171  * @param frame_id upon return contains the frame ID of the point cloud
172  * @param width upon return contains width of point cloud
173  * @param height upon return contains width of point cloud
174  * @param time capture time
175  * @param data_ptr upon return contains pointer to point cloud data
176  * @param point_size upon return contains size of a single point
177  * @param num_points upon return contains number of points
178  */
179 void
180 PointCloudAdapter::get_data(const std::string &id, std::string &frame_id,
181  unsigned int &width, unsigned int &height, fawkes::Time &time,
182  void **data_ptr, size_t &point_size, size_t &num_points)
183 {
184  if (__sas.find(id) == __sas.end()) {
185  __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str()));
186  }
187 
188  const pcl_utils::StorageAdapter *sa = __sas[id]->a;
189  frame_id = sa->frame_id();
190  width = sa->width();
191  height = sa->height();
192  *data_ptr = sa->data_ptr();
193  point_size = sa->point_size();
194  num_points = sa->num_points();
195  sa->get_time(time);
196 }
197 
198 
199 /** Get data and info of point cloud.
200  * @param id ID of point cloud to get info from
201  * @param frame_id upon return contains frame ID of the point cloud
202  * @param is_dense upon return contains true if point cloud is dense and false otherwise
203  * @param width upon return contains width of point cloud
204  * @param height upon return contains width of point cloud
205  * @param time capture time for which to get the data and info
206  * @param pfi upon return contains data type information
207  * @param data_ptr upon return contains pointer to point cloud data
208  * @param point_size upon return contains size of a single point
209  * @param num_points upon return contains number of points
210  */
211 void
212 PointCloudAdapter::get_data_and_info(const std::string &id, std::string &frame_id, bool &is_dense,
213  unsigned int &width, unsigned int &height, fawkes::Time &time,
214  V_PointFieldInfo &pfi,
215  void **data_ptr, size_t &point_size, size_t &num_points)
216 {
217  get_info(id, width, height, frame_id, is_dense, pfi);
218  get_data(id, frame_id, width, height, time, data_ptr, point_size, num_points);
219 }
220 
221 
222 /** Close an adapter.
223  * @param id ID of point cloud to close
224  */
225 void
226 PointCloudAdapter::close(const std::string &id)
227 {
228  if (__sas.find(id) != __sas.end()) {
229  delete __sas[id];
230  __sas.erase(id);
231  }
232 }
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:62
void close(const std::string &id)
Close an adapter.
Fawkes library namespace.
virtual unsigned int width() const =0
Get width of point cloud.
Information about the data fields.
Definition: pcl_adapter.h:42
~PointCloudAdapter()
Destructor.
Definition: pcl_adapter.cpp:80
A class for handling time.
Definition: time.h:91
virtual size_t point_size() const =0
Get size of a point.
virtual unsigned int height() const =0
Get height of point cloud.
void get_data_and_info(const std::string &id, std::string &frame_id, bool &is_dense, unsigned int &width, unsigned int &height, fawkes::Time &time, V_PointFieldInfo &pfi, void **data_ptr, size_t &point_size, size_t &num_points)
Get data and info of point cloud.
virtual std::string frame_id() const =0
Get frame ID of point cloud.
Point Cloud manager.
virtual void * data_ptr() const =0
Get pointer on data.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Base class for exceptions in Fawkes.
Definition: exception.h:36
PointCloudAdapter(fawkes::PointCloudManager *pcl_manager, fawkes::Logger *logger)
Constructor.
Definition: pcl_adapter.cpp:72
void get_info(const std::string &id, unsigned int &width, unsigned int &height, std::string &frame_id, bool &is_dense, V_PointFieldInfo &pfi)
Get info about point cloud.
virtual void get_time(fawkes::Time &time) const =0
Get last capture time.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual size_t num_points() const =0
Get numer of points in point cloud.
void get_data(const std::string &id, std::string &frame_id, unsigned int &width, unsigned int &height, fawkes::Time &time, void **data_ptr, size_t &point_size, size_t &num_points)
Get current data of point cloud.
const pcl_utils::StorageAdapter * get_storage_adapter(const char *id)
Get a storage adapter.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
Interface for logging.
Definition: logger.h:34