Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "pcl_adapter.h"
23 
24 #include <pcl/point_cloud.h>
25 #include <pcl/point_types.h>
26 #include <pcl/ros/conversions.h>
27 #include <logging/logger.h>
28 #include <aspect/pointcloud/pointcloud_manager.h>
29 
30 using namespace fawkes;
31 
32 /// @cond INTERNALS
33 /** ROS to PCL storage adapter. */
34 class RosPointCloudAdapter::StorageAdapter
35 {
36  public:
37  /** Constructor.
38  * @param a_ adapter to clone */
39  StorageAdapter(const PointCloudManager::StorageAdapter *a_)
40  : a(a_->clone()) {}
41 
42  /** Destructor. */
43  ~StorageAdapter()
44  { delete a; }
45 
46  /** PCL Point cloud storage adapter to encapsulate. */
48 };
49 /// @endcond
50 
51 /** @class RosPointCloudAdapter "pcl_adapter.h"
52  * Standalone PCL to ROS adapter class.
53  * Currently, the standalone PCL comes with sensor_msgs and std_msgs
54  * data types which are incompatible with the ones that come with
55  * ROS. Hence, to use both in the same plugin, we need to confine the
56  * two different type instances into their own modules. While the rest
57  * of the ros-pcl plugins uses ROS-specific data types, this very
58  * class interacts with and only with the standalone PCL and the
59  * PointCloudManager. Interaction to the ROS parts is done by passing
60  * internal data types so that exchange can happen without common
61  * sensor_msgs or std_msgs data types.
62  * @author Tim Niemueller
63  */
64 
65 /** Constructor.
66  * @param pcl_manager PCL manager
67  * @param logger logger
68  */
70  Logger *logger)
71  : __pcl_manager(pcl_manager)
72 {
73 }
74 
75 
76 /** Destructor. */
78 {
79  std::map<std::string, StorageAdapter *>::iterator i;
80  for (i = __sas.begin(); i != __sas.end(); ++i) {
81  delete i->second;
82  }
83  __sas.clear();
84 }
85 
86 /** Fill information of arbitrary point type.
87  * @param p point cloud to get info from
88  * @param width upon return contains width of point cloud
89  * @param height upon return contains width of point cloud
90  * @param frame_id upon return contains frame ID of the point cloud
91  * @param is_dense upon return contains true if point cloud is dense and false otherwise
92  * @param pfi upon return contains data type information
93  */
94 template <typename PointT>
95 static void
96 fill_info(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &p,
97  unsigned int &width, unsigned int &height,
98  std::string &frame_id, bool &is_dense,
100 {
101  width = p->width;
102  height = p->height;
103  frame_id = p->header.frame_id;
104  is_dense = p->is_dense;
105 
106  std::vector<sensor_msgs::PointField> pfields;
107  pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>
108  (pcl::detail::FieldAdder<PointT>(pfields));
109 
110  pfi.clear();
111  pfi.resize(pfields.size());
112  for (unsigned int i = 0; i < pfields.size(); ++i) {
113  sensor_msgs::PointField &pf = pfields[i];
114  pfi[i] = RosPointCloudAdapter::PointFieldInfo(pf.name, pf.offset,
115  pf.datatype, pf.count);
116  }
117 }
118 
119 /** Get info about point cloud.
120  * @param id ID of point cloud to get info from
121  * @param width upon return contains width of point cloud
122  * @param height upon return contains width of point cloud
123  * @param frame_id upon return contains frame ID of the point cloud
124  * @param is_dense upon return contains true if point cloud is dense and false otherwise
125  * @param pfi upon return contains data type information
126  */
127 void
129  unsigned int &width, unsigned int &height,
130  std::string &frame_id, bool &is_dense,
131  V_PointFieldInfo &pfi)
132 {
133  if (__sas.find(id) == __sas.end()) {
134  __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str()));
135  }
136 
137  if (__pcl_manager->exists_pointcloud<pcl::PointXYZ>(id.c_str())) {
139  __pcl_manager->get_pointcloud<pcl::PointXYZ>(id.c_str());
140  fill_info(p, width, height, frame_id, is_dense, pfi);
141 
142  } else if (__pcl_manager->exists_pointcloud<pcl::PointXYZRGB>(id.c_str())) {
144  __pcl_manager->get_pointcloud<pcl::PointXYZRGB>(id.c_str());
145  fill_info(p, width, height, frame_id, is_dense, pfi);
146 
147  } else {
148  throw Exception("PointCloud '%s' does not exist or unknown type", id.c_str());
149  }
150 }
151 
152 
153 /** Get current data of point cloud.
154  * @param id ID of point cloud to get info from
155  * @param width upon return contains width of point cloud
156  * @param height upon return contains width of point cloud
157  * @param time capture time
158  * @param data_ptr upon return contains pointer to point cloud data
159  * @param point_size upon return contains size of a single point
160  * @param num_points upon return contains number of points
161  */
162 void
163 RosPointCloudAdapter::get_data(const std::string &id,
164  unsigned int &width, unsigned int &height, fawkes::Time &time,
165  void **data_ptr, size_t &point_size, size_t &num_points)
166 {
167  if (__sas.find(id) == __sas.end()) {
168  __sas[id] = new StorageAdapter(__pcl_manager->get_storage_adapter(id.c_str()));
169  }
170 
171  const PointCloudManager::StorageAdapter *sa = __sas[id]->a;
172  width = sa->width();
173  height = sa->height();
174  *data_ptr = sa->data_ptr();
175  point_size = sa->point_size();
176  num_points = sa->num_points();
177  sa->get_time(time);
178 }
179 
180 
181 /** Close an adapter.
182  * @param id ID of point cloud to close
183  */
184 void
185 RosPointCloudAdapter::close(const std::string &id)
186 {
187  if (__sas.find(id) != __sas.end()) {
188  delete __sas[id];
189  __sas.erase(id);
190  }
191 }
const StorageAdapter * get_storage_adapter(const char *id)
Get a storage adapter.
void get_data(const std::string &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.
Information about the data fields.
Definition: pcl_adapter.h:41
void close(const std::string &id)
Close an adapter.
A class for handling time.
Definition: time.h:91
void get_info(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 * data_ptr() const =0
Get pointer on data.
virtual unsigned int height() const =0
Get height of point cloud.
~RosPointCloudAdapter()
Destructor.
Definition: pcl_adapter.cpp:77
Point Cloud manager.
virtual size_t point_size() const =0
Get size of a point.
const RefPtr< const pcl::PointCloud< PointT > > get_pointcloud(const char *id)
Get point cloud.
Base class for exceptions in Fawkes.
Definition: exception.h:36
std::vector< PointFieldInfo > V_PointFieldInfo
Vector of PointFieldInfo.
Definition: pcl_adapter.h:61
RosPointCloudAdapter(fawkes::PointCloudManager *pcl_manager, fawkes::Logger *logger)
Constructor.
Definition: pcl_adapter.cpp:69
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.
bool exists_pointcloud(const char *id)
Check if point cloud exists.
virtual void get_time(fawkes::Time &time) const =0
Get last capture time.
virtual unsigned int width() const =0
Get width of point cloud.
Interface for logging.
Definition: logger.h:34