def test_Unknown(self):
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEqual(sVAccel, 0.0)
self.assertEqual(sVDecel, 0.0)
self.assertEqual(sWAccel, 0.0)
self.assertEqual(sWDecel, 0.0)
def test_LINE_None(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg)
self.assertAlmostEqual(sVAccel, 0.25, delta=.01)
self.assertAlmostEqual(sVDecel, 0.75, delta=.01)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel, 1.0)
def test_LINE_None(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(self.currSeg)
self.assertAlmostEqual(sVAccel, 0.25, delta=.01)
self.assertAlmostEqual(sVDecel, 0.75, delta=.01)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_SPIN_None(self):
self.currSeg.seg_type = PathSegmentMsg.SPIN_IN_PLACE
self.currSeg.seg_length = pi/2.0
self.currSeg.max_speeds.linear.x = 0.0
self.currSeg.max_speeds.angular.z = .5
self.currSeg.curvature = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg)
self.assertEquals(sVDecel,1.0)
self.assertEquals(sVAccel,0.0)
self.assertAlmostEqual(sWAccel, 0.1592,delta=0.001)
self.assertAlmostEqual(sWDecel, 1-0.1592,delta=0.001)
def test_SPIN_None(self):
self.currSeg.seg_type = PathSegmentMsg.SPIN_IN_PLACE
self.currSeg.seg_length = pi / 2.0
self.currSeg.max_speeds.linear.x = 0.0
self.currSeg.max_speeds.angular.z = .5
self.currSeg.curvature = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(self.currSeg)
self.assertEquals(sVDecel, 1.0)
self.assertEquals(sVAccel, 0.0)
self.assertAlmostEqual(sWAccel, 0.1592, delta=0.001)
self.assertAlmostEqual(sWDecel, 1 - 0.1592, delta=0.001)
def test_posARC_Unknown(self):
self.currSeg.seg_type = PathSegmentMsg.ARC
self.currSeg.seg_length = pi/2
self.currSeg.curvature = 2.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.max_speeds.angular.z = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = 4
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
# these numbers may not be right
self.assertAlmostEqual(sVAccel, 0.25, delta = 0.0001)
self.assertAlmostEqual(sVDecel, 0.7500, delta = 0.0001)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_posARC_Unknown(self):
self.currSeg.seg_type = PathSegmentMsg.ARC
self.currSeg.seg_length = pi / 2
self.currSeg.curvature = 2.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.max_speeds.angular.z = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = 4
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
# these numbers may not be right
self.assertAlmostEqual(sVAccel, 0.25, delta=0.0001)
self.assertAlmostEqual(sVDecel, 0.7500, delta=0.0001)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_negLINE_LINE_LessThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = -2.0
self.currSeg.accel_limit = -0.5
self.currSeg.decel_limit = 0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 1.0
self.nextSeg.max_speeds.linear.x = -0.5
self.nextSeg.accel_limit = -0.7
self.nextSeg.decel_limit = 0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEquals(sVAccel,1.0)
self.assertEquals(sVDecel,0.4375)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_posLINE_LINE_GreaterThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 3.0
self.nextSeg.max_speeds.linear.x = 2.0
self.nextSeg.accel_limit = 0.5
self.nextSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEquals(sVDecel,1.0)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
self.assertTrue(sVDecel > sVAccel)
def test_negLINE_LINE_OppositeSign(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = -2.0
self.currSeg.accel_limit = -0.5
self.currSeg.decel_limit = 0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 1.0
self.nextSeg.max_speeds.linear.x = 1.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.666, delta = 0.001)
self.assertAlmostEqual(sVDecel, 0.333, delta = 0.001)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_negLINE_LINE_LessThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = -2.0
self.currSeg.accel_limit = -0.5
self.currSeg.decel_limit = 0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 1.0
self.nextSeg.max_speeds.linear.x = -0.5
self.nextSeg.accel_limit = -0.7
self.nextSeg.decel_limit = 0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEquals(sVAccel, 1.0)
self.assertEquals(sVDecel, 0.4375)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_negLINE_LINE_OppositeSign(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = -2.0
self.currSeg.accel_limit = -0.5
self.currSeg.decel_limit = 0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 1.0
self.nextSeg.max_speeds.linear.x = 1.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.666, delta=0.001)
self.assertAlmostEqual(sVDecel, 0.333, delta=0.001)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_posLINE_LINE_GreaterThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.LINE
self.nextSeg.seg_length = 3.0
self.nextSeg.max_speeds.linear.x = 2.0
self.nextSeg.accel_limit = 0.5
self.nextSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEquals(sVDecel, 1.0)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
self.assertTrue(sVDecel > sVAccel)
def test_LINE_Unknown(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = 4
self.nextSeg.seg_length = pi/2
self.nextSeg.curvature = 1.0
self.nextSeg.max_speeds.linear.x = 0.0
self.nextSeg.max_speeds.angular.z = 2.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.2500, delta = 0.0001)
self.assertAlmostEqual(sVDecel, 0.7500, delta = 0.0001)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_posLINE_ARC_OppositeSign(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.ARC
self.nextSeg.seg_length = 1.0
self.nextSeg.curvature = -1.0
self.nextSeg.max_speeds.linear.x = 2.0
self.nextSeg.max_speeds.angular.z = -2.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.1667, delta = 0.0001)
self.assertAlmostEqual(sVDecel, 0.8448, delta = 0.0001)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_posLINE_ARC_GreaterThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.ARC
self.nextSeg.seg_length = 1.0
self.nextSeg.curvature = 1.0
self.nextSeg.max_speeds.linear.x = 3.0
self.nextSeg.max_speeds.angular.z = 2.0
self.nextSeg.accel_limit = 100.0 # this number would never actually be this large
self.nextSeg.decel_limit = -0.5
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.1667, delta = 0.0001)
self.assertAlmostEqual(sVDecel, 1.0, delta = 0.0001)
self.assertEquals(sWAccel,0.0)
self.assertEquals(sWDecel,1.0)
def test_posLINE_ARC_GreaterThan(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.ARC
self.nextSeg.seg_length = 1.0
self.nextSeg.curvature = 1.0
self.nextSeg.max_speeds.linear.x = 3.0
self.nextSeg.max_speeds.angular.z = 2.0
self.nextSeg.accel_limit = 100.0 # this number would never actually be this large
self.nextSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.1667, delta=0.0001)
self.assertAlmostEqual(sVDecel, 1.0, delta=0.0001)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_posLINE_ARC_OppositeSign(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 6.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = PathSegmentMsg.ARC
self.nextSeg.seg_length = 1.0
self.nextSeg.curvature = -1.0
self.nextSeg.max_speeds.linear.x = 2.0
self.nextSeg.max_speeds.angular.z = -2.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.1667, delta=0.0001)
self.assertAlmostEqual(sVDecel, 0.8448, delta=0.0001)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_LINE_Unknown(self):
self.currSeg.seg_type = PathSegmentMsg.LINE
self.currSeg.seg_length = 4.0
self.currSeg.max_speeds.linear.x = 1.0
self.currSeg.accel_limit = 0.5
self.currSeg.decel_limit = -0.5
self.nextSeg.seg_type = 4
self.nextSeg.seg_length = pi / 2
self.nextSeg.curvature = 1.0
self.nextSeg.max_speeds.linear.x = 0.0
self.nextSeg.max_speeds.angular.z = 2.0
self.nextSeg.accel_limit = 0.7
self.nextSeg.decel_limit = -0.5
(sVAccel, sVDecel, sWAccel,
sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertAlmostEqual(sVAccel, 0.2500, delta=0.0001)
self.assertAlmostEqual(sVDecel, 0.7500, delta=0.0001)
self.assertEquals(sWAccel, 0.0)
self.assertEquals(sWDecel, 1.0)
def test_Unknown(self):
(sVAccel,sVDecel,sWAccel,sWDecel) = computeTrajectory(self.currSeg, self.nextSeg)
self.assertEqual(sVAccel, 0.0)
self.assertEqual(sVDecel, 0.0)
self.assertEqual(sWAccel, 0.0)
self.assertEqual(sWDecel, 0.0)
