def planMovement(self, currentPose, nextNodes, finalAbsoluteAngle):
currentX = currentPose[0]
currentY = currentPose[1]
currentTheta = currentPose[2]
moves = []
for nextNode in nextNodes:
currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1]))
rotationAngle = Vector.angleBetween(currentPoseVector, destinationVector)
if rotationAngle:
moves.append(Rotate(rotationAngle))
distanceToAdvance = Vector.length(destinationVector)
if distanceToAdvance:
moves.append(Advance(distanceToAdvance))
currentX = nextNode[0]
currentY = nextNode[1]
currentTheta += rotationAngle
currentRobotVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
finalRobotVector = Vector.buildUnitaryVectorFromAngle(finalAbsoluteAngle)
rotationAngle = Vector.angleBetween(currentRobotVector, finalRobotVector)
if math.fabs(rotationAngle) > 0.01:
moves.append(Rotate(rotationAngle))
return moves
def planMovement(self, currentPose, nextNodes):
currentX = currentPose[0]
currentY = currentPose[1]
currentTheta = currentPose[2]
currentPoseVector = Vector.buildUnitaryVectorFromAngle(currentTheta)
moves = []
print "current pose in shuffle: " + str(currentPose)
print "path to be planned in shuffle: " + str(nextNodes)
for nextNode in nextNodes:
destinationVector = Vector.buildFromTwoPoints((currentX, currentY), (nextNode[0], nextNode[1]))
shuffleDistance = Vector.length(destinationVector)
shuffleAngle = Vector.angleBetween(currentPoseVector, destinationVector)
if shuffleAngle < 0:
shuffleAngle += 360
moves.append(Shuffle(shuffleDistance, shuffleAngle))
currentX = nextNode[0]
currentY = nextNode[1]
return moves
def test_buildUnitaryVectorFromAngle_210(self):
vector = Vector.buildUnitaryVectorFromAngle(-210)
self.assertAlmostEqual(-math.sqrt(3)/2, vector[0], delta=0.000001)
self.assertAlmostEqual(-0.5, vector[1], delta=0.0000001)
def test_buildUnitaryVectorFromAngle_ninety(self):
vector = Vector.buildUnitaryVectorFromAngle(90)
self.assertAlmostEqual(0, vector[0], delta=0.000001)
self.assertEqual(-1, vector[1])
def test_buildUnitaryVectorFromAngle_zero(self):
vector = Vector.buildUnitaryVectorFromAngle(0)
self.assertEqual([1, 0], vector)
