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