From 9b9c170cc451e5e9b68b7a7927df306af89da7b4 Mon Sep 17 00:00:00 2001 From: Bjar Ne Date: Wed, 26 May 2021 20:49:24 +0000 Subject: [PATCH] Fix flake8 python scripts don't seem to be ported to ROS2 tho --- .../src/tf2_sensor_msgs/__init__.py | 2 ++ .../src/tf2_sensor_msgs/tf2_sensor_msgs.py | 25 +++++++++----- tf2_sensor_msgs/test/test_tf2_sensor_msgs.py | 34 ++++++++++++------- 3 files changed, 40 insertions(+), 21 deletions(-) diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py index 0a75ea21d..6b4eefa5f 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py @@ -25,3 +25,5 @@ # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. + +from tf2_sensor_msgs import * # noqa(E401) diff --git a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py index e1f72829b..aee6d0f6b 100644 --- a/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py @@ -1,8 +1,8 @@ # Copyright 2008 Willow Garage, Inc. -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: -# +# # * Redistributions of source code must retain the above copyright # notice, this list of conditions and the following disclaimer. # @@ -13,7 +13,7 @@ # * Neither the name of the Willow Garage, Inc. nor the names of its # contributors may be used to endorse or promote products derived from # this software without specific prior written permission. -# +# # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -29,26 +29,33 @@ from sensor_msgs.msg import PointCloud2 from sensor_msgs.point_cloud2 import read_points, create_cloud import PyKDL -import rospy +import rospy # noqa(E401) import tf2_ros + def to_msg_msg(msg): return msg + tf2_ros.ConvertRegistration().add_to_msg(PointCloud2, to_msg_msg) + def from_msg_msg(msg): return msg + tf2_ros.ConvertRegistration().add_from_msg(PointCloud2, from_msg_msg) + def transform_to_kdl(t): - return PyKDL.Frame(PyKDL.Rotation.Quaternion(t.transform.rotation.x, t.transform.rotation.y, - t.transform.rotation.z, t.transform.rotation.w), - PyKDL.Vector(t.transform.translation.x, - t.transform.translation.y, + return PyKDL.Frame(PyKDL.Rotation.Quaternion( + t.transform.rotation.x, t.transform.rotation.y, + t.transform.rotation.z, t.transform.rotation.w), + PyKDL.Vector(t.transform.translation.x, + t.transform.translation.y, t.transform.translation.z)) + # PointStamped def do_transform_cloud(cloud, transform): t_kdl = transform_to_kdl(transform) @@ -58,4 +65,6 @@ def do_transform_cloud(cloud, transform): points_out.append(p_out) res = create_cloud(transform.header, cloud.fields, points_out) return res + + tf2_ros.TransformRegistration().add(PointCloud2, do_transform_cloud) diff --git a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py index 119d4b7d4..9ce80b0f3 100644 --- a/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py +++ b/tf2_sensor_msgs/test/test_tf2_sensor_msgs.py @@ -36,44 +36,52 @@ from tf2_ros import TransformStamped import copy -## A sample python unit test + +# A sample python unit test class PointCloudConversions(unittest.TestCase): def setUp(self): self.point_cloud_in = point_cloud2.PointCloud2() - self.point_cloud_in.fields = [PointField('x', 0, PointField.FLOAT32, 1), - PointField('y', 4, PointField.FLOAT32, 1), - PointField('z', 8, PointField.FLOAT32, 1)] + self.point_cloud_in.fields = [ + PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] self.point_cloud_in.point_step = 4 * 3 self.point_cloud_in.height = 1 # we add two points (with x, y, z to the cloud) self.point_cloud_in.width = 2 - self.point_cloud_in.row_step = self.point_cloud_in.point_step * self.point_cloud_in.width + self.point_cloud_in.row_step = \ + self.point_cloud_in.point_step * self.point_cloud_in.width points = [1, 2, 0, 10, 20, 30] self.point_cloud_in.data = struct.pack('%sf' % len(points), *points) - self.transform_translate_xyz_300 = TransformStamped() self.transform_translate_xyz_300.transform.translation.x = 300 self.transform_translate_xyz_300.transform.translation.y = 300 self.transform_translate_xyz_300.transform.translation.z = 300 - self.transform_translate_xyz_300.transform.rotation.w = 1 # no rotation so we only set w + # no rotation so we only set w + self.transform_translate_xyz_300.transform.rotation.w = 1 - assert(list(point_cloud2.read_points(self.point_cloud_in)) == [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) + assert(list(point_cloud2.read_points(self.point_cloud_in)) == + [(1.0, 2.0, 0.0), (10.0, 20.0, 30.0)]) def test_simple_transform(self): - old_data = copy.deepcopy(self.point_cloud_in.data) # deepcopy is not required here because we have a str for now - point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud(self.point_cloud_in, self.transform_translate_xyz_300) + # deepcopy is not required here because we have a str for now + old_data = copy.deepcopy(self.point_cloud_in.data) + point_cloud_transformed = tf2_sensor_msgs.do_transform_cloud( + self.point_cloud_in, self.transform_translate_xyz_300) k = 300 expected_coordinates = [(1+k, 2+k, 0+k), (10+k, 20+k, 30+k)] new_points = list(point_cloud2.read_points(point_cloud_transformed)) print("new_points are %s" % new_points) assert(expected_coordinates == new_points) - assert(old_data == self.point_cloud_in.data) # checking no modification in input cloud + # checking no modification in input cloud + assert(old_data == self.point_cloud_in.data) + if __name__ == '__main__': import rosunit - rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", PointCloudConversions) - + rosunit.unitrun("test_tf2_sensor_msgs", "test_point_cloud_conversion", + PointCloudConversions)