def setUp(self):
self.movementPlanner = SnakeMovementPlanner()
self.currentPose = (0, 0, 0)
from python.src.robot.pathplanning.robotmover import RobotMover from python.src.robot.pathplanning.snakemovementplanner import SnakeMovementPlanner from python.src.robot.pathplanning.pose import Pose currentPose = Pose(0, 0, 0) path = [(10, 0), (10, 10), (20, 10), (0, 0)] movementPlanner = SnakeMovementPlanner() moves = movementPlanner.planMovement(currentPose, path) robotMover = RobotMover() robotMover.executeMoves(moves)
class TestSnakeMovementPlanner(unittest.TestCase):
def setUp(self):
self.movementPlanner = SnakeMovementPlanner()
self.currentPose = (0, 0, 0)
def test_planMovement_basicAdvanceMovement(self):
path = [(1, 0)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 0)
self.assertEqual(1, len(moves))
self.assertEqual(1, moves[0].distanceInCentimeters)
def test_planMovement_twoStepAdvanceMovement(self):
path = [(1, 0), (2, 0)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 0)
self.assertEqual(2, len(moves))
self.assertEqual(1, moves[0].distanceInCentimeters)
self.assertEqual(1, moves[1].distanceInCentimeters)
def test_planMovement_simpleAngle(self):
path = [(0, 1)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 270)
self.assertEqual(2, len(moves))
self.assertEqual(-90, moves[0].angleInDegrees)
self.assertEqual(1, moves[1].distanceInCentimeters)
def test_planMovement_twoSimpleAngles(self):
path = [(0, 1), (1, 1)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 0)
self.assertEqual(4, len(moves))
self.assertEqual(-90, moves[0].angleInDegrees)
self.assertEqual(1, moves[1].distanceInCentimeters)
self.assertEqual(90, moves[2].angleInDegrees)
self.assertEqual(1, moves[3].distanceInCentimeters)
def test_planMovement_complexMovement_1(self):
self.currentPose = (9, 27, 180)
path = [(7, -8), (-8, 7), (-5, -3), (0, 0)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 360 - math.degrees(math.atan(3/5)))
delta = 0.0000001
self.assertEqual(8, len(moves))
self.assertAlmostEqual(-1*math.degrees(math.acos(2/math.sqrt(1229))), moves[0].angleInDegrees, delta=delta)
self.assertAlmostEqual(math.sqrt(1229), moves[1].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(math.degrees(math.acos(-33/math.sqrt(2458))), moves[2].angleInDegrees, delta=delta)
self.assertAlmostEqual(math.sqrt(450), moves[3].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(-1*math.degrees(math.acos(-13/math.sqrt(218))), moves[4].angleInDegrees, delta=delta)
self.assertAlmostEqual(math.sqrt(109), moves[5].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(-1*math.degrees(math.acos(-15/math.sqrt(3706))), moves[6].angleInDegrees, delta=delta)
self.assertAlmostEqual(math.sqrt(34), moves[7].distanceInCentimeters, delta=delta)
def test_planMovement_fieldTest_1(self):
path = [(10, 0), (10, 10), (20, 10), (0, 0)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 153.434949)
delta = 0.0000001
self.assertEqual(7, len(moves))
self.assertAlmostEqual(10, moves[0].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(-90, moves[1].angleInDegrees, delta=delta)
self.assertAlmostEqual(10, moves[2].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(90, moves[3].angleInDegrees, delta=delta)
self.assertAlmostEqual(10, moves[4].distanceInCentimeters, delta=delta)
def test_planMovement_hello(self):
self.currentPose = (Terrain.FIGURE_6_FACE[0], Terrain.FIGURE_6_FACE[1], 180)
path = [(10, 0), (10, 10), (20, 10), (0, 0)]
moves = self.movementPlanner.planMovement(self.currentPose, path, 153.434949)
def test_planMovement_fieldTest_2(self):
self.currentPose = (180.1, Terrain.DRAWING_ZONE_NORTH_EAST_CORNER_OUTER[1], 180)
path = [(Terrain.DRAWING_ZONE_SOUTH_EAST_CORNER_OUTER[0], Terrain.DRAWING_ZONE_SOUTH_EAST_CORNER_OUTER[1])]
moves = self.movementPlanner.planMovement(self.currentPose, path, 0)
delta = 0.0000001
self.assertEqual(7, len(moves))
self.assertAlmostEqual(10, moves[0].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(-90, moves[1].angleInDegrees, delta=delta)
self.assertAlmostEqual(10, moves[2].distanceInCentimeters, delta=delta)
self.assertAlmostEqual(90, moves[3].angleInDegrees, delta=delta)
self.assertAlmostEqual(10, moves[4].distanceInCentimeters, delta=delta)
