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