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