Fix package dep for python2/3

.travis.yml

@@ -1,24 +1,33 @@
 sudo: required
-dist: trusty
 language: generic
-env:
-  - ROS_DISTRO=indigo
-  - ROS_DISTRO=jade
+# Build matrix
+jobs:
+  include:
+    - os: linux
+      dist: xenial
+      env: ROS_DISTRO=kinetic
+    - os: linux
+      dist: bionic
+      env: ROS_DISTRO=melodic
+    - os: linux
+      dist: focal
+      env: ROS_DISTRO=noetic
 # Install system dependencies, namely ROS.
 before_install:
   # Define some config vars.
-  - export ROS_CI_DESKTOP=`lsb_release -cs`  # e.g. [precise|trusty]
+  - export ROS_CI_DESKTOP=`lsb_release -cs`  # e.g. [xenial|bionic|focal]
   - export CI_SOURCE_PATH=$(pwd)
   - export REPOSITORY_NAME=${PWD##*/}
   - export ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall
   - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
   - export ROS_PARALLEL_JOBS='-j8 -l6'
+  - export PYTHON_NAME=$([[ $(lsb_release -cs) = focal ]] && echo python3 || echo python)
   # Install ROS
   - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - sudo apt-get update -qq
   # Install ROS
-  - sudo apt-get install -y python-catkin-pkg python-catkin-tools python-rosdep python-wstool ros-$ROS_DISTRO-catkin
+  - sudo apt-get install -y $PYTHON_NAME-catkin-pkg $PYTHON_NAME-catkin-tools $PYTHON_NAME-rosdep $PYTHON_NAME-wstool $PYTHON_NAME-osrf-pycommon ros-$ROS_DISTRO-catkin
   - source /opt/ros/$ROS_DISTRO/setup.bash
   # Setup for rosdep
   - sudo rosdep init

package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>ros_numpy</name>
   <version>0.0.3</version>
   <description>A collection of conversion function for extracting numpy arrays from messages</description>
@@ -14,15 +14,15 @@
 
   <author email="[email protected]">Eric Wieser</author>
 
-  <run_depend>python-numpy</run_depend>
-  <run_depend>rospy</run_depend>
-  <run_depend>sensor_msgs</run_depend>
-  <run_depend>nav_msgs</run_depend>
-  <run_depend>geometry_msgs</run_depend>
-  <run_depend>tf</run_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <export>
   </export>
-</package>
+</package>

src/ros_numpy/geometry.py

@@ -4,6 +4,139 @@
 
 import numpy as np
 
+# transformation helpers
+_EPS = np.finfo(float).eps * 4.0
+
+
+def translation_matrix(direction):
+	"""Create 4x4 array to translate by the direction.
+
+	Parameters
+	----------
+	direction : array-like
+		Translation x, y, z vector.
+
+	Returns
+	-------
+	array
+		4x4 transformation matrix with translation
+
+	Notes
+	-----
+	Adapted from:
+	https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py
+
+	Examples
+	--------
+	>>> v = np.random.random(3) - 0.5
+	>>> np.allclose(v, translation_matrix(v)[:3, 3])
+	True
+	"""
+	M = np.identity(4)
+	M[:3, 3] = direction[:3]
+	return M
+
+
+def translation_from_matrix(matrix):
+	"""Get translation vector from transformation matrix.
+
+	Parameters
+	----------
+	matrix : 2D array-like
+		4x4 transformation matrix
+
+	Returns
+	-------
+	array
+		Translation direction vector
+
+	Notes
+	-----
+	Adapted from:
+	https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py
+
+	Examples
+	--------
+	>>> v0 = np.random.random(3) - 0.5
+	>>> v1 = translation_from_matrix(translation_matrix(v0))
+	>>> np.allclose(v0, v1)
+	True
+	"""
+	return np.array(matrix, copy=False)[:3, 3].copy()
+
+
+def quaternion_matrix(quaternion):
+	"""Return homogeneous rotation matrix from quaternion.
+
+	Parameters
+	----------
+	quaternion : 1D array-like
+		Rotation as a quaternion (w, x, y, z)
+
+	Returns
+	-------
+	array
+		4x4 transformation matrix with rotation
+
+	Notes
+	-----
+	Adapted from:
+	https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py
+	"""
+	q = np.array(quaternion[:4], dtype=np.float64, copy=True)
+	nq = np.dot(q, q)
+	if nq < _EPS:
+		return np.identity(4, dtype=np.float64)
+	q *= np.sqrt(2.0 / nq)
+	q = np.outer(q, q)
+	return np.array((
+		(1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], 0.0),
+		(    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], 0.0),
+		(    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
+		(                0.0,                 0.0,                 0.0, 1.0)
+		), dtype=np.float64)
+
+
+def quaternion_from_matrix(matrix):
+	"""Return quaternion from rotation matrix.
+
+	Parameters
+	----------
+	matrix : 2D array-like
+		4x4 transformation matrix
+
+	Returns
+	-------
+	array-like
+		Rotation as a quaternion (w, x, y, z)
+
+	Notes
+	-----
+	Adapted from:
+ 	https://github.com/ros/geometry/blob/noetic-devel/tf/src/tf/transformations.py
+ 	"""
+	q = np.empty((4, ), dtype=np.float64)
+	M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
+	t = np.trace(M)
+	if t > M[3, 3]:
+		q[3] = t
+		q[2] = M[1, 0] - M[0, 1]
+		q[1] = M[0, 2] - M[2, 0]
+		q[0] = M[2, 1] - M[1, 2]
+	else:
+		i, j, k = 0, 1, 2
+		if M[1, 1] > M[0, 0]:
+			i, j, k = 1, 2, 0
+		if M[2, 2] > M[i, i]:
+			i, j, k = 2, 0, 1
+		t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
+		q[i] = t
+		q[j] = M[i, j] + M[j, i]
+		q[k] = M[k, i] + M[i, k]
+		q[3] = M[k, j] - M[j, k]
+	q *= 0.5 / np.sqrt(t * M[3, 3])
+	return q
+
 # basic types
 
 @converts_to_numpy(Vector3)
@@ -60,23 +193,19 @@ def numpy_to_quat(arr):
 
 @converts_to_numpy(Transform)
 def transform_to_numpy(msg):
-	from tf import transformations
-
 	return np.dot(
-        transformations.translation_matrix(numpify(msg.translation)),
-        transformations.quaternion_matrix(numpify(msg.rotation))
-    )
+		translation_matrix(numpify(msg.translation)),
+		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)
+		trans = translation_from_matrix(arr)
+		quat = quaternion_from_matrix(arr)
 
 		return Transform(
 			translation=Vector3(*trans),
@@ -86,29 +215,25 @@ def numpy_to_transform(arr):
 		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]))
+				translation=Vector3(*translation_from_matrix(arr[idx])),
+				rotation=Quaternion(*quaternion_from_matrix(arr[idx]))
 			)
 
 @converts_to_numpy(Pose)
 def pose_to_numpy(msg):
-	from tf import transformations
-
 	return np.dot(
-        transformations.translation_matrix(numpify(msg.position)),
-        transformations.quaternion_matrix(numpify(msg.orientation))
-    )
+		translation_matrix(numpify(msg.position)),
+		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)
+		trans = translation_from_matrix(arr)
+		quat = quaternion_from_matrix(arr)
 
 		return Pose(
 			position=Vector3(*trans),
@@ -118,7 +243,6 @@ def numpy_to_pose(arr):
 		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]))
+				position=Vector3(*translation_from_matrix(arr[idx])),
+				orientation=Quaternion(*quaternion_from_matrix(arr[idx]))
 			)
-