Skip to content

Commit

Permalink
Fix flake8
Browse files Browse the repository at this point in the history
python scripts don't seem to be ported to ROS2 tho
  • Loading branch information
gleichdick committed May 26, 2021
1 parent b392269 commit 9b9c170
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 21 deletions.
2 changes: 2 additions & 0 deletions tf2_sensor_msgs/src/tf2_sensor_msgs/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
25 changes: 17 additions & 8 deletions tf2_sensor_msgs/src/tf2_sensor_msgs/tf2_sensor_msgs.py
Original file line number Diff line number Diff line change
@@ -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.
#
Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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)
34 changes: 21 additions & 13 deletions tf2_sensor_msgs/test/test_tf2_sensor_msgs.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)

0 comments on commit 9b9c170

Please sign in to comment.