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