Compare poses to be equal within a tolerance

compare_poses.py

from geometry_msgs.msg import Pose

def are_poses_equal(p1, p2):
    pos1 = p1.position
    pos2 = p2.position
    if isclose(pos1.x, pos2.x) and isclose(pos1.y, pos2.y) and isclose(pos1.z, pos2.z):
        o1 = p1.orientation
        o2 = p2.orientation
        r1, p1, y1 = euler_from_quaternion([o1.x, o1.y, o1.z, o1.w])
        r2, p2, y2 = euler_from_quaternion([o2.x, o2.y, o2.z, o2.w])
        if isclose(r1, r2) and isclose(p1, p2) and isclose(y1, y2):
            return True
    return False


def isclose(n1, n2, tol=0.001):
    return abs(n1 - n2) <= tol
  
if __name__ == '__main__':
    p1 = Pose()
    p2 = pose()
    print are_poses_equal(p1, p2)

    p1.position.x = 1.0
    print are_poses_equal(p1, p2)