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
33namespace fawkes {
34namespace pcl_utils {
35
36/** Union to pack fawkes::Time into the pcl::PointCloud timestamp. */
37typedef 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 */
50inline void
51shutup()
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 */
61inline void
62shutup_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 */
79template <typename PointT>
80inline void
81set_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 */
104template <typename PointT>
105inline void
106set_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 */
117template <typename CloudPtrT>
118inline void
119set_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 */
130template <typename CloudPtrT>
131inline void
132get_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 */
153template <typename PointT>
154inline void
155get_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 */
174template <typename CloudPtrT, typename PointT2>
175inline void
176copy_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
199template <typename PointT>
201cloudptr_from_refptr(const fawkes::RefPtr<pcl::PointCloud<PointT>> &in)
202{
203 return typename pcl::PointCloud<PointT>::Ptr(*in, PointCloudNonDeleter());
204}
205
206template <typename PointT>
208cloudptr_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