tf2_sensor_msgs.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef TF2_SENSOR_MSGS_H
31 #define TF2_SENSOR_MSGS_H
32 
33 #include <tf2/convert.h>
34 #include <sensor_msgs/PointCloud2.h>
36 #include <Eigen/Eigen>
37 #include <Eigen/Geometry>
38 
39 namespace tf2
40 {
41 
42 /********************/
44 /********************/
45 
52 template <>
53 inline
54 const ros::Time& getTimestamp(const sensor_msgs::PointCloud2& p) {return p.header.stamp;}
55 
62 template <>
63 inline
64 const std::string& getFrameId(const sensor_msgs::PointCloud2 &p) {return p.header.frame_id;}
65 
66 // this method needs to be implemented by client library developers
67 template <>
68 inline
69 void doTransform(const sensor_msgs::PointCloud2 &p_in, sensor_msgs::PointCloud2 &p_out, const geometry_msgs::TransformStamped& t_in)
70 {
71  p_out = p_in;
72  p_out.header = t_in.header;
73  Eigen::Transform<float,3,Eigen::Affine> t = Eigen::Translation3f(t_in.transform.translation.x, t_in.transform.translation.y,
74  t_in.transform.translation.z) * Eigen::Quaternion<float>(
75  t_in.transform.rotation.w, t_in.transform.rotation.x,
76  t_in.transform.rotation.y, t_in.transform.rotation.z);
77 
81 
85 
86  Eigen::Vector3f point;
87  for(; x_in != x_in.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) {
88  point = t * Eigen::Vector3f(*x_in, *y_in, *z_in);
89  *x_out = point.x();
90  *y_out = point.y();
91  *z_out = point.z();
92  }
93 }
94 inline
95 sensor_msgs::PointCloud2 toMsg(const sensor_msgs::PointCloud2 &in)
96 {
97  return in;
98 }
99 inline
100 void fromMsg(const sensor_msgs::PointCloud2 &msg, sensor_msgs::PointCloud2 &out)
101 {
102  out = msg;
103 }
104 
105 } // namespace
106 
107 #endif // TF2_SENSOR_MSGS_H
const std::string & getFrameId(const T &t)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
void fromMsg(const A &, B &b)
B toMsg(const A &a)
const ros::Time & getTimestamp(const T &t)


tf2_sensor_msgs
Author(s): Vincent Rabaud
autogenerated on Fri Oct 16 2020 19:09:07