spatialmath-python and rospy bridge library
Project description
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.
Project details
Download files
Download the file for your platform. If you're not sure which to choose, learn more about installing packages.
Source Distribution
Built Distribution
Hashes for spatialmath_rospy-0.3.1-py3-none-any.whl
Algorithm | Hash digest | |
---|---|---|
SHA256 | fa0998ebc475f1660a3897bf567bdbd18632fecde35d95b10ca65bbe71636b8d |
|
MD5 | 68f8e42d0275b99659ee85d91fb0c4fb |
|
BLAKE2b-256 | be23e55814502a042a42681ea0e342dc0917f3c4258a028acb2b424808eb299f |