def __init__(self, obstacle1, obstacle2): Terrain.OBSTACLE_1 = obstacle1 Terrain.OBSTACLE_2 = obstacle2 print "obstacle 1: " + str(obstacle1) print "obstacle 2: " + str(obstacle2) SendEvent.send(SendConfirm()) self.robotMover = RobotMover() self.signalSearcher = ManchesterSignalSearcher() self.captorsController = CaptorsController() self.imagePointsTransformer = ImagePointsTransformer() self.cam = Camera() self.ledController = LedController() self.firstTurnOver = False
from python.src.robot.vision.Camera import Camera cam = Camera() print "Extracting points with camera..." drawingCountour = cam.getDrawingContour() print str(drawingCountour)
class BeginState: def __init__(self, obstacle1, obstacle2): Terrain.OBSTACLE_1 = obstacle1 Terrain.OBSTACLE_2 = obstacle2 print "obstacle 1: " + str(obstacle1) print "obstacle 2: " + str(obstacle2) SendEvent.send(SendConfirm()) self.robotMover = RobotMover() self.signalSearcher = ManchesterSignalSearcher() self.captorsController = CaptorsController() self.imagePointsTransformer = ImagePointsTransformer() self.cam = Camera() self.ledController = LedController() self.firstTurnOver = False def run(self): self.ledController.turnLedOff() interpretedSignal = () if not self.firstTurnOver: self.__acquireCurrentPose() self.__doZignage() interpretedSignal, signalPosition = self.signalSearcher.searchSignal() self.signalPosition = signalPosition else: self.__doZignage_b() self.robotMover.doSnakeMovement(self.signalPosition, 270) interpretedSignal = self.signalSearcher.doSimpleSignalDecoding() print "interpreted signal: " + str(interpretedSignal) figureId = interpretedSignal[0] orientation = interpretedSignal[1] scale = interpretedSignal[2] self.__doZignage() self.__goToProperImageForScanning(figureId) self.__doDrawing(orientation, scale) self.robotMover.doSnakeMovement(Terrain.AR_TAG_SOUTH_FACE, 90) self.__doZignage_a() self.flashLed() SendEvent.send(SendEnd()) self.firstTurnOver = True StateController.instance.endMainLoop() return def __doZignage(self): self.__doZignage_a() self.__doZignage_b() def __doZignage_a(self): self.robotMover.doSnakeMovement(Terrain.AR_TAG_SOUTH_FACE, 90) print "Doing zignage A..." self.captorsController.Zing_a() def __doZignage_b(self): print "Doing zignage B..." self.captorsController.Zing_b() Robot.setCurrentPose((Terrain.DRAWING_ZONE_CENTER[0], Terrain.DRAWING_ZONE_CENTER[1], 90)) def __acquireCurrentPose(self): poseAcquired = False pose = () while not poseAcquired: try: pose = self.cam.getCurrentPose() poseAcquired = True break except ValueError: print "rotating of 5 degrees..." self.robotMover.doRelativeRotation(5) Robot.setCurrentPose(pose) print "Current robot pose is: " + str(Robot.getCurrentPose()) def __goToProperImageForScanning(self, imageId): if imageId == ManchesterSignalInterpreter.FIGURE_0: print "going to figure 0" self.robotMover.doSnakeMovement(Terrain.FIGURE_0_FACE, 90) elif imageId == ManchesterSignalInterpreter.FIGURE_1: print "going to figure 1" self.robotMover.doSnakeMovement(Terrain.FIGURE_1_FACE, 100) elif imageId == ManchesterSignalInterpreter.FIGURE_2: print "going to figure 2" self.robotMover.doSnakeMovement(Terrain.FIGURE_2_FACE, 165) elif imageId == ManchesterSignalInterpreter.FIGURE_3: print "going to figure 3" self.robotMover.doSnakeMovement(Terrain.FIGURE_3_FACE, 180) elif imageId == ManchesterSignalInterpreter.FIGURE_4: print "going to figure 4" self.robotMover.doSnakeMovement(Terrain.FIGURE_4_FACE, 180) elif imageId == ManchesterSignalInterpreter.FIGURE_5: print "going to figure 5" self.robotMover.doSnakeMovement(Terrain.FIGURE_5_FACE, 195) elif imageId == ManchesterSignalInterpreter.FIGURE_6: print "going to figure 6" self.robotMover.doSnakeMovement(Terrain.FIGURE_6_FACE, 255) elif imageId == ManchesterSignalInterpreter.FIGURE_7: print "going to figure 7" self.robotMover.doSnakeMovement(Terrain.FIGURE_7_FACE, 270) def __doDrawing(self, orientation, scale): print "Extracting points with camera..." drawingCountoursFound = False drawingCountour = [] tryCount = 0 while not drawingCountoursFound: shuffleDistance = 3 try: drawingCountour = self.cam.getDrawingContour() drawingCountoursFound = True except ValueError: print "Failed to extract points from camera! Retrying... with count: " + str(tryCount) if not tryCount % 5: self.robotMover.relativeShuffle(shuffleDistance, -150) elif tryCount % 5 == 1: self.robotMover.relativeShuffle(shuffleDistance, 90) elif tryCount % 5 == 2: self.robotMover.relativeShuffle(shuffleDistance, -30) elif tryCount % 5 == 3: self.robotMover.doRelativeRotation(-10) else: self.robotMover.doRelativeRotation(20) tryCount += 1 points = drawingCountour[0] size = drawingCountour[1] print points print size pointsToDraw = self.imagePointsTransformer.transform(drawingCountour, orientation, scale) SendEvent.send(SendDessin(pointsToDraw)) movedPoints = PointsCloudOperations.move(pointsToDraw, 144.8, 25.5) print "going to first drawing point" self.robotMover.doSnakeMovement(movedPoints[0], 270) print "arrived at first drawing point!" prehensorController = PrehensorController() prehensorController.dropPrehensor() movedPoints.append(movedPoints[0]) # this is to close the figure self.robotMover.doShuffleMovement(movedPoints, 270) prehensorController.raisePrehensor() def flashLed(self): self.ledController.turnLedOn() time.sleep(3) self.ledController.turnLedOff()