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
33using namespace fawkes;
34
35/// @cond INTERNALS
36/** ROS to PCL storage adapter. */
37class PointCloudAdapter::StorageAdapter
38{
39public:
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 */
98template <typename PointT>
99static void
100fill_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 */
140void
141PointCloudAdapter::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 */
182void
183PointCloudAdapter::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 */
218void
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 */
237void
238PointCloudAdapter::close(const std::string &id)
239{
240 if (sas_.find(id) != sas_.end()) {
241 delete sas_[id];
242 sas_.erase(id);
243 }
244}
Information about the data fields.
Definition: pcl_adapter.h:43
PointCloudAdapter(fawkes::PointCloudManager *pcl_manager, fawkes::Logger *logger)
Constructor.
Definition: pcl_adapter.cpp:75
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.
void close(const std::string &id)
Close an adapter.
~PointCloudAdapter()
Destructor.
Definition: pcl_adapter.cpp:81
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.
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.
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:66
Base class for exceptions in Fawkes.
Definition: exception.h:36
Interface for logging.
Definition: logger.h:42
Point Cloud manager.
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.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
A class for handling time.
Definition: time.h:93
virtual size_t point_size() const =0
Get size of a point.
virtual unsigned int width() const =0
Get width of point cloud.
virtual unsigned int height() const =0
Get height of point cloud.
virtual size_t num_points() const =0
Get numer of points in point cloud.
virtual void get_time(fawkes::Time &time) const =0
Get last capture time.
virtual std::string frame_id() const =0
Get frame ID of point cloud.
virtual void * data_ptr() const =0
Get pointer on data.
Fawkes library namespace.