Calculer la différence entre Pose et Transform avec ROS en Python

Un groupe de fonctions qui calculent les différences dans Geometry_msgs en utilisant Python

• Vous voudrez peut-être faire la différence entre Pose et Transform. --Si vous êtes inscrit dans l'arborescence tf, vous devez absolument utiliser la transformation de recherche de tf «Ce n'était pas le cas, et il ne semblait pas y avoir de package approprié lorsque je n'utilisais pas C ++, j'ai donc créé un groupe de fonctions.

** C'était une réinvention de la roue, mais j'ai dû l'écrire moi-même parce que je ne trouvais pas de package facile à utiliser. ** ** Nous prévoyons d'ajouter les fonctions nécessaires dès leur apparition.

S'il y a une demande, gérez-la avec Git.

liste

--Fonctions liées à Geometry_msgs.msg.Transform () --transform2homogeneousM: Transforme le message de transformation en une matrice homogène --homogeneous2transform: Reconvertissez la matrice de transformation homogène en un message de transformation --transform_diff (tf1, tf2) : affiche la différence entre les deux messages de transformation sous forme de message de transformation --Fonctions liées à Geometry_msgs.msg.Pose () --pose2homogeneousM: convertit un message de pose en une matrice homogène --homogeneous2pose: Reconvertissez la matrice de conversion homogène en un message de pose --pose_diff (p1, p2) : affiche la différence entre deux messages de pose sous forme de message de pose

code

import rospy
import geometry_msg
import tf

# Transform to homogeneous matrix
def transform2homogeneousM(tfobj):
    #Il dit Quat à euler sxyz, mais l'ordre de XYZW est bien. N'est-ce pas un peu déroutant?
    tfeul= tf.transformations.euler_from_quaternion([tfobj.rotation.x,tfobj.rotation.y,tfobj.rotation.z,tfobj.rotation.w],axes='sxyz')
    #Description du montant de la traduction
    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)))
    

#Faire également une version Pose
def pose2homogeneousM(poseobj):
    try:
        #Il dit Quat à euler sxyz, mais l'ordre de XYZW est bien. N'est-ce pas un peu déroutant?
        tfeul= tf.transformations.euler_from_quaternion([poseobj.orientation.x,poseobj.orientation.y,poseobj.orientation.z,poseobj.orientation.w],axes='sxyz')
        #Description du montant de la traduction
        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)))