Python example: ros-poses-convert.py

Converts MRPT poses and pose PDFs from/to ROS geometry_msgs Pose and PoseWithCovariance.

Modules: mrpt.math, mrpt.poses

  1#!/usr/bin/env python3
  2"""
  3Converts MRPT poses and pose PDFs from/to ROS geometry_msgs Pose and PoseWithCovariance.
  4
  5Uses the ROS 2 (or ROS 1) geometry_msgs Python messages if available, or
  6minimal stand-in classes with the same fields otherwise, so it runs anywhere.
  7C++ code can use the equivalent functions of the mrpt_ros_bridge package.
  8"""
  9
 10import math
 11from types import SimpleNamespace
 12
 13import numpy as np
 14from mrpt.math import CMatrixDouble66
 15from mrpt.poses import CPose2D, CPose3D, CPose3DPDFGaussian
 16
 17try:
 18    from geometry_msgs.msg import Pose, PoseWithCovariance
 19except ImportError:
 20    def Pose():
 21        return SimpleNamespace(
 22            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
 23            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0))
 24
 25    def PoseWithCovariance():
 26        return SimpleNamespace(pose=Pose(), covariance=[0.0] * 36)
 27
 28# Covariance index of each MRPT variable (x y z yaw pitch roll) in ROS
 29# (x y z rot_x rot_y rot_z):
 30MRPT_TO_ROS_COV = [0, 1, 2, 5, 4, 3]
 31
 32
 33def pose3d_to_ros(p: CPose3D):
 34    yaw, pitch, roll = p.getYawPitchRoll()
 35    cy, sy = math.cos(yaw / 2), math.sin(yaw / 2)
 36    cp, sp = math.cos(pitch / 2), math.sin(pitch / 2)
 37    cr, sr = math.cos(roll / 2), math.sin(roll / 2)
 38    msg = Pose()
 39    msg.position.x, msg.position.y, msg.position.z = p.x, p.y, p.z
 40    msg.orientation.w = cr * cp * cy + sr * sp * sy
 41    msg.orientation.x = sr * cp * cy - cr * sp * sy
 42    msg.orientation.y = cr * sp * cy + sr * cp * sy
 43    msg.orientation.z = cr * cp * sy - sr * sp * cy
 44    return msg
 45
 46
 47def ros_to_pose3d(msg) -> CPose3D:
 48    q = msg.orientation
 49    yaw = math.atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z))
 50    pitch = math.asin(max(-1.0, min(1.0, 2 * (q.w * q.y - q.z * q.x))))
 51    roll = math.atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.x * q.x + q.y * q.y))
 52    pos = msg.position
 53    return CPose3D.FromXYZYawPitchRoll(pos.x, pos.y, pos.z, yaw, pitch, roll)
 54
 55
 56def pose2d_to_ros(p: CPose2D):
 57    return pose3d_to_ros(CPose3D(p))
 58
 59
 60def ros_to_pose2d(msg) -> CPose2D:
 61    return CPose2D(ros_to_pose3d(msg))
 62
 63
 64def pdf3d_to_ros(pdf: CPose3DPDFGaussian):
 65    msg = PoseWithCovariance()
 66    msg.pose = pose3d_to_ros(pdf.mean)
 67    cov = np.array(pdf.cov)
 68    ros_cov = np.zeros((6, 6))
 69    for i in range(6):
 70        for j in range(6):
 71            ros_cov[MRPT_TO_ROS_COV[i], MRPT_TO_ROS_COV[j]] = cov[i, j]
 72    msg.covariance = ros_cov.flatten().tolist()
 73    return msg
 74
 75
 76def ros_to_pdf3d(msg) -> CPose3DPDFGaussian:
 77    ros_cov = np.array(msg.covariance).reshape(6, 6)
 78    cov = np.zeros((6, 6))
 79    for i in range(6):
 80        for j in range(6):
 81            cov[i, j] = ros_cov[MRPT_TO_ROS_COV[i], MRPT_TO_ROS_COV[j]]
 82    return CPose3DPDFGaussian(ros_to_pose3d(msg.pose), CMatrixDouble66(cov.tolist()))
 83
 84
 85def fmt(msg):
 86    p, q = msg.position, msg.orientation
 87    return 'position=({:.3f} {:.3f} {:.3f}) orientation(x y z w)=({:.4f} {:.4f} {:.4f} {:.4f})'.format(
 88        p.x, p.y, p.z, q.x, q.y, q.z, q.w)
 89
 90
 91# SE(2):
 92p1 = CPose2D(1.0, 2.0, math.radians(90.0))
 93ros_p1 = pose2d_to_ros(p1)
 94print('mrpt CPose2D        : ' + str(p1))
 95print('  -> ROS Pose       : ' + fmt(ros_p1))
 96print('  -> back to MRPT   : ' + str(ros_to_pose2d(ros_p1)))
 97
 98# SE(3):
 99p2 = CPose3D.FromXYZYawPitchRoll(10.0, 5.0, 0.5, math.radians(30), math.radians(-10), math.radians(5))
100ros_p2 = pose3d_to_ros(p2)
101print('mrpt CPose3D        : ' + str(p2))
102print('  -> ROS Pose       : ' + fmt(ros_p2))
103print('  -> back to MRPT   : ' + str(ros_to_pose3d(ros_p2)))
104
105# SE(3) with uncertainty:
106cov = np.diag([0.1, 0.2, 0.3, 0.01, 0.02, 0.03])  # x y z yaw pitch roll
107pdf = CPose3DPDFGaussian(p2, CMatrixDouble66(cov.tolist()))
108ros_pdf = pdf3d_to_ros(pdf)
109print('ROS covariance diag : ' + str(np.diag(np.array(ros_pdf.covariance).reshape(6, 6))))
110pdf_back = ros_to_pdf3d(ros_pdf)
111print('back to MRPT, diag  : ' + str(np.diag(np.array(pdf_back.cov))))

Output:

mrpt CPose2D        : [1.000000 2.000000 90.000000deg]
  -> ROS Pose       : position=(1.000 2.000 0.000) orientation(x y z w)=(0.0000 0.0000 0.7071 0.7071)
  -> back to MRPT   : [1.000000 2.000000 90.000000deg]
mrpt CPose3D        : [10.000000 5.000000 0.500000 30.000000 -10.000000 5.000000]
  -> ROS Pose       : position=(10.000 5.000 0.500) orientation(x y z w)=(0.0645 -0.0729 0.2613 0.9604)
  -> back to MRPT   : [10.000000 5.000000 0.500000 30.000000 -10.000000 5.000000]
ROS covariance diag : [0.1  0.2  0.3  0.03 0.02 0.01]
back to MRPT, diag  : [0.1  0.2  0.3  0.01 0.02 0.03]