eyes_qual.py 文件源码

def detection(self, hsv_image):
        """Check for detection in the image """
        mask = cv2.inRange(hsv_image, self.color_low, self.color_high)
        if self.baseline_cnt > 0:
            nmask = cv2.bitwise_not(mask)
            if self.baseline is None:
                rospy.loginfo("getting baseline for {}".format(self.name))
                self.baseline = nmask
            else:
                self.baseline = cv2.bitwise_or(nmask, self.baseline)
                mask = cv2.bitwise_and(mask, self.baseline)
                count = cv2.countNonZero(mask) + self.baseline_fuzzy
                self.low_count = max(self.low_count, count)
                self.high_count = self.low_count + self.baseline_fuzzy
            self.baseline_cnt -= 1
            return
        elif self.baseline is not None:
            mask = cv2.bitwise_and(mask, self.baseline)
        count = cv2.countNonZero(mask)
        if count > self.low_count and self.active is None:
            self.active = rospy.get_rostime()
            rospy.loginfo("{} ACTIVE ({})".format(self.name, count))
            self.cloud.reset()
            if self.callbacks[0] is not None:
                self.callbacks[0](self.name)
        elif self.active is not None and count < self.high_count:
            rospy.loginfo("{} GONE ({})".format(self.name, count))
            self.cloud.reset()
            self.active = None
            if self.callbacks[2] is not None:
                self.published = False
            self.report_count = 0
            if self.callbacks[1] is not None:
                self.callbacks[1](self.name)

        # DEBUGGING to see what the masked image for the request is
        if self.debug:
            cv2.namedWindow(self.name, cv2.WINDOW_NORMAL)
            if self.baseline is not None:
                cv2.namedWindow(self.name+'_baseline', cv2.WINDOW_NORMAL)
                cv2.imshow(self.name+'_baseline', self.baseline)
            cv2.imshow(self.name, mask)
            cv2.waitKey(1)

        # to to see if we notify the location callback
        if self.is_active() and self.report_count > self.min_reports:
            now = rospy.get_rostime()
            if (self.active + self.stablize_time) < now:
                self.published = True
                point = PointStamped()
                center = self.cloud.find_center()
                point.header.seq = 1
                point.header.stamp = now
                point.header.frame_id = self.frame_id
                point.point.x = center[0]
                point.point.y = center[1]
                point.point.z = center[2]
                if self.callbacks[2] is not None:
                    self.callbacks[2](self.name, point)