checkpoint: all tests are passing

Author: tpanzarella

CMakeLists.txt

@@ -3,7 +3,6 @@ project(ros_numpy)
 
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
-find_package(rclpy REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -12,9 +11,13 @@ find_Package(tf2 REQUIRED)
 ament_python_install_package(${PROJECT_NAME})
 
 if(BUILD_TESTING)
+  find_package(rclpy REQUIRED)
   find_package(ament_cmake_nose REQUIRED)
-  #test_geometry.py  test_images.py  test_occupancygrids.py
+
   ament_add_nose_test(pointclouds test/test_pointclouds.py)
+  ament_add_nose_test(images test/test_images.py)
+  ament_add_nose_test(occupancygrids test/test_occupancygrids.py)
+  ament_add_nose_test(geometry test/test_geometry.py)
 endif()
 
 ##############

package.xml

@@ -20,7 +20,6 @@
   <author email="wieser@mit.edu">Eric Wieser</author>
 
   <exec_depend>python3-numpy</exec_depend>
-  <exec_depend>rclpy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
@@ -31,6 +30,7 @@
 
   <test_depend>python3-nose</test_depend>
   <test_depend>ament_cmake_nose</test_depend>
+  <test_depend>rclpy</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

ros_numpy/__init__.py

@@ -2,9 +2,8 @@
 A module for converting ROS message types into numpy types, where appropriate
 """
 
-from .numpy_msg import numpy_msg
 from .registry import numpify, msgify
 from . import point_cloud2
 from . import image
 from . import occupancy_grid
-from . import geometry
+from . import geometry

ros_numpy/geometry.py

@@ -8,117 +8,133 @@
 
 @converts_to_numpy(Vector3)
 def vector3_to_numpy(msg, hom=False):
-	if hom:
-		return np.array([msg.x, msg.y, msg.z, 0])
-	else:
-		return np.array([msg.x, msg.y, msg.z])
+    if hom:
+        return np.array([msg.x, msg.y, msg.z, 0])
+    else:
+        return np.array([msg.x, msg.y, msg.z])
 
 @converts_from_numpy(Vector3)
 def numpy_to_vector3(arr):
-	if arr.shape[-1] == 4:
-		assert np.all(arr[...,-1] == 0)
-		arr = arr[...,:-1]
+    if arr.shape[-1] == 4:
+        assert np.all(arr[...,-1] == 0)
+        arr = arr[...,:-1]
 
-	if len(arr.shape) == 1:
-		return Vector3(*arr)
-	else:
-		return np.apply_along_axis(lambda v: Vector3(*v), axis=-1, arr=arr)
+    if len(arr.shape) == 1:
+        return Vector3(**dict(zip(['x', 'y', 'z'], arr)))
+    else:
+        return np.apply_along_axis(
+            lambda v: Vector3(**dict(zip(['x', 'y', 'z'], v))), axis=-1,
+            arr=arr)
 
 @converts_to_numpy(Point)
 def point_to_numpy(msg, hom=False):
-	if hom:
-		return np.array([msg.x, msg.y, msg.z, 1])
-	else:
-		return np.array([msg.x, msg.y, msg.z])
+    if hom:
+        return np.array([msg.x, msg.y, msg.z, 1])
+    else:
+        return np.array([msg.x, msg.y, msg.z])
 
 @converts_from_numpy(Point)
 def numpy_to_point(arr):
-	if arr.shape[-1] == 4:
-		arr = arr[...,:-1] / arr[...,-1]
+    if arr.shape[-1] == 4:
+        arr = arr[...,:-1] / arr[...,-1]
 
-	if len(arr.shape) == 1:
-		return Point(*arr)
-	else:
-		return np.apply_along_axis(lambda v: Point(*v), axis=-1, arr=arr)
+    if len(arr.shape) == 1:
+        return Point(**dict(zip(['x', 'y', 'z'], arr)))
+    else:
+        return np.apply_along_axis(
+            lambda v: Point(**dict(zip(['x', 'y', 'z'], v))), axis=-1, arr=arr)
 
 @converts_to_numpy(Quaternion)
 def quat_to_numpy(msg):
-	return np.array([msg.x, msg.y, msg.z, msg.w])
+    return np.array([msg.x, msg.y, msg.z, msg.w])
 
 @converts_from_numpy(Quaternion)
 def numpy_to_quat(arr):
-	assert arr.shape[-1] == 4
+    assert arr.shape[-1] == 4
 
-	if len(arr.shape) == 1:
-		return Quaternion(*arr)
-	else:
-		return np.apply_along_axis(lambda v: Quaternion(*v), axis=-1, arr=arr)
+    if len(arr.shape) == 1:
+        return Quaternion(**dict(zip(['x', 'y', 'z', 'w'], arr)))
+    else:
+        return np.apply_along_axis(
+            lambda v: Quaternion(**dict(zip(['x', 'y', 'z', 'w'], v))),
+            axis=-1, arr=arr)
 
 
 # compound types
 # all of these take ...x4x4 homogeneous matrices
 
 @converts_to_numpy(Transform)
 def transform_to_numpy(msg):
-	from tf import transformations
+    from . import transformations
 
-	return np.dot(
+    return np.dot(
         transformations.translation_matrix(numpify(msg.translation)),
         transformations.quaternion_matrix(numpify(msg.rotation))
     )
 
 @converts_from_numpy(Transform)
 def numpy_to_transform(arr):
-	from tf import transformations
-
-	shape, rest = arr.shape[:-2], arr.shape[-2:]
-	assert rest == (4,4)
-
-	if len(shape) == 0:
-		trans = transformations.translation_from_matrix(arr)
-		quat = transformations.quaternion_from_matrix(arr)
-
-		return Transform(
-			translation=Vector3(*trans),
-			rotation=Quaternion(*quat)
-		)
-	else:
-		res = np.empty(shape, dtype=np.object_)
-		for idx in np.ndindex(shape):
-			res[idx] = Transform(
-				translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
-				rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
-			)
+    from . import transformations
+
+    shape, rest = arr.shape[:-2], arr.shape[-2:]
+    assert rest == (4,4)
+
+    if len(shape) == 0:
+        trans = transformations.translation_from_matrix(arr)
+        quat = transformations.quaternion_from_matrix(arr)
+
+        return Transform(
+            translation=Vector3(**dict(zip(['x', 'y', 'z'], trans))),
+            rotation=Quaternion(**dict(zip(['x', 'y', 'z', 'w'], quat)))
+        )
+    else:
+        res = np.empty(shape, dtype=np.object_)
+        for idx in np.ndindex(shape):
+            res[idx] = Transform(
+                translation=Vector3(
+                    **dict(
+                        zip(['x', 'y', 'z'],
+                        transformations.translation_from_matrix(arr[idx])))),
+                rotation=Quaternion(
+                    **dict(
+                        zip(['x', 'y', 'z', 'w'],
+                        transformations.quaternion_from_matrix(arr[idx]))))
+            )
 
 @converts_to_numpy(Pose)
 def pose_to_numpy(msg):
-	from tf import transformations
+    from . import transformations
 
-	return np.dot(
+    return np.dot(
         transformations.translation_matrix(numpify(msg.position)),
         transformations.quaternion_matrix(numpify(msg.orientation))
     )
 
 @converts_from_numpy(Pose)
 def numpy_to_pose(arr):
-	from tf import transformations
-
-	shape, rest = arr.shape[:-2], arr.shape[-2:]
-	assert rest == (4,4)
-
-	if len(shape) == 0:
-		trans = transformations.translation_from_matrix(arr)
-		quat = transformations.quaternion_from_matrix(arr)
-
-		return Pose(
-			position=Vector3(*trans),
-			orientation=Quaternion(*quat)
-		)
-	else:
-		res = np.empty(shape, dtype=np.object_)
-		for idx in np.ndindex(shape):
-			res[idx] = Pose(
-				position=Vector3(*transformations.translation_from_matrix(arr[idx])),
-				orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
-			)
-
+    from . import transformations
+
+    shape, rest = arr.shape[:-2], arr.shape[-2:]
+    assert rest == (4,4)
+
+    if len(shape) == 0:
+        trans = transformations.translation_from_matrix(arr)
+        quat = transformations.quaternion_from_matrix(arr)
+
+        return Pose(
+            position=Point(**dict(zip(['x', 'y', 'z'], trans))),
+            orientation=Quaternion(**dict(zip(['x', 'y', 'z', 'w'], quat)))
+        )
+    else:
+        res = np.empty(shape, dtype=np.object_)
+        for idx in np.ndindex(shape):
+            res[idx] = Pose(
+                position=Point(
+                    **dict(
+                        zip(['x', 'y', 'z'],
+                        transformations.translation_from_matrix(arr[idx])))),
+                orientation=Quaternion(
+                    **dict(
+                        zip(['x', 'y', 'z', 'w'],
+                        transformations.quaternion_from_matrix(arr[idx]))))
+            )

ros_numpy/image.py

@@ -17,7 +17,7 @@
 	"bgra16":  (np.uint16, 4),
 	"mono8":   (np.uint8,  1),
 	"mono16":  (np.uint16, 1),
-	
+
     # for bayer image (based on cv_bridge.cpp)
 	"bayer_rggb8":	(np.uint8,  1),
 	"bayer_bggr8":	(np.uint8,  1),
@@ -63,13 +63,13 @@
 def image_to_numpy(msg):
 	if not msg.encoding in name_to_dtypes:
 		raise TypeError('Unrecognized encoding {}'.format(msg.encoding))
-	
+
 	dtype_class, channels = name_to_dtypes[msg.encoding]
 	dtype = np.dtype(dtype_class)
 	dtype = dtype.newbyteorder('>' if msg.is_bigendian else '<')
 	shape = (msg.height, msg.width, channels)
 
-	data = np.fromstring(msg.data, dtype=dtype).reshape(shape)
+	data = np.frombuffer(msg.data, dtype=dtype).reshape(shape)
 	data.strides = (
 		msg.step,
 		dtype.itemsize * channels,
@@ -113,7 +113,7 @@ def numpy_to_image(arr, encoding):
 	im.data = contig.tostring()
 	im.step = contig.strides[0]
 	im.is_bigendian = (
-		arr.dtype.byteorder == '>' or 
+		arr.dtype.byteorder == '>' or
 		arr.dtype.byteorder == '=' and sys.byteorder == 'big'
 	)
 

ros_numpy/numpy_msg.py

@@ -1,3 +1,4 @@
+from array import array as Array
 import sys
 
 from .registry import converts_from_numpy, converts_to_numpy
@@ -8,25 +9,28 @@
 
 @converts_to_numpy(OccupancyGrid)
 def occupancygrid_to_numpy(msg):
-	data = np.asarray(msg.data, dtype=np.int8).reshape(msg.info.height, msg.info.width)
+    data = \
+      np.asarray(msg.data,
+                 dtype=np.int8).reshape(msg.info.height, msg.info.width)
 
-	return np.ma.array(data, mask=data==-1, fill_value=-1)
+    return np.ma.array(data, mask=data==-1, fill_value=-1)
 
 
 @converts_from_numpy(OccupancyGrid)
 def numpy_to_occupancy_grid(arr, info=None):
-	if not len(arr.shape) == 2:
-		raise TypeError('Array must be 2D')
-	if not arr.dtype == np.int8:
-		raise TypeError('Array must be of int8s')
-
-	grid = OccupancyGrid()
-	if isinstance(arr, np.ma.MaskedArray):
-		# We assume that the masked value are already -1, for speed
-		arr = arr.data
-	grid.data = arr.ravel()
-	grid.info = info or MapMetaData()
-	grid.info.height = arr.shape[0]
-	grid.info.width = arr.shape[1]
-
-	return grid
+    if not len(arr.shape) == 2:
+        raise TypeError('Array must be 2D')
+    if not arr.dtype == np.int8:
+        raise TypeError('Array must be of int8s')
+
+    grid = OccupancyGrid()
+    if isinstance(arr, np.ma.MaskedArray):
+        # We assume that the masked value are already -1, for speed
+        arr = arr.data
+
+    grid.data = Array('b', arr.ravel().astype(np.int8))
+    grid.info = info or MapMetaData()
+    grid.info.height = arr.shape[0]
+    grid.info.width = arr.shape[1]
+
+    return grid