Fawkes API  Fawkes Development Version
utils.h
1 
2 /***************************************************************************
3  * utils.h - General PCL utilities
4  *
5  * Created: Tue Nov 08 17:50:07 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 #ifndef __LIBS_PCL_UTILS_UTILS_H_
23 #define __LIBS_PCL_UTILS_UTILS_H_
24 
25 #include <pcl/point_cloud.h>
26 #include <pcl/console/print.h>
27 #include <core/utils/refptr.h>
28 #include <utils/time/time.h>
29 #include <config/config.h>
30 
31 namespace fawkes {
32  namespace pcl_utils {
33 #if 0 /* just to make Emacs auto-indent happy */
34  }
35 }
36 #endif
37 
38 /** Union to pack fawkes::Time into the pcl::PointCloud timestamp. */
39 typedef union {
40  struct {
41  uint64_t sec : 44; ///< seconds part of time
42  uint64_t usec : 20; ///< microseconds part of time
43  } time; ///< Access timestamp as time
44  uint64_t timestamp; ///< Access timestamp as number only
46 
47 
48 /** Call this function to make PCL shutup.
49  * Warning: this makes PCL completely quiet for everything using
50  * PCL in the same process.
51  */
52 inline void
53 shutup()
54 {
55  pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
56 }
57 
58 
59 /** Shutup PCL based on configuration.
60  * Check the configuration flag /pcl/shutup and call pcl_utils::shutup() if
61  * it is set to true.
62  * @param config config to query for value
63  */
64 inline void
65 shutup_conditional(Configuration *config)
66 {
67  bool pcl_shutup = false;
68  try {
69  pcl_shutup = config->get_bool("/pcl/shutup");
70  } catch (Exception &e) {} // ignore, use default
71  if (pcl_shutup) ::fawkes::pcl_utils::shutup();
72 }
73 
74 
75 /** Set time of a point cloud from a fawkes::Time instance.
76  * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL
77  * timestamp field (if non-ROS PCL is used).
78  * @param cloud cloud of which to set the time
79  * @param time time to use
80  */
81 template <typename PointT>
82 inline void
83 set_time(pcl::PointCloud<PointT> &cloud, const fawkes::Time &time)
84 {
85 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
86  cloud.header.stamp.sec = time.get_sec();
87  cloud.header.stamp.nsec = time.get_usec() * 1000;
88 #else
89 # if PCL_VERSION_COMPARE(>=,1,7,0)
90  cloud.header.stamp = time.in_usec();
91 # else
92  PointCloudTimestamp pclts;
93  pclts.time.sec = time.get_sec();
94  pclts.time.usec = time.get_usec();
95  cloud.header.stamp = pclts.timestamp;
96 # endif
97 #endif
98 }
99 
100 /** Set time of a point cloud from a fawkes::Time instance.
101  * This uses the PointCloudTimestamp struct to set the time in the PCL
102  * timestamp field (if non-ROS PCL is used).
103  * @param cloud cloud of which to set the time
104  * @param time time to use
105  */
106 template <typename PointT>
107 inline void
108 set_time(fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, const fawkes::Time &time)
109 {
110  set_time<PointT>(**cloud, time);
111 }
112 
113 /** Set time of a point cloud from a fawkes::Time instance.
114  * This uses the PointCloudTimestamp struct to set the time in the PCL
115  * timestamp field (if non-ROS PCL is used).
116  * @param cloud cloud of which to set the time
117  * @param time time to use
118  */
119 template <typename PointT>
120 inline void
121 set_time(boost::shared_ptr<pcl::PointCloud<PointT> > &cloud, const fawkes::Time &time)
122 {
123  set_time<PointT>(*cloud, time);
124 }
125 
126 
127 
128 /** Get time of a point cloud as a fawkes::Time instance.
129  * This uses the PointCloudTimestamp struct to set the time in the PCL
130  * timestamp field (if non-ROS PCL is used).
131  * @param cloud cloud of which to get the time
132  * @param time upon return contains the timestamp of the cloud
133  */
134 template <typename PointT>
135 inline void
136 get_time(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &cloud, fawkes::Time &time)
137 {
138 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
139  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
140 #else
141 # if PCL_VERSION_COMPARE(>=,1,7,0)
142  time.set_time(cloud->header.stamp / 1000000U, cloud->header.stamp % 1000000);
143 # else
144  PointCloudTimestamp pclts;
145  pclts.timestamp = cloud->header.stamp;
146  time.set_time(pclts.time.sec, pclts.time.usec);
147 # endif
148 #endif
149 }
150 
151 
152 /** Get time of a point cloud as a fawkes::Time instance.
153  * This uses the PointCloudTimestamp struct to set the time in the PCL
154  * timestamp field (if non-ROS PCL is used).
155  * @param cloud cloud of which to get the time
156  * @param time upon return contains the timestamp of the cloud
157  */
158 template <typename PointT>
159 inline void
160 get_time(const fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, fawkes::Time &time)
161 {
162 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
163  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
164 #else
165 # if PCL_VERSION_COMPARE(>=,1,7,0)
166  time.set_time(cloud->header.stamp / 1000000U, cloud->header.stamp % 1000000);
167 # else
168  PointCloudTimestamp pclts;
169  pclts.timestamp = cloud->header.stamp;
170  time.set_time(pclts.time.sec, pclts.time.usec);
171 #endif
172 #endif
173 }
174 
175 
176 /** Get time of a point cloud as a fawkes::Time instance.
177  * This uses the PointCloudTimestamp struct to set the time in the PCL
178  * timestamp field (if non-ROS PCL is used).
179  * @param cloud cloud of which to get the time
180  * @param time upon return contains the timestamp of the cloud
181  */
182 template <typename PointT>
183 inline void
184 get_time(const pcl::PointCloud<PointT> &cloud, fawkes::Time &time)
185 {
186 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
187  time.set_time(cloud.header.stamp.sec, cloud.header.stamp.nsec / 1000);
188 #else
189 # if PCL_VERSION_COMPARE(>=,1,7,0)
190  time.set_time(cloud.header.stamp / 1000000U, cloud.header.stamp % 1000000);
191 # else
192  PointCloudTimestamp pclts;
193  pclts.timestamp = cloud.header.stamp;
194  time.set_time(pclts.time.sec, pclts.time.usec);
195 # endif
196 #endif
197 }
198 
199 
200 /** Get time of a point cloud as a fawkes::Time instance.
201  * This uses the PointCloudTimestamp struct to set the time in the PCL
202  * timestamp field (if non-ROS PCL is used).
203  * @param cloud cloud of which to get the time
204  * @param time upon return contains the timestamp of the cloud
205  */
206 template <typename PointT>
207 inline void
208 get_time(const boost::shared_ptr<pcl::PointCloud<PointT> > &cloud, fawkes::Time &time)
209 {
210 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
211  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
212 #else
213 # if PCL_VERSION_COMPARE(>=,1,7,0)
214  time.set_time(cloud->header.stamp / 1000000U, cloud->header.stamp % 1000000);
215 # else
216  PointCloudTimestamp pclts;
217  pclts.timestamp = cloud->header.stamp;
218  time.set_time(pclts.time.sec, pclts.time.usec);
219 # endif
220 #endif
221 }
222 
223 
224 /** Get time of a point cloud as a fawkes::Time instance.
225  * This uses the PointCloudTimestamp struct to set the time in the PCL
226  * timestamp field (if non-ROS PCL is used).
227  * @param cloud cloud of which to get the time
228  * @param time upon return contains the timestamp of the cloud
229  */
230 template <typename PointT>
231 inline void
232 get_time(const boost::shared_ptr<const pcl::PointCloud<PointT>> &cloud, fawkes::Time &time)
233 {
234 #if defined(HAVE_ROS_PCL) || defined(ROSCPP_TYPES_H)
235  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
236 #else
237 # if PCL_VERSION_COMPARE(>=,1,7,0)
238  time.set_time(cloud->header.stamp / 1000000U, cloud->header.stamp % 1000000);
239 # else
240  PointCloudTimestamp pclts;
241  pclts.timestamp = cloud->header.stamp;
242  time.set_time(pclts.time.sec, pclts.time.usec);
243 # endif
244 #endif
245 }
246 
247 
248 /** Copy time from one point cloud to another.
249  * @param from point cloud to copy time from
250  * @param to point cloud to copy time to
251  */
252 template <typename PointT1, typename PointT2>
253 inline void
254 copy_time(fawkes::RefPtr<const pcl::PointCloud<PointT1> > &from,
256 {
257  to->header.stamp = from->header.stamp;
258 }
259 
260 
261 /** Copy time from one point cloud to another.
262  * @param from point cloud to copy time from
263  * @param to point cloud to copy time to
264  */
265 template <typename PointT1, typename PointT2>
266 inline void
267 copy_time(boost::shared_ptr<const pcl::PointCloud<PointT1> > &from,
269 {
270  to->header.stamp = from->header.stamp;
271 }
272 
273 /** Copy time from one point cloud to another.
274  * @param from point cloud to copy time from
275  * @param to point cloud to copy time to
276  */
277 template <typename PointT1, typename PointT2>
278 inline void
279 copy_time(const pcl::PointCloud<PointT1> &from,
281 {
282  to->header.stamp = from->header.stamp;
283 }
284 
285 
286 /** Helper struct to avoid deletion of PointClouds.
287  * The input point cloud is accessible using a RefPtr. Since the PCL
288  * expectes Boost shared_ptr, we need to create such a shared pointer.
289  * But destruction of this would cause the deletion of the point cloud,
290  * which we do not want. Therefore, we provide this helper deleter
291  * that causes the PointCloud *not* to be deleted on reset.
292  */
294  /** Delete operator that does nothing.
295  * @param t object to destroy
296  */
297  template<typename T> void operator()(T*t) {}
298 };
299 
300 template <typename PointT>
302 cloudptr_from_refptr(const fawkes::RefPtr<pcl::PointCloud<PointT> > &in)
303 {
304  return
305  boost::shared_ptr<pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter());
306 }
307 
308 
309 template <typename PointT>
311 cloudptr_from_refptr(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &in)
312 {
313  return
314  boost::shared_ptr<const pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter());
315 }
316 
317 } // end namespace pclutils
318 } // end namespace fawkes
319 
320 #endif
uint64_t sec
seconds part of time
Definition: utils.h:41
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
uint64_t timestamp
Access timestamp as number only.
Definition: utils.h:44
A class for handling time.
Definition: time.h:91
Base class for exceptions in Fawkes.
Definition: exception.h:36
Union to pack fawkes::Time into the pcl::PointCloud timestamp.
Definition: utils.h:39
uint64_t usec
microseconds part of time
Definition: utils.h:42
void operator()(T *t)
Delete operator that does nothing.
Definition: utils.h:297
Helper struct to avoid deletion of PointClouds.
Definition: utils.h:293
long get_sec() const
Get seconds.
Definition: time.h:110
long get_usec() const
Get microseconds.
Definition: time.h:112
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
void set_time(const timeval *tv)
Sets the time.
Definition: time.cpp:262
long in_usec() const
Convert the stored time into micro-seconds.
Definition: time.cpp:252
struct fawkes::pcl_utils::PointCloudTimestamp::@4 time
Access timestamp as time.
Interface for configuration handling.
Definition: config.h:67