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)