class MissionTasks:
    def __init__(self):
        self.zedListener = ZedListener()
	self.zedListener.listen()
        self.vision = VisionTasks()
        self.pub = rospy.Publisher('objectCoordinates', VISION, queue_size=10)
        self.rate = None
    
    def missionControl(self):
        # THIS CAN BE MOVED TO MAIN CONTROL IF NEEDED
        #find gate
        self.doTask("gate") #change time as needed
        #find dice
        #self.doTask(300, "dice") #change time as needed

    def doTask(self, task):
        self.rate = rospy.Rate(10)  # 10 Hz
        while True:
            try:
                image = self.zedListener.getImage()
                depth = self.zedListener.getDepth()
                if image is not None:
                    coords = self.processImage(task, image)
                    img, x, y, w = coords
                    meanDepth = -1
                    if depth is not None:
                        #depthArray = np.asarray(depth)
                        #meanDepth = depthArray[y,x]
                        meanDepth = depth[y, x]
                    self.talk(coords, meanDepth)
                self.rate.sleep()
            except rospy.ROSInterruptException:
                rospy.loginfo("Program interrupted")
    
    def processImage(self, task, image):
        if task == "gate":
            coords = self.vision.findGate(image)
        elif task == "dice":
            coords = self.vision.findDice(image)
        return coords

    def talk(self, coords, depth):
        #rospy.loginfo(x,y)
        msg = VISION()
        img, x, y, w = coords
        #cv2.imshow("image", img)
        #cv2.waitKey(1)
        msg.x_coord = x
        msg.y_coord = y
        msg.width = w
        msg.depth = depth
        self.pub.publish(msg)
示例#2
0
 def __init__(self):
     self.zedListener = ZedListener()
     self.zedListener.listen()
     self.vision = VisionTasks()
     self.pub = rospy.Publisher('objectCoordinates', Vision, queue_size=10)
     self.rate = None