fc.py 文件源码

def send_hazard_camera(self):
        """ Dont bump into things! """
        self.zarj.pelvis.lean_body_to(0)
        self.zarj.neck.neck_control([0.5, 0.0, 0.0], True)
        rospy.sleep(1.0)
        cloud = self.zarj.eyes.get_stereo_cloud()
        forward, _ = self.zarj.eyes.get_cloud_image_with_details(cloud)
        forward = cv2.cvtColor(forward, cv2.COLOR_BGR2GRAY)
        forward = cv2.copyMakeBorder(forward, 0, 0, 560, 630,
                                     cv2.BORDER_CONSTANT, value=(0, 0, 0))

        self.zarj.neck.neck_control([0.5, 1.0, 0.0], True)
        rospy.sleep(1.0)
        cloud = self.zarj.eyes.get_stereo_cloud()
        right, _ = self.zarj.eyes.get_cloud_image_with_details(cloud)
        right = cv2.copyMakeBorder(right, 0, 0, 1190, 0,
                                   cv2.BORDER_CONSTANT, value=(0, 0, 0))
        right = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)

        self.zarj.neck.neck_control([0.5, -1.0, 0.0], True)
        rospy.sleep(1.0)
        cloud = self.zarj.eyes.get_stereo_cloud()
        left, _ = self.zarj.eyes.get_cloud_image_with_details(cloud)
        left = cv2.copyMakeBorder(left, 0, 0, 0, 1190,
                                  cv2.BORDER_CONSTANT, value=(0, 0, 0))
        left = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)

        self.zarj.neck.neck_control([0.0, 0.0, 0.0], True)

        haz_cam = cv2.bitwise_or(forward, left)
        haz_cam = cv2.bitwise_or(haz_cam, right)

        haz_cam = cv2.cvtColor(haz_cam, cv2.COLOR_GRAY2BGR)

        haz_cam = PERSPECTIVE_HEAD_DOWN.build_rangefinding_image(haz_cam)

        pictsize = np.shape(haz_cam)
        resized = cv2.resize(haz_cam, (pictsize[1]/2, pictsize[0]/2),
                             interpolation=cv2.INTER_AREA)

        (_, png) = cv2.imencode(".png", resized)
        msg = ZarjPicture("hazard", png)
        msg.time = rospy.get_time()
        self.zarj_comm.push_message(msg)