zps.py 文件源码

def _find_stair_base(self, cloud, distance, center, width):
        """ Try to find the base of the stairs really exactly """
        _, details = self.zarj.eyes.get_cloud_image_with_details(cloud)
        occ = 0
        pnt = None
        while pnt is None:
            if not np.isnan(details[distance][center-occ][0]):
                pnt = details[distance][center-occ]
                break
            if not np.isnan(details[distance][center+occ][0]):
                pnt = details[distance][center+occ]
                break
            occ += 1
            if occ >= width/6:
                occ = 0
                distance -= 1
        if pnt is not None:
            point = PointStamped()
            point.header = cloud.header
            point.point.x = pnt[0]
            point.point.y = pnt[1]
            point.point.z = pnt[2]
            pnt = self.zarj.transform.tf_buffer.transform(point,
                                                          self.zarj.walk.lfname)
            return pnt.point.x
        log("ERROR: Could not find stair base")
        return None