Skip to content

Commit 71c9570

Browse files
tpanzarellatpanzarella
committed
checkpoint: all tests are passing
1 parent ac56007 commit 71c9570

13 files changed

+2149
-187
lines changed

‎CMakeLists.txt

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,6 @@ project(ros_numpy)
33

44
find_package(ament_cmake REQUIRED)
55
find_package(ament_cmake_python REQUIRED)
6-
find_package(rclpy REQUIRED)
76
find_package(sensor_msgs REQUIRED)
87
find_package(nav_msgs REQUIRED)
98
find_package(geometry_msgs REQUIRED)
@@ -12,9 +11,13 @@ find_Package(tf2 REQUIRED)
1211
ament_python_install_package(${PROJECT_NAME})
1312

1413
if(BUILD_TESTING)
14+
find_package(rclpy REQUIRED)
1515
find_package(ament_cmake_nose REQUIRED)
16-
#test_geometry.py test_images.py test_occupancygrids.py
16+
1717
ament_add_nose_test(pointclouds test/test_pointclouds.py)
18+
ament_add_nose_test(images test/test_images.py)
19+
ament_add_nose_test(occupancygrids test/test_occupancygrids.py)
20+
ament_add_nose_test(geometry test/test_geometry.py)
1821
endif()
1922

2023
##############

‎package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,6 @@
2020
<author email="wieser@mit.edu">Eric Wieser</author>
2121

2222
<exec_depend>python3-numpy</exec_depend>
23-
<exec_depend>rclpy</exec_depend>
2423
<exec_depend>sensor_msgs</exec_depend>
2524
<exec_depend>nav_msgs</exec_depend>
2625
<exec_depend>geometry_msgs</exec_depend>
@@ -31,6 +30,7 @@
3130

3231
<test_depend>python3-nose</test_depend>
3332
<test_depend>ament_cmake_nose</test_depend>
33+
<test_depend>rclpy</test_depend>
3434

3535
<export>
3636
<build_type>ament_cmake</build_type>

