Calculate Pose and Transform differences in Python with ROS

Functions that calculate differences in Geometry_msgs using Python

--You may want to take the difference between Pose and Transform. --If registered in tf tree, you should definitely use tf lookup transform --Otherwise, I didn't seem to have a suitable package when I didn't use C ++, so I created a set of functions.

** It was a reinvention of the wheel, but I had to write it myself because I couldn't find a package that was easy to use. ** ** We plan to add the necessary functions as soon as they come up.

If there is demand, manage it with Git.

List

--Functions related to Geometry_msgs.msg.Transform () --transform2homogeneousM: Transform the transform message into a homogeneous transformation matrix (Homogeneous Matrix) --homogeneous2transform: Retransform the homogeneous transformation matrix into a transform message --transform_diff (tf1, tf2): Output the difference between the two transform messages as a transform message --Functions related to Geometry_msgs.msg.Pose () --pose2homogeneousM: Converts a pose message into a homogeneous transformation matrix (Homogeneous Matrix) --homogeneous2pose: Reconvert the homogeneous transformation matrix into a pose message --pose_diff (p1, p2): Output the difference between two pose messages as a pose message

code

import rospy
import geometry_msg
import tf

# Transform to homogeneous matrix
def transform2homogeneousM(tfobj):
    #It says Quat to euler sxyz, but the order of XYZW is fine. Isn't it a little confusing?
    tfeul= tf.transformations.euler_from_quaternion([tfobj.rotation.x,tfobj.rotation.y,tfobj.rotation.z,tfobj.rotation.w],axes='sxyz')
    #Description of translation amount
    tftrans = [ tfobj.translation.x,tfobj.translation.y,tfobj.translation.z]
    tfobjM = tf.transformations.compose_matrix(angles=tfeul,translate=tftrans)
    # return
    return  tfobjM

def homogeneous2transform(Mat):
    scale, shear, angles, trans, persp = tf.transformations.decompose_matrix(Mat)
    quat = tf.transformations.quaternion_from_euler(angles[0],angles[1],angles[2])
    tfobj = geometry_msgs.msg.Transform()
    tfobj.rotation.x = quat[0]
    tfobj.rotation.y = quat[1]
    tfobj.rotation.z = quat[2]
    tfobj.rotation.w = quat[3]
    tfobj.translation.x = trans[0]
    tfobj.translation.y = trans[1]
    tfobj.translation.z = trans[2]
    return tfobj
    
# Transform diff tf1 to 2

def transform_diff(tf1,tf2):
    tf1M = transform2homogeneousM(tf1)
    tf2M = transform2homogeneousM(tf2)
    return  homogeneous2transform(tf2M.dot(tf.transformations.inverse_matrix(tf1M)))
    

#Also make a Pose version
def pose2homogeneousM(poseobj):
    try:
        #It says Quat to euler sxyz, but the order of XYZW is fine. Isn't it a little confusing?
        tfeul= tf.transformations.euler_from_quaternion([poseobj.orientation.x,poseobj.orientation.y,poseobj.orientation.z,poseobj.orientation.w],axes='sxyz')
        #Description of translation amount
        tftrans = [ poseobj.position.x,poseobj.position.y,poseobj.position.z]
        poseobjM = tf.transformations.compose_matrix(angles=tfeul,translate=tftrans)
        return poseobjM
    except:
        print("Input must be a pose object!")

        
def homogeneous2pose(Mat):
    scale, shear, angles, trans, persp = tf.transformations.decompose_matrix(Mat)
    quat = tf.transformations.quaternion_from_euler(angles[0],angles[1],angles[2])
    poseobj = geometry_msgs.msg.Pose()
    poseobj.orientation.x = quat[0]
    poseobj.orientation.y = quat[1]
    poseobj.orientation.z = quat[2]
    poseobj.orientation.w = quat[3]
    poseobj.position.x = trans[0]
    poseobj.position.y = trans[1]
    poseobj.position.z = trans[2]
    return poseobj

def pose_diff(p1,p2):
    p1M = pose2homogeneousM(p1)
    p2M = pose2homogeneousM(p2)
    return  homogeneous2pose(p2M.dot(tf.transformations.inverse_matrix(p1M)))