#!/usr/bin/env python import rospy import cv2 from CrustCrawler import CrustCrawler from Camera import Camera if __name__ == "__main__": rospy.init_node("au_dynamixel_test_node") crustCrawler = CrustCrawler() camera = Camera() right = [] left = [] while True: crustCrawler.reset() blocks, img = camera.get_blocks() cv2.imshow('Camera', img), cv2.waitKey(40) for block in blocks: if len(right) == 0 or block.same_color(right[0].color): crustCrawler.place_block_right(block, len(right)) right.append(block) elif len(left) == 0 or block.same_color(left[0].color): crustCrawler.place_block_left(block, len(left)) left.append(block)