task3.py 文件源码

def _find_feature(self, tag, low, high):
        """ Find a feature on the door"""
        # we only want the center of the image
        cloud = self.fc.zarj.eyes.get_stereo_cloud()
        image, details = self.fc.zarj.eyes.get_cloud_image_with_details(cloud)
        shape = image.shape
        image = image[0:shape[0], shape[1]/3:2*shape[1]/3]
        details = details[0:shape[0], shape[1]/3:2*shape[1]/3]
        colors = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        image_dump = os.environ.get("ZARJ_IMAGE_DUMP")

        mask = cv2.inRange(colors, low, high)
        if image_dump is not None:
            image_idx = rospy.get_time()
            cv2.imwrite(image_dump+'/door_{}.png'.format(image_idx), image)
            cv2.imwrite(image_dump+'/door_{}_{}.png'.format(tag, image_idx),
                        mask)
        indexes = mask.nonzero()
        points = []
        for i in range(len(indexes[1])):
            pnt = details[indexes[0][i], indexes[1][i]]
            if pnt[2] > 2.0:
                print 'Discard non {} point, too far away'.format(tag)
                continue
            points.append(details[indexes[0][i], indexes[1][i]])
        points = np.array(points)

        if len(points) < 10:
            log("Failed to find {} in the door".format(tag))
            return None

        avg = np.mean(points, axis=0)

        point = PointStamped()
        point.header = cloud.header
        point.point.x = avg[0]
        point.point.y = avg[1]
        point.point.z = avg[2]

        final = self.fc.zarj.transform.tf_buffer.transform(point, 'pelvis')
        log('{} located at about {}'.format(tag, final.point))
        return final.point.y