Урок 20

import rospy
import numpy as np
import cv2
from cv_bridge import CvBridge

from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage


class ArucoDetector():

    def __init__(self):

        self.sub_image_original = rospy.Subscriber('/front_camera/compressed', CompressedImage, self.cbGetImage, queue_size = 1)
        self.detect_pub = rospy.Publisher('/aruco_detect', String, queue_size = 1)

        self.cvBridge = CvBridge()
        self.cv_image = 0

        self.arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_100)
        self.arucoParams = cv2.aruco.DetectorParameters_create()

        self.counter = 0

        while not rospy.is_shutdown():
            rospy.sleep(1)
            self.ArucoDetect()

    def cbGetImage(self, image_msg):
        # Change the frame rate by yourself. Now, it is set to 1/3 (10fps). 
        # Unappropriate value of frame rate may cause huge delay on entire recognition process.
        # This is up to your computer's operating power.
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1
        np_arr = np.frombuffer(image_msg.data, np.uint8)
        self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)

    def FindAruco(self):
        (corners, marker, rejected) = cv2.aruco.detectMarkers(self.cv_image, self.arucoDict, parameters=self.arucoParams)
        return marker

    def DetermineAruco(self, marker):
        if marker:
            return marker[0][0]
        return -1


    def ResultPub(self, result):
        r = String()
        r.data = str(result)
        self.detect_pub.publish(r)

    def ArucoDetect(self):
        marker = self.FindAruco()
        number_in_aruco = self.DetermineAruco(marker)
        self.ResultPub(number_in_aruco)

    def main(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('detect_aruco')
    a = ArucoDetector()
    a.main()