code: def distance(p1, p2):
      
  
      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)
docs: 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)