Berechnen Sie Pose- und Transformationsunterschiede in Python mit ROS

Eine Gruppe von Funktionen, die die Differenz von Geometry_msgs mit Python berechnen

• Vielleicht möchten Sie den Unterschied zwischen Pose und Transform nehmen.
• Wenn Sie im tf-Baum registriert sind, sollten Sie auf jeden Fall die tf-Lookup-Transformation verwenden ――Es war nicht so, und es schien kein geeignetes Paket zu geben, wenn C ++ nicht verwendet wurde. Deshalb habe ich eine Gruppe von Funktionen erstellt.

** Es war eine Neuerfindung des Rades, aber ich musste es selbst schreiben, weil ich kein einfach zu bedienendes Paket finden konnte. ** ** ** Wir planen, die erforderlichen Funktionen hinzuzufügen, sobald sie verfügbar sind.

Wenn Bedarf besteht, verwalten Sie es mit Git.

Aufführen

--Funktionen im Zusammenhang mit Geometry_msgs.msg.Transform () --transform2homogeneM: Transformiert die Transformationsnachricht in eine homogene Matrix --homogene2transform: Konvertiert die homogene Transformationsmatrix in eine Transformationsnachricht --transform_diff (tf1, tf2) : Gibt die Differenz zwischen den beiden Transformationsnachrichten als Transformationsnachricht aus --Funktionen im Zusammenhang mit Geometry_msgs.msg.Pose () --pose2homogeneM: Konvertiert eine Pose-Nachricht in eine homogene Matrix --homogene2pose: Konvertiert die homogene Konvertierungsmatrix in eine Pose-Nachricht --pose_diff (p1, p2) : Gibt die Differenz zwischen zwei Pose-Nachrichten als Pose-Nachricht aus

Code

import rospy
import geometry_msg
import tf

# Transform to homogeneous matrix
def transform2homogeneousM(tfobj):
    #Es sagt Quat zu euler sxyz, aber die Reihenfolge von XYZW ist in Ordnung. Ist es nicht ein bisschen verwirrend?
    tfeul= tf.transformations.euler_from_quaternion([tfobj.rotation.x,tfobj.rotation.y,tfobj.rotation.z,tfobj.rotation.w],axes='sxyz')
    #Beschreibung des Übersetzungsbetrags
    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)))
    

#Machen Sie auch eine Pose-Version
def pose2homogeneousM(poseobj):
    try:
        #Es sagt Quat zu euler sxyz, aber die Reihenfolge von XYZW ist in Ordnung. Ist es nicht ein bisschen verwirrend?
        tfeul= tf.transformations.euler_from_quaternion([poseobj.orientation.x,poseobj.orientation.y,poseobj.orientation.z,poseobj.orientation.w],axes='sxyz')
        #Beschreibung des Übersetzungsbetrags
        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)))