‎ros_numpy/__init__.py

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,8 @@
22
A module for converting ROS message types into numpy types, where appropriate
33
"""
44

5-
from .numpy_msg import numpy_msg
65
from .registry import numpify, msgify
76
from . import point_cloud2
87
from . import image
98
from . import occupancy_grid
10-
from . import geometry
9+
from . import geometry

‎ros_numpy/geometry.py

Lines changed: 88 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -8,117 +8,133 @@
88

99
@converts_to_numpy(Vector3)
1010
def vector3_to_numpy(msg, hom=False):
11-
if hom:
12-
return np.array([msg.x, msg.y, msg.z, 0])
13-
else:
14-
return np.array([msg.x, msg.y, msg.z])
11+
if hom:
12+
return np.array([msg.x, msg.y, msg.z, 0])
13+
else:
14+
return np.array([msg.x, msg.y, msg.z])
1515

1616
@converts_from_numpy(Vector3)
1717
def numpy_to_vector3(arr):
18-
if arr.shape[-1] == 4:
19-
assert np.all(arr[...,-1] == 0)
20-
arr = arr[...,:-1]
18+
if arr.shape[-1] == 4:
19+
assert np.all(arr[...,-1] == 0)
20+
arr = arr[...,:-1]
2121

22-
if len(arr.shape) == 1:
23-
return Vector3(*arr)
24-
else:
25-
return np.apply_along_axis(lambda v: Vector3(*v), axis=-1, arr=arr)
22+
if len(arr.shape) == 1:
23+
return Vector3(**dict(zip(['x', 'y', 'z'], arr)))
24+
else:
25+
return np.apply_along_axis(
26+
lambda v: Vector3(**dict(zip(['x', 'y', 'z'], v))), axis=-1,
27+
arr=arr)
2628

2729
@converts_to_numpy(Point)
2830
def point_to_numpy(msg, hom=False):
29-
if hom:
30-
return np.array([msg.x, msg.y, msg.z, 1])
31-
else:
32-
return np.array([msg.x, msg.y, msg.z])
31+
if hom:
32+
return np.array([msg.x, msg.y, msg.z, 1])
33+
else:
34+
return np.array([msg.x, msg.y, msg.z])
3335

3436
@converts_from_numpy(Point)
3537
def numpy_to_point(arr):
36-
if arr.shape[-1] == 4:
37-
arr = arr[...,:-1] / arr[...,-1]
38+
if arr.shape[-1] == 4:
39+
arr = arr[...,:-1] / arr[...,-1]
3840

39-
if len(arr.shape) == 1:
40-
return Point(*arr)
41-
else:
42-
return np.apply_along_axis(lambda v: Point(*v), axis=-1, arr=arr)
41+
if len(arr.shape) == 1:
42+
return Point(**dict(zip(['x', 'y', 'z'], arr)))
43+
else:
44+
return np.apply_along_axis(
45+
lambda v: Point(**dict(zip(['x', 'y', 'z'], v))), axis=-1, arr=arr)
4346

4447
@converts_to_numpy(Quaternion)
4548
def quat_to_numpy(msg):
46-
return np.array([msg.x, msg.y, msg.z, msg.w])
49+
return np.array([msg.x, msg.y, msg.z, msg.w])
4750

4851
@converts_from_numpy(Quaternion)
4952
def numpy_to_quat(arr):
50-
assert arr.shape[-1] == 4
53+
assert arr.shape[-1] == 4
5154

52-
if len(arr.shape) == 1:
53-
return Quaternion(*arr)
54-
else:
55-
return np.apply_along_axis(lambda v: Quaternion(*v), axis=-1, arr=arr)
55+
if len(arr.shape) == 1:
56+
return Quaternion(**dict(zip(['x', 'y', 'z', 'w'], arr)))
57+
else:
58+
return np.apply_along_axis(
59+
lambda v: Quaternion(**dict(zip(['x', 'y', 'z', 'w'], v))),
60+
axis=-1, arr=arr)
5661

5762

5863
# compound types
5964
# all of these take ...x4x4 homogeneous matrices
6065

6166
@converts_to_numpy(Transform)
6267
def transform_to_numpy(msg):
63-
from tf import transformations
68+
from . import transformations
6469

65-
return np.dot(
70+
return np.dot(
6671
transformations.translation_matrix(numpify(msg.translation)),
6772
transformations.quaternion_matrix(numpify(msg.rotation))
6873
)
6974

7075
@converts_from_numpy(Transform)
7176
def numpy_to_transform(arr):
72-
from tf import transformations
73-
74-
shape, rest = arr.shape[:-2], arr.shape[-2:]
75-
assert rest == (4,4)
76-
77-
if len(shape) == 0:
78-
trans = transformations.translation_from_matrix(arr)
79-
quat = transformations.quaternion_from_matrix(arr)
80-
81-
return Transform(
82-
translation=Vector3(*trans),
83-
rotation=Quaternion(*quat)
84-
)
85-
else:
86-
res = np.empty(shape, dtype=np.object_)
87-
for idx in np.ndindex(shape):
88-
res[idx] = Transform(
89-
translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
90-
rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
91-
)
77+
from . import transformations
78+
79+
shape, rest = arr.shape[:-2], arr.shape[-2:]
80+
assert rest == (4,4)
81+
82+
if len(shape) == 0:
83+
trans = transformations.translation_from_matrix(arr)
84+
quat = transformations.quaternion_from_matrix(arr)
85+
86+
return Transform(
87+
translation=Vector3(**dict(zip(['x', 'y', 'z'], trans))),
88+
rotation=Quaternion(**dict(zip(['x', 'y', 'z', 'w'], quat)))
89+
)
90+
else:
91+
res = np.empty(shape, dtype=np.object_)
92+
for idx in np.ndindex(shape):
93+
res[idx] = Transform(
94+
translation=Vector3(
95+
**dict(
96+
zip(['x', 'y', 'z'],
97+
transformations.translation_from_matrix(arr[idx])))),
98+
rotation=Quaternion(
99+
**dict(
100+
zip(['x', 'y', 'z', 'w'],
101+
transformations.quaternion_from_matrix(arr[idx]))))
102+
)
92103

93104
@converts_to_numpy(Pose)
94105
def pose_to_numpy(msg):
95-
from tf import transformations
106+
from . import transformations
96107

97-
return np.dot(
108+
return np.dot(
98109
transformations.translation_matrix(numpify(msg.position)),
99110
transformations.quaternion_matrix(numpify(msg.orientation))
100111
)
101112

102113
@converts_from_numpy(Pose)
103114
def numpy_to_pose(arr):
104-
from tf import transformations
105-
106-
shape, rest = arr.shape[:-2], arr.shape[-2:]
107-
assert rest == (4,4)
108-
109-
if len(shape) == 0:
110-
trans = transformations.translation_from_matrix(arr)
111-
quat = transformations.quaternion_from_matrix(arr)
112-
113-
return Pose(
114-
position=Vector3(*trans),
115-
orientation=Quaternion(*quat)
116-
)
117-
else:
118-
res = np.empty(shape, dtype=np.object_)
119-
for idx in np.ndindex(shape):
120-
res[idx] = Pose(
121-
position=Vector3(*transformations.translation_from_matrix(arr[idx])),
122-
orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
123-
)
124-
115+
from . import transformations
116+
117+
shape, rest = arr.shape[:-2], arr.shape[-2:]
118+
assert rest == (4,4)
119+
120+
if len(shape) == 0:
121+
trans = transformations.translation_from_matrix(arr)
122+
quat = transformations.quaternion_from_matrix(arr)
123+
124+
return Pose(
125+
position=Point(**dict(zip(['x', 'y', 'z'], trans))),
126+
orientation=Quaternion(**dict(zip(['x', 'y', 'z', 'w'], quat)))
127+
)
128+
else:
129+
res = np.empty(shape, dtype=np.object_)
130+
for idx in np.ndindex(shape):
131+
res[idx] = Pose(
132+
position=Point(
133+
**dict(
134+
zip(['x', 'y', 'z'],
135+
transformations.translation_from_matrix(arr[idx])))),
136+
orientation=Quaternion(
137+
**dict(
138+
zip(['x', 'y', 'z', 'w'],
139+
transformations.quaternion_from_matrix(arr[idx]))))
140+
)

‎ros_numpy/image.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
"bgra16": (np.uint16, 4),
1818
"mono8": (np.uint8, 1),
1919
"mono16": (np.uint16, 1),
20-
20+
2121
# for bayer image (based on cv_bridge.cpp)
2222
"bayer_rggb8": (np.uint8, 1),
2323
"bayer_bggr8": (np.uint8, 1),
@@ -63,13 +63,13 @@
6363
def image_to_numpy(msg):
6464
if not msg.encoding in name_to_dtypes:
6565
raise TypeError('Unrecognized encoding {}'.format(msg.encoding))
66-
66+
6767
dtype_class, channels = name_to_dtypes[msg.encoding]
6868
dtype = np.dtype(dtype_class)
6969
dtype = dtype.newbyteorder('>' if msg.is_bigendian else '<')
7070
shape = (msg.height, msg.width, channels)
7171

72-
data = np.fromstring(msg.data, dtype=dtype).reshape(shape)
72+
data = np.frombuffer(msg.data, dtype=dtype).reshape(shape)
7373
data.strides = (
7474
msg.step,
7575
dtype.itemsize * channels,
@@ -113,7 +113,7 @@ def numpy_to_image(arr, encoding):
113113
im.data = contig.tostring()
114114
im.step = contig.strides[0]
115115
im.is_bigendian = (
116-
arr.dtype.byteorder == '>' or
116+
arr.dtype.byteorder == '>' or
117117
arr.dtype.byteorder == '=' and sys.byteorder == 'big'
118118
)
119119

‎ros_numpy/numpy_msg.py

Lines changed: 0 additions & 21 deletions
This file was deleted.

‎ros_numpy/occupancy_grid.py

Lines changed: 21 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
from array import array as Array
12
import sys
23

34
from .registry import converts_from_numpy, converts_to_numpy
@@ -8,25 +9,28 @@
89

910
@converts_to_numpy(OccupancyGrid)
1011
def occupancygrid_to_numpy(msg):
11-
data = np.asarray(msg.data, dtype=np.int8).reshape(msg.info.height, msg.info.width)
12+
data = \
13+
np.asarray(msg.data,
14+
dtype=np.int8).reshape(msg.info.height, msg.info.width)
1215

13-
return np.ma.array(data, mask=data==-1, fill_value=-1)
16+
return np.ma.array(data, mask=data==-1, fill_value=-1)
1417

1518

1619
@converts_from_numpy(OccupancyGrid)
1720
def numpy_to_occupancy_grid(arr, info=None):
18-
if not len(arr.shape) == 2:
19-
raise TypeError('Array must be 2D')
20-
if not arr.dtype == np.int8:
21-
raise TypeError('Array must be of int8s')
22-
23-
grid = OccupancyGrid()
24-
if isinstance(arr, np.ma.MaskedArray):
25-
# We assume that the masked value are already -1, for speed
26-
arr = arr.data
27-
grid.data = arr.ravel()
28-
grid.info = info or MapMetaData()
29-
grid.info.height = arr.shape[0]
30-
grid.info.width = arr.shape[1]
31-
32-
return grid
21+
if not len(arr.shape) == 2:
22+
raise TypeError('Array must be 2D')
23+
if not arr.dtype == np.int8:
24+
raise TypeError('Array must be of int8s')
25+
26+
grid = OccupancyGrid()
27+
if isinstance(arr, np.ma.MaskedArray):
28+
# We assume that the masked value are already -1, for speed
29+
arr = arr.data
30+
31+
grid.data = Array('b', arr.ravel().astype(np.int8))
32+
grid.info = info or MapMetaData()
33+
grid.info.height = arr.shape[0]
34+
grid.info.width = arr.shape[1]
35+
36+
return grid

0 commit comments

Comments
 (0)