Norm between two PoseStamped or PoseLists
    :param point: point to compute the norm for (list or PoseStamped)
    :return: its norm (float)

def norm(point):
    """
    Norm between two PoseStamped or PoseLists
    :param point: point to compute the norm for (list or PoseStamped)
    :return: its norm (float)
    """
    if isinstance(point, PoseStamped):
        x = point.pose.position.x
        y = point.pose.position.y
        z = point.pose.position.z
    elif isinstance(point, Pose):
        x = point.position.x
        y = point.position.y
        z = point.position.z
    else:
        x = point[0][0]
        y = point[0][1]
        z = point[0][2]
    return sqrt(x * x + y * y + z * z)