tf2_sensor_msgs.py
Go to the documentation of this file.
1 # Copyright (c) 2008, Willow Garage, Inc.
2 # All rights reserved.
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions are met:
6 #
7 # * Redistributions of source code must retain the above copyright
8 # notice, this list of conditions and the following disclaimer.
9 # * Redistributions in binary form must reproduce the above copyright
10 # notice, this list of conditions and the following disclaimer in the
11 # documentation and/or other materials provided with the distribution.
12 # * Neither the name of the Willow Garage, Inc. nor the names of its
13 # contributors may be used to endorse or promote products derived from
14 # this software without specific prior written permission.
15 #
16 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
20 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
21 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
22 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
23 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
24 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
25 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
26 # POSSIBILITY OF SUCH DAMAGE.
27 
28 from sensor_msgs.msg import PointCloud2
29 from sensor_msgs.point_cloud2 import read_points, create_cloud
30 import PyKDL
31 import rospy
32 import tf2_ros
33 
34 def to_msg_msg(msg):
35  return msg
36 
37 tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg)
38 
39 def from_msg_msg(msg):
40  return msg
41 
42 tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg)
43 
45  return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y,
46  t.transform.rotation.z, t.transform.rotation.w),
47  PyKDL.Vector(t.transform.translation.x,
48  t.transform.translation.y,
49  t.transform.translation.z))
50 
51 # PointStamped
52 def do_transform_cloud(cloud, transform):
53  t_kdl = transform_to_kdl(transform)
54  points_out = []
55  for p_in in read_points(cloud):
56  p_out = t_kdl * PyKDL.Vector(p_in[0], p_in[1], p_in[2])
57  points_out.append((p_out[0], p_out[1], p_out[2]) + p_in[3:])
58  res = create_cloud(transform.header, cloud.fields, points_out)
59  return res
60 tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud)
def do_transform_cloud(cloud, transform)


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