コード例 #1
0
ファイル: kinect.py プロジェクト: vincentsavard/design3
def runKinect(table, callback):
    kinectImageTaker = KinectImageTaker()
    time.sleep(2)

    while True:
        rgbImage, depthImage = kinectImageTaker.getImages()

        cv2.imshow("", rgbImage)
        cc = cv2.waitKey(10)
        
        if cc == 27:
            break
        elif cc == 32:
            callback(table, rgbImage, depthImage)

    cv2.destroyAllWindows()
コード例 #2
0
ファイル: MapUpdater.py プロジェクト: vincentsavard/design3
class MapUpdater:
    ROW_NUMBER = 117
    SQUARE_SIZE_IN_CENTIMETERS = 2
    NUMBER_OF_RETRIES = 3

    def __init__(self, transformMatrix):
        self.imageTaker=KinectImageTaker()
        self.transformMatrix = transformMatrix
        self.lastRobotPosition = (50, 50)
        self.lastRobotOrientation = 0

    def updateMap(self):
        for _ in range(self.NUMBER_OF_RETRIES):
            try:
                rgbImage, depthImage = self.imageTaker.getImages()
                pointConvertor = KinectPointConvertor(rgbImage, depthImage, self.transformMatrix)
                robotFinder = RobotFinder(rgbImage, pointConvertor)
                self.lastRobotPosition, self.lastRobotOrientation= robotFinder.findPositionAndOrientation()
            except (InvalidDistanceException, NoContourFoundException) as e:
                print "updateMap: ", str(e)

        x, y = convertCoordinatesToMapPoint(self.lastRobotPosition, self.ROW_NUMBER, self.SQUARE_SIZE_IN_CENTIMETERS)            
        x-=5 #We want upperleft point
        y-=5
        robotPosition=(int(x),int(y))
        robotOrientation = self.lastRobotOrientation * 180 / numpy.pi

        rowNumber, columnNumber = 117, 56

        borders = [(RectangularObject(1, columnNumber), (0, 0), False),
                   (RectangularObject(1, columnNumber), (rowNumber - 1, 0), False),
                   (RectangularObject(rowNumber, 1), (0, 0), False),
                   (RectangularObject(rowNumber, 1), (0, columnNumber - 1), False)]
        startingZoneCorners = [(RectangularObject(1, 1), (106, 44), True),
                               (RectangularObject(1, 1), (73, 44), True),
                               (RectangularObject(1, 1), (106, 11), True),
                               (RectangularObject(1, 1), (73, 11), True)]

        puckZones = [(RectangularObject(40, 3), (1, 1), False),
                     (RectangularObject(3, 54), (1, 1), False),
                     (RectangularObject(40, 3), (1, 52), False)]

        cornerCommandPanel, cornerCommandPanelPosition = RectangularObject(1, 8), (0, 14)
        clockwiseCommandPanel, clockwiseCommandPanelPosition = RectangularObject(1, 8), (0, 32)
        orangePanel, orangePanelPosition = RectangularObject(7, 1), (103, columnNumber-1)
        robot= PaddedRectangularObject(11, 11, 4)

        cartographySystem = CartographySystem(rowNumber, columnNumber)    
        for mapObject, coordinates, walkable in borders + startingZoneCorners + puckZones:
            cartographySystem.createObject(mapObject, *coordinates, walkable=walkable)
        cartographySystem.createObject(cornerCommandPanel, *cornerCommandPanelPosition, walkable=False)
        cartographySystem.createObject(clockwiseCommandPanel, *clockwiseCommandPanelPosition, walkable=False)
        cartographySystem.createObject(orangePanel, *orangePanelPosition, walkable=False)
        cartographySystem.createObject(robot, *robotPosition, walkable=True)
        cartographySystem.rotateObject(robot, robotOrientation)

        return cartographySystem
コード例 #3
0
ファイル: MapUpdater.py プロジェクト: vincentsavard/design3
 def __init__(self, transformMatrix):
     self.imageTaker=KinectImageTaker()
     self.transformMatrix = transformMatrix
     self.lastRobotPosition = (50, 50)
     self.lastRobotOrientation = 0