Python example: ros-poses-convert.py

This example shows how to convert back and forth between MRPT poses and ROS 1 or ROS 2 (both are compatible with this same code) Pose and PoseWithCovariance.

 1#!/usr/bin/env python3
 2
 3# ---------------------------------------------------------------------
 4# Install python3-pymrpt, ros-$ROS_DISTRO-python-mrpt,
 5# ros-$ROS_DISTRO-mrpt2, or test with a local build with:
 6# export PYTHONPATH=$HOME/code/mrpt/build-Release/:$PYTHONPATH
 7# ---------------------------------------------------------------------
 8
 9# This example shows how to convert back and forth between MRPT poses
10# and ROS 1 or ROS 2 (both are compatible with this same code) Pose
11
12from mrpt.pymrpt import mrpt
13from mrpt import ros_bridge
14from math import radians
15from geometry_msgs.msg import Pose, PoseWithCovariance
16
17
18# Example 1: 2D pose
19# --------------------------
20p1 = mrpt.poses.CPose2D(1.0, 2.0, radians(90.0))
21rp1 = ros_bridge.CPose2D_to_ROS_Pose_msg(p1)
22
23p2 = ros_bridge.ROS_Pose_msg_to_CPose3D(rp1)
24
25print('mrpt p1             : ' + str(p1))
26print('ros  p1             : ' + str(rp1))
27print('mrpt p2             : ' + str(p2))
28
29# Example 2: 3D pose
30# ----------------------
31r2 = Pose()
32r2.position.x = 10.0
33r2.position.y = 5.0
34r2.position.z = 0.0
35r2.orientation.w = 1.0
36r2.orientation.x = 0.0
37r2.orientation.y = 0.0
38r2.orientation.z = 0.0
39
40mr2 = ros_bridge.ROS_Pose_msg_to_CPose3D(r2)
41mr2b = ros_bridge.ROS_Pose_msg_to_CPose3DQuat(r2)
42
43print()
44print('ros  r2             : ' + str(r2))
45print('mrpt mr2            : ' + str(mr2))
46print('mrpt mr2b           : ' + str(mr2b))
47
48# Example 3: with covariance
49# -------------------------------
50r3 = PoseWithCovariance()
51r3.pose = r2
52r3.covariance = [1.0, 0.0, 0.0,  0.0, 0.0, 0.0,
53                 0.0, 1.0, 0.0,  0.0, 0.0, 0.0,
54                 0.0, 0.0, 1.0,  0.0, 0.0, 0.0,
55                 0.0, 0.0, 0.0,  1.0, 0.0, 0.0,
56                 0.0, 0.0, 0.0,  0.0, 1.0, 0.0,
57                 0.0, 0.0, 0.0,  0.0, 0.0, 1.0]
58
59mr3 = ros_bridge.ROS_PoseWithCovariance_msg_to_CPose3DPDFGaussian(r3)
60
61mr3b = mr3
62mr3b.mean.x(mr3b.mean.x()+1.0)
63r3b = ros_bridge.CPose3DPDFGaussian_to_ROS_PoseWithCovariance_msg(mr3)
64
65print()
66print('ros PDF r3         : ' + str(r3))
67print('mrpt PDF mr3       : ' + str(mr3))
68print('mrpt PDF mr3b      : ' + str(mr3b))
69print('ros PDF r3b        : ' + str(r3b))
70
71a = mr3
72b = mr3
73c = a+b
74
75print('r3b (+) rb3        : ' + str(c))

Output:

 1mrpt p1             : (1.000,2.000,90.00deg)
 2ros  p1             : geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=1.0, y=2.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.7071067811865475, w=0.7071067811865476))
 3mrpt p2             : (x,y,z,yaw,pitch,roll)=(1.0000,2.0000,0.0000,90.00deg,-0.00deg,0.00deg)
 4
 5ros  r2             : geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=10.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))
 6mrpt mr2            : (x,y,z,yaw,pitch,roll)=(10.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
 7mrpt mr2b           : (x,y,z,qr,qx,qy,qz)=(10.0000,5.0000,0.0000,1.0000,0.0000,0.0000,0.0000)
 8
 9ros PDF r3         : geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=10.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
10       0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
11       0., 1.]))
12mrpt PDF mr3       : Mean: (x,y,z,yaw,pitch,roll)=(11.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
13Covariance:
141 0 0 0 0 0
150 1 0 0 0 0
160 0 1 0 0 0
170 0 0 1 0 0
180 0 0 0 1 0
190 0 0 0 0 1
20
21mrpt PDF mr3b      : Mean: (x,y,z,yaw,pitch,roll)=(11.0000,5.0000,0.0000,0.00deg,-0.00deg,0.00deg)
22Covariance:
231 0 0 0 0 0
240 1 0 0 0 0
250 0 1 0 0 0
260 0 0 1 0 0
270 0 0 0 1 0
280 0 0 0 0 1
29
30ros PDF r3b        : geometry_msgs.msg.PoseWithCovariance(pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=11.0, y=5.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), covariance=array([1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0.,
31       0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.,
32       0., 1.]))