def __sendPoseToBase_shuffle(self, distanceInCentimeters):
currentPose = Robot.getCurrentPose()
deltaY = math.sin(math.radians(currentPose[2])) * distanceInCentimeters
deltaX = math.cos(math.radians(currentPose[2])) * distanceInCentimeters
Robot.setCurrentPose((currentPose[0] + deltaX, currentPose[1] - deltaY, currentPose[2]))
def relativeShuffle(self, distance, relativeAngle):
currentPose = Robot.getCurrentPose()
currentAngle = currentPose[2]
B = 360 - currentAngle - relativeAngle
deltaX = distance * math.sin(math.radians(B))
deltaY = distance * math.cos(math.radians(B))
destination = (currentPose[0] + deltaX, currentPose[1] + deltaY, currentAngle)
self.doShuffleMovement([currentPose, destination], currentAngle)
def doSnakeMovement(self, destination, finalAbsoluteAngle):
path = self.__buildPath(destination)
moves = SnakeMovementPlanner().planMovement(Robot.getCurrentPose(), path, finalAbsoluteAngle)
print "snake moves:"
print moves
self.executeMoves(moves)
Robot.setCurrentPose((destination[0], destination[1], finalAbsoluteAngle))
def doShuffleMovement(self, path, finalAbsoluteAngle):
print "about to delete first node on shuffle path..."
print "node to delete is: " + str(path[0])
del path[0] # path[0] is the current robot's pose
print "planning shuffle movement..."
moves = ShuffleMovementPlanner().planMovement(Robot.getCurrentPose(), path)
print "about to execute shuffle moves..."
print moves
self.executeMoves(moves)
lastNode = path[len(path) - 1]
Robot.setCurrentPose((lastNode[0], lastNode[1], finalAbsoluteAngle))
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 doRelativeRotation(self, relativeAngleInDegrees):
currentPose = Robot.getCurrentPose()
self.positionController.rotate(relativeAngleInDegrees)
Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + relativeAngleInDegrees))
def __sendPoseToBase_rotate(self, angleInDegrees):
currentPose = Robot.getCurrentPose()
Robot.setCurrentPose((currentPose[0], currentPose[1], currentPose[2] + angleInDegrees))
def __adjustPosition(self, distance, verticalDrift):
ancientPose = Robot.getCurrentPose()
newPose = (ancientPose[0] - (int(distance)/10), ancientPose[1] + verticalDrift, ancientPose[2])
Robot.setCurrentPose(newPose)