Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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 
27 namespace fawkes {
28  namespace pcl_utils {
29 #if 0 /* just to make Emacs auto-indent happy */
30  }
31 }
32 #endif
33 
34 /** Set time of a point cloud from a fawkes::Time instance.
35  * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL
36  * timestamp field (if non-ROS PCL is used).
37  * @param cloud cloud of which to set the time
38  * @param time time to use
39  */
40 template <typename PointT>
41 inline void
42 set_time(fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, const fawkes::Time &time)
43 {
44 #if HAVE_ROS_PCL
45  cloud->header.stamp.sec = time.get_sec();
46  cloud->header.stamp.nsec = time.get_usec() * 1000;
47 #else
49  pclts.time.sec = time.get_sec();
50  pclts.time.usec = time.get_usec();
51  cloud->header.stamp = pclts.timestamp;
52 #endif
53 }
54 
55 
56 /** Get time of a point cloud as a fawkes::Time instance.
57  * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL
58  * timestamp field (if non-ROS PCL is used).
59  * @param cloud cloud of which to get the time
60  * @param time upon return contains the timestamp of the cloud
61  */
62 template <typename PointT>
63 inline void
64 get_time(const fawkes::RefPtr<const pcl::PointCloud<PointT> > &cloud, fawkes::Time &time)
65 {
66 #if HAVE_ROS_PCL
67  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
68 #else
70  pclts.timestamp = cloud->header.stamp;
71  time.set_time(pclts.time.sec, time.get_usec());
72 #endif
73 }
74 
75 
76 /** Get time of a point cloud as a fawkes::Time instance.
77  * This uses the fawkes::PointCloudTimestamp struct to set the time in the PCL
78  * timestamp field (if non-ROS PCL is used).
79  * @param cloud cloud of which to get the time
80  * @param time upon return contains the timestamp of the cloud
81  */
82 template <typename PointT>
83 inline void
84 get_time(const fawkes::RefPtr<pcl::PointCloud<PointT> > &cloud, fawkes::Time &time)
85 {
86 #if HAVE_ROS_PCL
87  time.set_time(cloud->header.stamp.sec, cloud->header.stamp.nsec / 1000);
88 #else
90  pclts.timestamp = cloud->header.stamp;
91  time.set_time(pclts.time.sec, time.get_usec());
92 #endif
93 }
94 
95 
96 /** Copy time from one point cloud to another.
97  * @param from point cloud to copy time from
98  * @param to point cloud to copy time to
99  */
100 template <typename PointT1, typename PointT2>
101 inline void
102 copy_time(fawkes::RefPtr<const pcl::PointCloud<PointT1> > &from,
104 {
105  to->header.stamp = from->header.stamp;
106 }
107 
108 
109 /** Helper struct to avoid deletion of PointClouds.
110  * The input point cloud is accessible using a RefPtr. Since the PCL
111  * expectes Boost shared_ptr, we need to create such a shared pointer.
112  * But destruction of this would cause the deletion of the point cloud,
113  * which we do not want. Therefore, we provide this helper deleter
114  * that causes the PointCloud *not* to be deleted on reset.
115  */
117  /** Delete operator that does nothing.
118  * @param t object to destroy
119  */
120  template<typename T> void operator()(T*t) {}
121 };
122 
123 template <typename PointT>
125 cloudptr_from_refptr(fawkes::RefPtr<pcl::PointCloud<PointT> > &in)
126 {
127  return
128  boost::shared_ptr<pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter());
129 }
130 
131 
132 template <typename PointT>
134 cloudptr_from_refptr(fawkes::RefPtr<const pcl::PointCloud<PointT> > &in)
135 {
136  return
137  boost::shared_ptr<const pcl::PointCloud<PointT> >(*in, PointCloudNonDeleter());
138 }
139 
140 } // end namespace pclutils
141 } // end namespace fawkes
142 
143 #endif