Cartesian distance between two PoseStamped or PoseLists
    :param p1: point 1 (list, Pose or PoseStamped)
    :param p2: point 2 (list, Pose or PoseStamped)
    :return: cartesian distance (float)

def distance(p1, p2):
    """
    Cartesian distance between two PoseStamped or PoseLists
    :param p1: point 1 (list, Pose or PoseStamped)
    :param p2: point 2 (list, Pose or PoseStamped)
    :return: cartesian distance (float)
    """

    def xyz(some_pose):
        if isinstance(some_pose, PoseStamped):
            return some_pose.pose.position.x, some_pose.pose.position.y, some_pose.pose.position.z
        elif isinstance(some_pose, Pose):
            return some_pose.position.x, some_pose.position.y, some_pose.position.z
        elif _is_indexable(some_pose[0]):
            return some_pose[0][0], some_pose[0][1], some_pose[0][2]
        else:
            return some_pose[0], some_pose[1], some_pose[2]

    x1, y1, z1 = xyz(p1)
    x2, y2, z2 = xyz(p2)

    x = x1 - x2
    y = y1 - y2
    z = z1 - z2
    return sqrt(x * x + y * y + z * z)