spatialmath-rospy

Spatial Math for ROS. Intergration library between rospy and spatialmath-python. Currently this lib just contains conversion functionality. Expect the conversion modules to work in any ROS1 version. Tested only on ROS1 Noetic due to difficulties with the CI config.

Install

pip install spatialmath-rospy

Usage

Extended Classes [Recommended]

# These classes extend their original spatialmath counterparts and  
from spatialmath_rospy import SE3, SO3, UnitQuaternion

pose_msg = SE3(1, 2, 3).to_ros()
print(type(pose_msg), '\n', pose_msg)
"""
<class 'geometry_msgs.msg._Pose.Pose'> 
position: 
  x: 1.0
  y: 2.0
  z: 3.0
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
"""

se3: SE3 = SE3.from_ros(pose_msg)
print(se3)
"""
  1         0         0         1         
  0         1         0         2         
  0         0         1         3         
  0         0         0         1
"""

Conversion Functions

For those who prefer a functional style or don't want to use the extended classes

import spatialmath as sm
from spatialmath_rospy import to_spatialmath, to_ros

pose_msg = to_ros(sm.SE3(1, 2, 3))
se3: sm.SE3 = to_spatialmath(pose_msg)

Monkey Patching

[Not Recommended]

You may prefer to use this option if wanting to add the .from_ros() and .to_ros() methods to the original SE3, SO3 and UnitQuaternion classes via a monkey-patch. This may be useful for integrating legacy code. Not recommended as static type analysis tools like PyLance will not work.

import spatialmath as sm
from spatialmath_rospy import monkey_patch_spatialmath

# Invoke this at any point before calling conversion functions
monkey_patch_spatialmath()

pose_msg = sm.SE3(1, 2, 3).to_ros()

ROS Transform Messages

The to_ros() function returns a Pose msg by default.

A Transform msg can be returned instead with to_ros(as_tf=True).

from spatialmath_rospy import SE3

tf_msg = SE3(1, 2, 3).to_ros(as_tf=True)
print(type(tf_msg), '\n', tf_msg)
"""
<class 'geometry_msgs.msg._Transform.Transform'>
translation: 
  x: 1.0
  y: 2.0
  z: 3.0
rotation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
"""

ROS Quaternion Messages

Quaternion msgs convert to UnitQuaternion objects and vice versa:

from spatialmath_rospy import UnitQuaternion

quat_msg = UnitQuaternion(1, [0, 0, 0]).to_ros()
print(type(quat_msg), '\n', quat_msg)
"""
<class 'geometry_msgs.msg._Quaternion.Quaternion'> 
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
unit_quat = UnitQuaternion.from_ros(quat_msg)
print(unit_quat)
" 1.0000 <<  0.0000,  0.0000,  0.0000 >>

UnitQuaternion can also be converted to a Transform msg with to_ros(as_tf=True):

from spatialmath_rospy import UnitQuaternion, SE3

quat = UnitQuaternion(1, [0, 0, 0])

tf_msg = quat.to_ros(as_tf=True)
print(tf_msg)
"""
translation: 
  x: 0
  y: 0
  z: 0
rotation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
"""

This Transform will have always zero translation.

ROS Stamped messages

Just pass a std_msgs.msg.Header in to to_ros() to construct stamped objects:

from std_msgs.msg import Header
from spatialmath_rospy import SE3

pose_stamped_msg = SE3(1, 2, 3).to_ros(
  Header(frame_id="world")
)
print(type(pose_stamped_msg), '\n', pose_stamped_msg)
"""
<class 'geometry_msgs.msg._PoseStamped.PoseStamped'> 
header: 
  seq: 0
  stamp: 
    secs: 0
    nsecs: 0
  frame_id: "world"
pose: 
  position: 
    x: 1.0
    y: 2.0
    z: 3.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 1.0
"""

This works for all supported ros msg types:

• Pose / PoseStamped

• Transform / TransformStamped

• Quaternion / QuaternionStamped

Contributors ✨

Thanks goes to these wonderful people (emoji key):

This project follows the all-contributors specification. Contributions of any kind welcome!

Credits

This package was created with Cookiecutter and the browniebroke/cookiecutter-pypackage project template.