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_) : a(a_->clone())
43  {
44  }
45 
46  /** Destructor. */
47  ~StorageAdapter()
48  {
49  delete a;
50  }
51 
52  /** PCL Point cloud storage adapter to encapsulate. */
54 };
55 /// @endcond
56 
57 /** @class PointCloudAdapter <pcl_utils/pcl_adapter.h>
58  * Point cloud adapter class.
59  * Currently, the standalone PCL comes with sensor_msgs and std_msgs
60  * data types which are incompatible with the ones that come with
61  * ROS. Hence, to use both in the same plugin, we need to confine the
62  * two different type instances into their own modules. While the rest
63  * of the ros-pcl plugins uses ROS-specific data types, this very
64  * class interacts with and only with the standalone PCL and the
65  * PointCloudManager. Interaction to the ROS parts is done by passing
66  * internal data types so that exchange can happen without common
67  * sensor_msgs or std_msgs data types.
68  * @author Tim Niemueller
69  */
70 
71 /** Constructor.
72  * @param pcl_manager PCL manager
73  * @param logger logger
74  */
76 : pcl_manager_(pcl_manager)
77 {
78 }
79 
80 /** Destructor. */
82 {
83  std::map<std::string, StorageAdapter *>::iterator i;
84  for (i = sas_.begin(); i != sas_.end(); ++i) {
85  delete i->second;
86  }
87  sas_.clear();
88 }
89 
90 /** Fill information of arbitrary point type.
91  * @param p point cloud to get info from
92  * @param width upon return contains width of point cloud
93  * @param height upon return contains width of point cloud
94  * @param frame_id upon return contains frame ID of the point cloud
95  * @param is_dense upon return contains true if point cloud is dense and false otherwise
96  * @param pfi upon return contains data type information
97  */
98 template <typename PointT>
99 static void
100 fill_info(const fawkes::RefPtr<const pcl::PointCloud<PointT>> &p,
101  unsigned int & width,
102  unsigned int & height,
103  std::string & frame_id,
104  bool & is_dense,
106 {
107  width = p->width;
108  height = p->height;
109  frame_id = p->header.frame_id;
110  is_dense = p->is_dense;
111 
112 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
113  std::vector<pcl::PCLPointField> pfields;
114 #else
115  std::vector<sensor_msgs::PointField> pfields;
116 #endif
117  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(
118  pcl::detail::FieldAdder<PointT>(pfields));
119 
120  pfi.clear();
121  pfi.resize(pfields.size());
122  for (unsigned int i = 0; i < pfields.size(); ++i) {
123 #if PCL_VERSION_COMPARE(>=, 1, 7, 0)
124  pcl::PCLPointField &pf = pfields[i];
125 #else
126  sensor_msgs::PointField &pf = pfields[i];
127 #endif
128  pfi[i] = PointCloudAdapter::PointFieldInfo(pf.name, pf.offset, pf.datatype, pf.count);
129  }
130 }
131 
132 /** Get info about point cloud.
133  * @param id ID of point cloud to get info from
134  * @param width upon return contains width of point cloud
135  * @param height upon return contains width of point cloud
136  * @param frame_id upon return contains frame ID of the point cloud
137  * @param is_dense upon return contains true if point cloud is dense and false otherwise
138  * @param pfi upon return contains data type information
139  */
140 void
141 PointCloudAdapter::get_info(const std::string &id,
142  unsigned int & width,
143  unsigned int & height,
144  std::string & frame_id,
145  bool & is_dense,
146  V_PointFieldInfo & pfi)
147 {
148  if (sas_.find(id) == sas_.end()) {
149  sas_[id] = new StorageAdapter(pcl_manager_->get_storage_adapter(id.c_str()));
150  }
151 
152  if (pcl_manager_->exists_pointcloud<pcl::PointXYZ>(id.c_str())) {
154  pcl_manager_->get_pointcloud<pcl::PointXYZ>(id.c_str());
155  fill_info(p, width, height, frame_id, is_dense, pfi);
156 
157  } else if (pcl_manager_->exists_pointcloud<pcl::PointXYZRGB>(id.c_str())) {
159  pcl_manager_->get_pointcloud<pcl::PointXYZRGB>(id.c_str());
160  fill_info(p, width, height, frame_id, is_dense, pfi);
161 
162  } else if (pcl_manager_->exists_pointcloud<pcl::PointXYZL>(id.c_str())) {
164  pcl_manager_->get_pointcloud<pcl::PointXYZL>(id.c_str());
165  fill_info(p, width, height, frame_id, is_dense, pfi);
166 
167  } else {
168  throw Exception("PointCloud '%s' does not exist or unknown type", id.c_str());
169  }
170 }
171 
172 /** Get current data of point cloud.
173  * @param id ID of point cloud to get info from
174  * @param frame_id upon return contains the frame ID of the point cloud
175  * @param width upon return contains width of point cloud
176  * @param height upon return contains width of point cloud
177  * @param time capture time
178  * @param data_ptr upon return contains pointer to point cloud data
179  * @param point_size upon return contains size of a single point
180  * @param num_points upon return contains number of points
181  */
182 void
183 PointCloudAdapter::get_data(const std::string &id,
184  std::string & frame_id,
185  unsigned int & width,
186  unsigned int & height,
187  fawkes::Time & time,
188  void ** data_ptr,
189  size_t & point_size,
190  size_t & num_points)
191 {
192  if (sas_.find(id) == sas_.end()) {
193  sas_[id] = new StorageAdapter(pcl_manager_->get_storage_adapter(id.c_str()));
194  }
195 
196  const pcl_utils::StorageAdapter *sa = sas_[id]->a;
197  frame_id = sa->frame_id();
198  width = sa->width();
199  height = sa->height();
200  *data_ptr = sa->data_ptr();
201  point_size = sa->point_size();
202  num_points = sa->num_points();
203  sa->get_time(time);
204 }
205 
206 /** Get data and info of point cloud.
207  * @param id ID of point cloud to get info from
208  * @param frame_id upon return contains frame ID of the point cloud
209  * @param is_dense upon return contains true if point cloud is dense and false otherwise
210  * @param width upon return contains width of point cloud
211  * @param height upon return contains width of point cloud
212  * @param time capture time for which to get the data and info
213  * @param pfi upon return contains data type information
214  * @param data_ptr upon return contains pointer to point cloud data
215  * @param point_size upon return contains size of a single point
216  * @param num_points upon return contains number of points
217  */
218 void
220  std::string & frame_id,
221  bool & is_dense,
222  unsigned int & width,
223  unsigned int & height,
224  fawkes::Time & time,
225  V_PointFieldInfo & pfi,
226  void ** data_ptr,
227  size_t & point_size,
228  size_t & num_points)
229 {
230  get_info(id, width, height, frame_id, is_dense, pfi);
231  get_data(id, frame_id, width, height, time, data_ptr, point_size, num_points);
232 }
233 
234 /** Close an adapter.
235  * @param id ID of point cloud to close
236  */
237 void
238 PointCloudAdapter::close(const std::string &id)
239 {
240  if (sas_.find(id) != sas_.end()) {
241  delete sas_[id];
242  sas_.erase(id);
243  }
244 }
fawkes::PointCloudManager::exists_pointcloud
bool exists_pointcloud(const char *id)
Check if point cloud exists.
Definition: pointcloud_manager.cpp:81
fawkes::pcl_utils::StorageAdapter::point_size
virtual size_t point_size() const =0
Get size of a point.
fawkes::pcl_utils::StorageAdapter::height
virtual unsigned int height() const =0
Get height of point cloud.
PointCloudAdapter::V_PointFieldInfo
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:66
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
PointCloudAdapter::PointFieldInfo
Information about the data fields.
Definition: pcl_adapter.h:43
fawkes::pcl_utils::StorageAdapter::width
virtual unsigned int width() const =0
Get width of point cloud.
fawkes::PointCloudManager
Point Cloud manager.
Definition: pointcloud_manager.h:47
fawkes::pcl_utils::StorageAdapter::num_points
virtual size_t num_points() const =0
Get numer of points in point cloud.
PointCloudAdapter::close
void close(const std::string &id)
Close an adapter.
Definition: pcl_adapter.cpp:238
fawkes::pcl_utils::StorageAdapter::get_time
virtual void get_time(fawkes::Time &time) const =0
Get last capture time.
PointCloudAdapter::PointCloudAdapter
PointCloudAdapter(fawkes::PointCloudManager *pcl_manager, fawkes::Logger *logger)
Constructor.
Definition: pcl_adapter.cpp:75
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes
Fawkes library namespace.
fawkes::PointCloudManager::get_pointcloud
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Definition: pointcloud_manager.h:91
fawkes::pcl_utils::StorageAdapter
Adapter base class.
Definition: storage_adapter.h:37
pcl::PointCloud
Definition: pointcloud.h:32
fawkes::PointCloudManager::get_storage_adapter
const pcl_utils::StorageAdapter * get_storage_adapter(const char *id)
Get a storage adapter.
Definition: pointcloud_manager.cpp:126
PointCloudAdapter::get_data_and_info
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.
Definition: pcl_adapter.cpp:219
fawkes::pcl_utils::StorageAdapter::data_ptr
virtual void * data_ptr() const =0
Get pointer on data.
fawkes::pcl_utils::StorageAdapter::frame_id
virtual std::string frame_id() const =0
Get frame ID of point cloud.
fawkes::Time
A class for handling time.
Definition: time.h:93
PointCloudAdapter::get_data
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.
Definition: pcl_adapter.cpp:183
PointCloudAdapter::get_info
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.
Definition: pcl_adapter.cpp:141
PointCloudAdapter::~PointCloudAdapter
~PointCloudAdapter()
Destructor.
Definition: pcl_adapter.cpp:81
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36