def distance(p1, p2):
def xyz(some_pose):<EOL><INDENT>if isinstance(some_pose, PoseStamped):<EOL><INDENT>return some_pose.pose.position.x, some_pose.pose.position.y, some_pose.pose.position.z<EOL><DEDENT>elif isinstance(some_pose, Pose):<EOL><INDENT>return some_pose.position.x, some_pose.position.y, some_pose.position.z<EOL><DEDENT>elif _is_indexable(some_pose[<NUM_LIT:0>]):<EOL><INDENT>return some_pose[<NUM_LIT:0>][<NUM_LIT:0>], some_pose[<NUM_LIT:0>][<NUM_LIT:1>], some_pose[<NUM_LIT:0>][<NUM_LIT:2>]<EOL><DEDENT>else:<EOL><INDENT>return some_pose[<NUM_LIT:0>], some_pose[<NUM_LIT:1>], some_pose[<NUM_LIT:2>]<EOL><DEDENT><DEDENT>x1, y1, z1 = xyz(p1)<EOL>x2, y2, z2 = xyz(p2)<EOL>x = x1 - x2<EOL>y = y1 - y2<EOL>z = z1 - z2<EOL>return sqrt(x * x + y * y + z * z)<EOL>
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)