class ReferentialConverterTest(TestCase):
DEFAULT_POINT = (100, 100)
DEFAULT_ORIENTATION = 90
ANOTHER_POINT = (150, 200)
ANOTHER_ORIENTATION = 75
def setUp(self):
self.referentialConverter = ReferentialConverter(
self.DEFAULT_POINT, self.DEFAULT_ORIENTATION)
def test_whenCreatingReferentialConverterThenClassAttributesAreSetCorrectly(
self):
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
defaultOrientation = float(self.DEFAULT_ORIENTATION % 360) / 180
self.assertEqual(self.DEFAULT_POINT.__getitem__(0), positionX)
self.assertEqual(self.DEFAULT_POINT.__getitem__(1), positionY)
self.assertEqual(defaultOrientation, orientation)
def test_whenSettingPositionThenPositionIsSetWithSaidData(self):
self.referentialConverter = ReferentialConverter(
self.ANOTHER_POINT, self.ANOTHER_ORIENTATION)
thePoint = self.ANOTHER_POINT
theOrientation = float(self.ANOTHER_ORIENTATION % 360) / 180
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
self.assertEqual(thePoint.__getitem__(0), positionX)
self.assertEqual(thePoint.__getitem__(1), positionY)
self.assertEqual(theOrientation, orientation)
def test_givenDefaultPointToBeCovertedWithDefaultOrientationWhenConverWorldToRobotThenPointIsConvertedCorrectly(
self):
pointConverted = self.referentialConverter.convertWorldToRobot(
self.DEFAULT_POINT)
ConvertedPointShouldBe = (0, 0)
self.assertEqual(ConvertedPointShouldBe.__getitem__(0),
pointConverted.__getitem__(0))
self.assertEqual(ConvertedPointShouldBe.__getitem__(1),
pointConverted.__getitem__(1))
def test_givenNotDefaultPointToBeCovertedWithDefaultOrientationWhenConverWorldToRobotThenPointIsConvertedCorrectly(
self):
pointConverted = self.referentialConverter.convertWorldToRobot(
self.ANOTHER_POINT)
ConvertedPointShouldBe = (100, -50)
self.assertEqual(ConvertedPointShouldBe.__getitem__(0),
pointConverted.__getitem__(0))
self.assertEqual(ConvertedPointShouldBe.__getitem__(1),
pointConverted.__getitem__(1))
def test_whenSettingPositionThenPositionIsSetWithSaidData(self):
self.referentialConverter = ReferentialConverter(
self.ANOTHER_POINT, self.ANOTHER_ORIENTATION)
thePoint = self.ANOTHER_POINT
theOrientation = float(self.ANOTHER_ORIENTATION % 360) / 180
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
self.assertEqual(thePoint.__getitem__(0), positionX)
self.assertEqual(thePoint.__getitem__(1), positionY)
self.assertEqual(theOrientation, orientation)
class ReferentialConverterTest(TestCase):
DEFAULT_POINT = (100,100)
DEFAULT_ORIENTATION = 90
ANOTHER_POINT = (150,200)
ANOTHER_ORIENTATION = 75
def setUp(self):
self.referentialConverter = ReferentialConverter(self.DEFAULT_POINT, self.DEFAULT_ORIENTATION)
def test_whenCreatingReferentialConverterThenClassAttributesAreSetCorrectly(self):
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
defaultOrientation = float(self.DEFAULT_ORIENTATION%360)/180
self.assertEqual(self.DEFAULT_POINT.__getitem__(0), positionX)
self.assertEqual(self.DEFAULT_POINT.__getitem__(1), positionY)
self.assertEqual(defaultOrientation, orientation)
def test_whenSettingPositionThenPositionIsSetWithSaidData(self):
self.referentialConverter = ReferentialConverter(self.ANOTHER_POINT, self.ANOTHER_ORIENTATION)
thePoint = self.ANOTHER_POINT
theOrientation = float(self.ANOTHER_ORIENTATION%360)/180
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
self.assertEqual(thePoint.__getitem__(0), positionX)
self.assertEqual(thePoint.__getitem__(1), positionY)
self.assertEqual(theOrientation, orientation)
def test_givenDefaultPointToBeCovertedWithDefaultOrientationWhenConverWorldToRobotThenPointIsConvertedCorrectly(self):
pointConverted = self.referentialConverter.convertWorldToRobot(self.DEFAULT_POINT)
ConvertedPointShouldBe = (0,0)
self.assertEqual(ConvertedPointShouldBe.__getitem__(0), pointConverted.__getitem__(0))
self.assertEqual(ConvertedPointShouldBe.__getitem__(1), pointConverted.__getitem__(1))
def test_givenNotDefaultPointToBeCovertedWithDefaultOrientationWhenConverWorldToRobotThenPointIsConvertedCorrectly(self):
pointConverted = self.referentialConverter.convertWorldToRobot(self.ANOTHER_POINT)
ConvertedPointShouldBe = (100,-50)
self.assertEqual(ConvertedPointShouldBe.__getitem__(0), pointConverted.__getitem__(0))
self.assertEqual(ConvertedPointShouldBe.__getitem__(1), pointConverted.__getitem__(1))
def __adjustOrientation(self, pointToMove):
degreeAngle = (np.arctan((pointToMove[1]/pointToMove[0]))/np.pi)*180
angleToRotate = degreeAngle%90
if angleToRotate > 45:
angleToRotate = angleToRotate - 90
print angleToRotate
point = (0,0)
referentialConverter = ReferentialConverter(point, angleToRotate)
print "adjusting angle", -angleToRotate
self.rotate(-angleToRotate)
pointAdjusted = referentialConverter.convertWorldToRobot(pointToMove)
print "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ BOBA FETT", pointAdjusted[0], pointAdjusted[1]
return pointAdjusted
def __adjustOrientation(self, pointToMove):
degreeAngle = (np.arctan(
(pointToMove[1] / pointToMove[0])) / np.pi) * 180
angleToRotate = degreeAngle % 90
if angleToRotate > 45:
angleToRotate = angleToRotate - 90
print angleToRotate
point = (0, 0)
referentialConverter = ReferentialConverter(point, angleToRotate)
print "adjusting angle", -angleToRotate
self.rotate(-angleToRotate)
pointAdjusted = referentialConverter.convertWorldToRobot(pointToMove)
print "@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@ BOBA FETT", pointAdjusted[
0], pointAdjusted[1]
return pointAdjusted
def test_whenSettingPositionThenPositionIsSetWithSaidData(self):
self.referentialConverter = ReferentialConverter(self.ANOTHER_POINT, self.ANOTHER_ORIENTATION)
thePoint = self.ANOTHER_POINT
theOrientation = float(self.ANOTHER_ORIENTATION%360)/180
positionX = self.referentialConverter.positionRobotInWorldX
positionY = self.referentialConverter.positionRobotInWorldY
orientation = self.referentialConverter.orientation
self.assertEqual(thePoint.__getitem__(0), positionX)
self.assertEqual(thePoint.__getitem__(1), positionY)
self.assertEqual(theOrientation, orientation)
def setUp(self):
self.referentialConverter = ReferentialConverter(self.DEFAULT_POINT, self.DEFAULT_ORIENTATION)
def setUp(self):
self.referentialConverter = ReferentialConverter(
self.DEFAULT_POINT, self.DEFAULT_ORIENTATION)
