def test_process_spline_II(self):
"""Check that the vehicle only trusts the jerk coefficients to recreate
the spline (not trusting controllers knowledge of vehicle state)."""
spline = CubicSpline(
[50, 53.333333333333336, 73.333333333333343, 110.0, 190.0, 226.66666666666666, 246.66666666666666, 250, 250], # pos
[0, 5.0, 15.0, 20.0, 20.0, 15.0, 5.0, 0, 0], # vel
[0, 5, 5, 0, 0, -5, -5, 0, 0], # accel
[2.5, 0, -2.5, 0, -2.5, 0, 2.5, 0], # jerk
[0, 2.0, 4.0, 6.0, 10.0, 12.0, 14.0, 16.0, 100]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
# Zero out everything but jerk and times in the message
for poly in spline_msg.polys:
for i in range(1,4): # skip jerk
poly.coeffs[i] = 0
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 0)
v0.process_spline_msg(spline_msg)
# Test that the vehicle's spline matches the original
for q_orig, q_vehicle in zip(spline.q, v0._spline.q):
self.assertAlmostEqual(q_orig, q_vehicle, self.PLACES)
for v_orig, v_vehicle in zip(spline.v, v0._spline.v):
self.assertAlmostEqual(v_orig, v_vehicle, self.PLACES)
for a_orig, a_vehicle in zip(spline.a, v0._spline.a):
self.assertAlmostEqual(a_orig, a_vehicle, self.PLACES)
for coeffs_orig, coeffs_vehicle in zip(spline.coeffs, v0._spline.coeffs):
for x_orig, x_vehicle in zip(coeffs_orig, coeffs_vehicle):
self.assertAlmostEqual(x_orig, x_vehicle, self.PLACES)
def test_updating_trajectory(self):
"""Creates a vehicle, starts the sim with it travelling at a fixed velocity,
then changes the trajectory 11 seconds after the start. When the trajectory
is changed, the vehicle is straddling two track segments."""
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 5) # fixed velocity, 5 m/s
# stop at 50 meters on ts2
spline = CubicSpline(
[2, 6.2324427610773778, 20.693227678868798, 46.258768272424305, 46.67126664464751, 49.999999999978492], # pos
[5, 5.8065992674127145, 9.581117951456692, 5.3923182554918929, 4.9953989633679301, -9.5825569701446511e-12], # vel
[0, 2.0082321422244926, 2.0082321422244926, -4.9976989522066617, -4.9976989522066617, -1.8332002582610585e-12], # accel
[2.5, 0, -2.5, 0, 2.5], # jerk
[11, 11.803292856889797, 13.682815948210798, 16.48518838598326, 16.564608794439177, 18.56368837532111]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
controller = MockController()
controller.add_cmd(11, v0.process_spline_msg, spline_msg)
Sim.activate(controller, controller.run())
Sim.activate(v0, v0.ctrl_loop())
Sim.simulate(until=20)
self.assertAlmostEqual(v0.pos, 50, self.PLACES)
self.assertTrue(v0.loc is self.ts2)
self.assertAlmostEqual(v0.vel, 0, self.PLACES)
self.assertAlmostEqual(v0.accel, 0, self.PLACES)
self.assertAlmostEqual(v0.tail_pos, 45, self.PLACES)
self.assertTrue(v0.tail_loc is self.ts2)
self.assertTrue(len(self.ts0.vehicles) == 0)
self.assertTrue(len(self.ts1.vehicles) == 0)
self.assertTrue(len(self.ts2.vehicles) == 1)
def test_process_spline_I(self):
"""Check that a spline was transferred correctly to the vehicle."""
spline = CubicSpline(
[50, 53.333333333333336, 73.333333333333343, 110.0, 190.0, 226.66666666666666, 246.66666666666666, 250, 250], # pos
[0, 5.0, 15.0, 20.0, 20.0, 15.0, 5.0, 0, 0], # vel
[0, 5, 5, 0, 0, -5, -5, 0, 0], # accel
[2.5, 0, -2.5, 0, -2.5, 0, 2.5, 0], # jerk
[0, 2.0, 4.0, 6.0, 10.0, 12.0, 14.0, 16.0, 100]) # time
spline_msg = api.Spline()
spline.fill_spline_msg(spline_msg)
v0 = vehicle.BaseVehicle(0, self.ts0, 50, 0)
v0.process_spline_msg(spline_msg)
# Test that the vehicle's spline matches the original
for q_orig, q_vehicle in zip(spline.q, v0._spline.q):
self.assertAlmostEqual(q_orig, q_vehicle, self.PLACES)
for v_orig, v_vehicle in zip(spline.v, v0._spline.v):
self.assertAlmostEqual(v_orig, v_vehicle, self.PLACES)
for a_orig, a_vehicle in zip(spline.a, v0._spline.a):
self.assertAlmostEqual(a_orig, a_vehicle, self.PLACES)
for coeffs_orig, coeffs_vehicle in zip(spline.coeffs, v0._spline.coeffs):
for x_orig, x_vehicle in zip(coeffs_orig, coeffs_vehicle):
self.assertAlmostEqual(x_orig, x_vehicle, self.PLACES)