def setUp(self):
super(TestNavigator, self).setUp()
self.current_position = Position(53,-2,5,5)
self.mock_gps = Mock(position=self.current_position,speed=1)
self.globe = Globe()
self.mock_logger = Mock()
self.config = {'min time to steer' : MIN_TIME_TO_STEER, 'max time to steer' : MAX_TIME_TO_STEER}
def test_should_calculate_new_position_from_bearing_and_distance(self):
globe = Globe()
chorlton = globe.new_position(Manchester,200.1,4565)
new_york = globe.new_position(London,288.3,5570000)
self.assertLess(percentage_diff(chorlton.latitude, Chorlton.latitude),1)
self.assertLess(percentage_diff(chorlton.longitude, Chorlton.longitude),1)
self.assertLess(percentage_diff(new_york.longitude, NewYork.longitude),1)
self.assertLess(percentage_diff(new_york.latitude, NewYork.latitude),1)
def test_should_calculate_new_position_from_bearing_and_distance(self):
globe = Globe()
chorlton = globe.new_position(Manchester, 200.1, 4565)
new_york = globe.new_position(London, 288.3, 5570000)
self.assertLess(percentage_diff(chorlton.latitude, Chorlton.latitude),
1)
self.assertLess(
percentage_diff(chorlton.longitude, Chorlton.longitude), 1)
self.assertLess(percentage_diff(new_york.longitude, NewYork.longitude),
1)
self.assertLess(percentage_diff(new_york.latitude, NewYork.latitude),
1)
def test_should_navigate_to_next_waypoint_with_kink_in_route(self):
destination = Waypoint(Position(10.03, 10.03), 10)
gps = FakeMovingGPS([
Position(10, 10),
Position(10.01, 10.01),
Position(10.025, 10.015),
Position(10.03, 10.03)
])
sensors = FakeSensors(gps, 1, 45)
steerer = Steerer(self.servo, self.logger, CONFIG['steerer'])
helm = Helm(self.exchange, sensors, steerer, self.logger,
CONFIG['helm'])
navigator = Navigator(sensors, Globe(), self.exchange, self.logger,
CONFIG['navigator'])
self.exchange.publish(Event(EventName.navigate, waypoint=destination))
self.ticks(number=14, duration=200)
self.logger.info.assert_has_calls([
call(
'Navigator, steering to +10.030000,+10.030000, bearing 44.6, distance 4681.8m, review after 600s'
),
call(
'Navigator, steering to +10.030000,+10.030000, bearing 44.6, distance 3121.2m, review after 600s'
),
call(
'Navigator, steering to +10.030000,+10.030000, bearing 71.3, distance 1734.0m, review after 600s'
),
call('Navigator, arrived at +10.030000,+10.030000')
])
def test_should_steer_repeatedly_during_navigation(self):
logger = Mock()
destination = Waypoint(Position(10.0003, 10.0003), 10)
gps = FakeMovingGPS([
Position(10, 10),
Position(10.0001, 10.00015),
Position(10.00025, 10.0002),
Position(10.0003, 10.0003)
])
sensors = FakeSensors(gps, 1, 45)
steerer = Steerer(self.servo, logger, CONFIG['steerer'])
helm = Helm(self.exchange, sensors, steerer, logger, CONFIG['helm'])
navigator = Navigator(sensors, Globe(), self.exchange, logger,
CONFIG['navigator'])
self.exchange.publish(Event(EventName.navigate, waypoint=destination))
self.ticks(number=7, duration=20)
logger.debug.assert_has_calls([
call(
'Navigator, distance from waypoint +46.819018, combined tolerance +10.000000'
),
call(
'Navigator, distance from waypoint +27.647432, combined tolerance +10.000000'
),
call(
'Steerer, steering 36.4, heading 45.0, rate of turn +1.0, rudder +0.0, new rudder +4.6'
),
call(
'Steerer, steering 36.4, heading 45.0, rate of turn +1.0, rudder +4.6, new rudder +9.2'
),
call(
'Navigator, distance from waypoint +12.281099, combined tolerance +10.000000'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder +9.2, new rudder +0.4'
),
call(
'Navigator, distance from waypoint +0.000000, combined tolerance +10.000000'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder +0.4, new rudder -8.3'
),
call(
'Steerer, steering 63.1, heading 45.0, rate of turn +1.0, rudder -8.3, new rudder -17.1'
)
])
logger.info.assert_has_calls([
call(
'Navigator, steering to +10.000300,+10.000300, bearing 44.6, distance 46.8m, review after 23s'
),
call(
'Navigator, steering to +10.000300,+10.000300, bearing 36.4, distance 27.6m, review after 13s'
),
call(
'Navigator, steering to +10.000300,+10.000300, bearing 63.1, distance 12.3m, review after 6s'
),
call('Navigator, arrived at +10.000300,+10.000300')
])
def test_should_calculate_distance_to_within_one_tenth_percent(self):
globe = Globe()
self.assertLess(percentage_diff(4565,globe.distance_between(Manchester,Chorlton)),0.1)
self.assertLess(percentage_diff(262100,globe.distance_between(Manchester,London)),0.1)
self.assertLess(percentage_diff(5570000,globe.distance_between(London,NewYork)),0.1)
self.assertLess(percentage_diff(2543000,globe.distance_between(Manchester,Moscow)),0.1)
self.assertLess(percentage_diff(11010000,globe.distance_between(Sydney,Capetown)),0.1)
self.assertLess(percentage_diff(12560000,globe.distance_between(Capetown,NewYork)),0.1)
self.assertLess(percentage_diff(11680000,globe.distance_between(Santiago,Chorlton)),0.1)
def test_should_calculate_the_initial_compass_bearing_between_points(self):
globe = Globe()
self.assertLess(percentage_diff(200.1,globe.bearing(Manchester,Chorlton)),0.1)
self.assertLess(percentage_diff(145.9,globe.bearing(Manchester,London)),0.1)
self.assertLess(percentage_diff(288.3,globe.bearing(London,NewYork)),0.1)
self.assertLess(percentage_diff(64.3,globe.bearing(London,Moscow)),0.1)
self.assertLess(percentage_diff(357.3,globe.bearing(Santiago,NewYork)),0.1)
def test_should_calculate_the_initial_compass_bearing_between_points(self):
globe = Globe()
self.assertLess(
percentage_diff(200.1, globe.bearing(Manchester, Chorlton)), 0.1)
self.assertLess(
percentage_diff(145.9, globe.bearing(Manchester, London)), 0.1)
self.assertLess(percentage_diff(288.3, globe.bearing(London, NewYork)),
0.1)
self.assertLess(percentage_diff(64.3, globe.bearing(London, Moscow)),
0.1)
self.assertLess(
percentage_diff(357.3, globe.bearing(Santiago, NewYork)), 0.1)
def __init__(self, gps=False, servo_port=SERVO_PORT):
# devices
self._gps = gps
self.windsensor = WindSensor(I2C(WINDSENSOR_I2C_ADDRESS))
self.compass = Compass(I2C(COMPASS_I2C_ADDRESS),
I2C(ACCELEROMETER_I2C_ADDRESS))
self.red_led = GpioWriter(17, os)
self.green_led = GpioWriter(18, os)
# Navigation
self.globe = Globe()
self.timer = Timer()
self.application_logger = self._rotating_logger(APPLICATION_NAME)
self.position_logger = self._rotating_logger("position")
self.exchange = Exchange(self.application_logger)
self.timeshift = TimeShift(self.exchange, self.timer.time)
self.event_source = EventSource(self.exchange, self.timer,
self.application_logger,
CONFIG['event source'])
self.sensors = Sensors(self.gps, self.windsensor, self.compass,
self.timer.time, self.exchange,
self.position_logger, CONFIG['sensors'])
self.gps_console_writer = GpsConsoleWriter(self.gps)
self.rudder_servo = Servo(serial.Serial(servo_port),
RUDDER_SERVO_CHANNEL, RUDDER_MIN_PULSE,
RUDDER_MIN_ANGLE, RUDDER_MAX_PULSE,
RUDDER_MAX_ANGLE)
self.steerer = Steerer(self.rudder_servo, self.application_logger,
CONFIG['steerer'])
self.helm = Helm(self.exchange, self.sensors, self.steerer,
self.application_logger, CONFIG['helm'])
self.course_steerer = CourseSteerer(self.sensors, self.helm,
self.timer,
CONFIG['course steerer'])
self.navigator = Navigator(self.sensors, self.globe, self.exchange,
self.application_logger,
CONFIG['navigator'])
self.self_test = SelfTest(self.red_led, self.green_led, self.timer,
self.rudder_servo, RUDDER_MIN_ANGLE,
RUDDER_MAX_ANGLE)
# Tracking
self.tracking_logger = self._rotating_logger("track")
self.tracking_sensors = Sensors(self.gps, self.windsensor,
self.compass, self.timer.time,
self.exchange, self.tracking_logger,
CONFIG['sensors'])
self.tracker = Tracker(self.tracking_logger, self.tracking_sensors,
self.timer)
def __init__(self):
self.globe = Globe()
self.console_logger = self._console_logger()
self.exchange = Exchange(self.console_logger)
self.gps = SimulatedGPS(CHORLTON.position,0,0.1)
self.vehicle = SimulatedVehicle(self.gps, self.globe,self.console_logger,single_step=False)
self.timeshift = TimeShift(self.exchange,self.vehicle.timer.time)
self.event_source = EventSource(self.exchange,self.vehicle.timer,self.console_logger,CONFIG['event source'])
self.sensors = Sensors(self.vehicle.gps, self.vehicle.windsensor,self.vehicle.compass,self.vehicle.timer.time,self.exchange,self.console_logger,CONFIG['sensors'])
self.steerer = Steerer(self.vehicle.rudder,self.console_logger,CONFIG['steerer'])
self.helm = Helm(self.exchange, self.sensors, self.steerer, self.console_logger, CONFIG['helm'])
self.course_steerer = CourseSteerer(self.sensors,self.helm,self.vehicle.timer, CONFIG['course steerer'])
self.navigator_simulator = Navigator(self.sensors,self.globe,self.exchange,self.console_logger,CONFIG['navigator'])
self.tracking_timer = Timer()
self.tracker_simulator = Tracker(self.console_logger,self.sensors,self.tracking_timer)
def test_should_calculate_distance_to_within_one_tenth_percent(self):
globe = Globe()
self.assertLess(
percentage_diff(4565, globe.distance_between(Manchester,
Chorlton)), 0.1)
self.assertLess(
percentage_diff(262100, globe.distance_between(Manchester,
London)), 0.1)
self.assertLess(
percentage_diff(5570000, globe.distance_between(London, NewYork)),
0.1)
self.assertLess(
percentage_diff(2543000,
globe.distance_between(Manchester, Moscow)), 0.1)
self.assertLess(
percentage_diff(11010000, globe.distance_between(Sydney,
Capetown)), 0.1)
self.assertLess(
percentage_diff(12560000,
globe.distance_between(Capetown, NewYork)), 0.1)
self.assertLess(
percentage_diff(11680000,
globe.distance_between(Santiago, Chorlton)), 0.1)
def setUp(self):
self.mock_logger = Mock()
self.mock_logger.error = Mock(side_effect=print_msg)
self.exchange = Exchange(self.mock_logger)
self.timer = StubTimer()
self.timeshift = TimeShift(self.exchange, self.timer.time)
self.event_source = EventSource(self.exchange, self.timer,
self.mock_logger,
{'tick interval': 0.2})
gps = FakeMovingGPS([
Position(10, 10),
Position(11, 11),
Position(12, 12),
Position(13, 13)
])
self.navigator = Navigator(gps, Globe(), self.exchange,
self.mock_logger, {
'min time to steer': 5,
'max time to steer': 20
})
def setUp(self):
self.start_position = Position(53,-2)
self.globe = Globe()
self.reliable_gps = SimulatedGPS(self.start_position,0,0.5,True)
self.mock_logger = Mock()
def __init__(self,positions,globe = Globe()):
self.positions = positions
self.globe = globe
self.position_index = 0
self.wind_direction = 0
def create_globe(self):
print("Creating Map")
self.globe = Globe()
def setUp(self):
self.start_position = Position(53, -2)
self.globe = Globe()
self.reliable_gps = SimulatedGPS(self.start_position, 0, 0.5, True)
self.mock_logger = Mock()
class TestSimulatedVehicle(unittest.TestCase):
def setUp(self):
self.start_position = Position(53, -2)
self.globe = Globe()
self.reliable_gps = SimulatedGPS(self.start_position, 0, 0.5, True)
self.mock_logger = Mock()
def test_should_set_the_gps_speed_and_wind_direction_immediately(self):
vehicle = SimulatedVehicle(self.reliable_gps, self.globe,
self.mock_logger)
self.assertEqual(vehicle.speed, vehicle.gps.speed)
self.assertEqual(INITIAL_WIND_DIRECTION, vehicle.windsensor.angle)
def test_should_calculate_new_position_track_and_wind_direction_going_straight_ahead(
self):
bearing, time_s, wind_direction = 30, 2, 15
starting_position = Position(53, -2)
gps = SimulatedGPS(starting_position, bearing, INITIAL_SPEED_IN_MS,
True)
vehicle = SimulatedVehicle(gps, self.globe, self.mock_logger)
expected_position = self.globe.new_position(
starting_position, bearing, INITIAL_SPEED_IN_MS * time_s)
vehicle.rudder.set_position(0)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(bearing, vehicle.gps.track)
self.assertEqual(bearing, vehicle.compass.bearing)
self.assertEqual(wind_direction, vehicle.windsensor.angle)
def test_should_turn_a_corner_if_the_rudder_is_deflected(self):
bearing, time_s = 30, 2
starting_position = Position(53, -2)
rudder_deflection = 10
gps = SimulatedGPS(starting_position, bearing, INITIAL_SPEED_IN_MS,
True)
vehicle = SimulatedVehicle(gps, self.globe, self.mock_logger)
turn_radius = vehicle._turn_radius(rudder_deflection)
bearing_change = -vehicle._track_delta(INITIAL_SPEED_IN_MS * time_s,
turn_radius)
expected_track = to_360(bearing + bearing_change)
expected_position = self.globe.new_position(
starting_position, bearing + (0.5 * bearing_change),
vehicle._straightline_distance(turn_radius, bearing_change))
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(round(expected_track, 5), round(vehicle.gps.track, 5))
self.assertEqual(round(expected_track, 5),
round(vehicle.compass.bearing, 5))
def test_should_turn_right_if_rudder_deflection_is_negative(self):
bearing, time_s = 30, 2
starting_position = Position(53, -2)
rudder_deflection = -10
gps = SimulatedGPS(starting_position, bearing, INITIAL_SPEED_IN_MS,
True)
vehicle = SimulatedVehicle(gps, self.globe, self.mock_logger)
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
self.assertGreater(vehicle.gps.track, bearing)
def test_straightline_angle_is_half_turn_angle(self):
vehicle = SimulatedVehicle(self.reliable_gps, self.globe,
self.mock_logger)
self.assertEqual(vehicle._straightline_angle(47), 23.5)
def test_straightline_distance_using_radius_and_turn_angle(self):
vehicle = SimulatedVehicle(self.reliable_gps, self.globe,
self.mock_logger)
self.assertLess(
percentage_diff(vehicle._straightline_distance(1, 90), sqrt(2)),
0.0001)
self.assertLess(
percentage_diff(vehicle._straightline_distance(1, 180), 2), 0.0001)
self.assertLess(
percentage_diff(vehicle._straightline_distance(1, -180), 2),
0.0001)
self.assertLess(
percentage_diff(vehicle._straightline_distance(1, 60), 1), 0.0001)
def test_turn_radius_always_positive_and_inversely_proportional_to_rudder_angle(
self):
vehicle = SimulatedVehicle(self.reliable_gps, self.globe,
self.mock_logger)
self.assertEqual(vehicle._turn_radius(30), 0.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-30), 0.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-15), 1 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-10), 1.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(3), 5 + MIN_TURN_RADIUS)
def test_track_delta_based_on_distance_and_radius(self):
vehicle = SimulatedVehicle(self.reliable_gps, self.globe,
self.mock_logger)
# arc of length 1 radius subtends 1 radian
self.assertEqual(vehicle._track_delta(2, 2), degrees(1))
self.assertEqual(vehicle._track_delta(1, 2), degrees(0.5))
self.assertEqual(vehicle._track_delta(3, 2), degrees(1.5))
def time_to_destination(position, destination, speed):
globe = Globe()
distance = globe.distance_between(position, destination)
return distance /speed
class TestNavigator(EventTestCase):
def new_navigator(self,gps):
return Navigator(gps,self.globe, self.exchange, self.mock_logger, self.config)
def setUp(self):
super(TestNavigator, self).setUp()
self.current_position = Position(53,-2,5,5)
self.mock_gps = Mock(position=self.current_position,speed=1)
self.globe = Globe()
self.mock_logger = Mock()
self.config = {'min time to steer' : MIN_TIME_TO_STEER, 'max time to steer' : MAX_TIME_TO_STEER}
def test_should_not_steer_and_log_arrival_if_arrived(self):
self.listen(EventName.arrived)
self.listen(EventName.set_course)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,Waypoint(self.current_position,0)))
self.assertEqual(self.event_count(EventName.set_course),0,"expected no event to set course if we have arrived")
self.assertEqual(self.event_count(EventName.arrived),1,"expected arrived event if we have arrived")
self.mock_logger.info.assert_called_with('Navigator, arrived at {:+f},{:+f}'.format(self.current_position.latitude,self.current_position.longitude))
def test_should_allow_a_tolerance_and_consider_errors_when_calculating_if_we_have_reached_waypoint(self):
self.listen(EventName.arrived)
self.listen(EventName.set_course)
waypoint = Waypoint(Position(53.0001,-1.9999),10)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.arrived),1,"expected arrived event if we have arrived")
self.mock_logger.info.assert_called_with('Navigator, arrived at {:+f},{:+f}'.format(waypoint.latitude,waypoint.longitude))
def test_should_steer_along_the_bearing_to_the_next_waypoint(self):
self.listen(EventName.set_course)
waypoint = Waypoint(Position(53.5,-1.5),0)
expected_distance = self.globe.distance_between(self.current_position,waypoint.position)
expected_bearing = self.globe.bearing(self.current_position,waypoint.position)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.set_course),1,"expected set course event following navigate")
self.assertEqual(self.last_event.heading,expected_bearing)
# self.mock_logger.info.assert_any_call('Navigator, steering to {:+f},{:+f}, bearing {:5.1f}, distance {:.1f}m, review after 600s'
# .format(waypoint.latitude,waypoint.longitude,expected_bearing,expected_distance))
self.mock_logger.info.assert_has_calls([call('Navigator, steering to {:+f},{:+f}, bearing {:5.1f}, distance {:.1f}m, review after 32394s'
.format(waypoint.latitude,waypoint.longitude,expected_bearing,expected_distance))])
def test_should_steer_to_waypoint_if_outside_tolerance(self):
self.listen(EventName.set_course)
waypoint = Waypoint(Position(53.0001,-1.9999),5)
expected_bearing = self.globe.bearing(self.current_position,waypoint.position)
navigator = self.new_navigator(self.mock_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.set_course),1,"expected set course event following navigate")
self.assertEqual(self.last_event.heading,expected_bearing)
def test_at_intermediate_point_should_adjust_heading(self):
self.listen(EventName.set_course)
waypoint = Waypoint(Position(11,11),0)
intermediate_position = Position(12,12)
fake_gps = FakeMovingGPS([self.current_position, intermediate_position, waypoint.position])
bearing1 = self.globe.bearing(self.current_position,waypoint.position)
bearing2 = self.globe.bearing(intermediate_position,waypoint.position)
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.heading,bearing1)
self.exchange.publish(Event(EventName.navigate_review))
self.assertEqual(self.event_count(EventName.set_course),2,"expected 2 set course events")
self.assertEqual(self.last_event.heading,bearing2)
def test_should_not_fire_a_steer_event_if_no_GPS_signal(self):
self.listen(EventName.set_course)
waypoint = Waypoint(Position(-60,22),0)
no_position = Position(NaN,NaN,NaN,NaN)
first_bearing = self.globe.bearing(self.current_position,waypoint.position)
fake_gps = FakeMovingGPS([self.current_position, no_position, waypoint.position])
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.exchange.publish(Event(EventName.navigate_review))
steer_events = self.event_count(EventName.set_course)
self.assertEqual(steer_events,1,"expected only 1 set course event, got {0}".format(steer_events))
self.assertEqual(self.last_event.heading,first_bearing)
def test_should_signal_review_after_half_way_to_way_point_based_on_speed(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(53.0001,-1.999699),5) #23m from current position
bearing = self.globe.bearing(self.current_position,waypoint.position)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
for_expected_seconds = int(0.5 * time_to_destination(self.current_position,waypoint.position,fake_gps.speed))
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.event_count(EventName.after),1,"expected 1 'after' event")
self.assertEqual(self.last_event.name,EventName.after)
self.assertEqual(self.last_event.seconds,for_expected_seconds)
self.assertEqual(self.last_event.next_event.name,EventName.navigate_review)
def test_should_return_minimum_review_time_if_time_calculation_results_in_NaN(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(-60,22),0)
no_position = Position(NaN,NaN,NaN,NaN)
fake_gps = FakeMovingGPS([self.current_position, no_position, waypoint.position])
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,MAX_TIME_TO_STEER)
self.exchange.publish(Event(EventName.navigate_review,waypoint))
self.assertEqual(self.event_count(EventName.after),2,"expected 1 'after' events")
self.assertEqual(self.last_event.seconds,MIN_TIME_TO_STEER)
def test_should_use_minimum_steer_time_if_time_calculation_returns_small_value(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(53.0001,-1.9999),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 100
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,MIN_TIME_TO_STEER)
def test_should_use_maximum_steer_time_if_its_a_long_way_to_go(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(60.0001,10),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 1
expected_bearing = self.globe.bearing(self.current_position,waypoint.position)
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,MAX_TIME_TO_STEER)
def test_should_use_a_minimum_speed_for_calculation_preventing_divide_by_zero_error(self):
self.listen(EventName.after)
waypoint = Waypoint(Position(53.001,-2.001),5)
fake_gps = FakeMovingGPS([self.current_position, waypoint.position])
fake_gps.speed = 0
expected_time = expected_time_to_steer(self.current_position,waypoint.position,0.01)
navigator = self.new_navigator(fake_gps)
self.exchange.publish(Event(EventName.navigate,waypoint))
self.assertEqual(self.last_event.seconds,expected_time)
def test_should_return_false_for_arrived_if_current_position_is_NaN(self):
navigator = self.new_navigator(Mock())
destination = Waypoint(Position(53.001,-2.001),5)
nan_position = Position(NaN,NaN,NaN,NaN)
self.assertFalse(navigator._arrived(nan_position, destination))
class TestSimulatedVehicle(unittest.TestCase):
def setUp(self):
self.start_position = Position(53,-2)
self.globe = Globe()
self.reliable_gps = SimulatedGPS(self.start_position,0,0.5,True)
self.mock_logger = Mock()
def test_should_set_the_gps_speed_and_wind_direction_immediately(self):
vehicle = SimulatedVehicle(self.reliable_gps,self.globe,self.mock_logger)
self.assertEqual(vehicle.speed,vehicle.gps.speed)
self.assertEqual(INITIAL_WIND_DIRECTION,vehicle.windsensor.angle)
def test_should_calculate_new_position_track_and_wind_direction_going_straight_ahead(self):
bearing,time_s,wind_direction = 30,2,15
starting_position = Position(53,-2)
gps = SimulatedGPS(starting_position,bearing,INITIAL_SPEED_IN_MS,True)
vehicle = SimulatedVehicle(gps,self.globe,self.mock_logger)
expected_position = self.globe.new_position(starting_position,bearing,INITIAL_SPEED_IN_MS * time_s)
vehicle.rudder.set_position(0)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(bearing, vehicle.gps.track)
self.assertEqual(bearing, vehicle.compass.bearing)
self.assertEqual(wind_direction, vehicle.windsensor.angle)
def test_should_turn_a_corner_if_the_rudder_is_deflected(self):
bearing,time_s = 30,2
starting_position = Position(53,-2)
rudder_deflection = 10
gps = SimulatedGPS(starting_position,bearing,INITIAL_SPEED_IN_MS,True)
vehicle = SimulatedVehicle(gps,self.globe,self.mock_logger)
turn_radius = vehicle._turn_radius(rudder_deflection)
bearing_change = - vehicle._track_delta(INITIAL_SPEED_IN_MS*time_s,turn_radius)
expected_track = to_360(bearing + bearing_change)
expected_position = self.globe.new_position(starting_position,bearing + (0.5 * bearing_change),vehicle._straightline_distance(turn_radius,bearing_change))
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(round(expected_track,5),round(vehicle.gps.track,5))
self.assertEqual(round(expected_track,5), round(vehicle.compass.bearing,5))
def test_should_turn_right_if_rudder_deflection_is_negative(self):
bearing,time_s = 30,2
starting_position = Position(53,-2)
rudder_deflection = -10
gps = SimulatedGPS(starting_position,bearing,INITIAL_SPEED_IN_MS,True)
vehicle = SimulatedVehicle(gps,self.globe,self.mock_logger)
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
self.assertGreater(vehicle.gps.track,bearing)
def test_straightline_angle_is_half_turn_angle(self):
vehicle = SimulatedVehicle(self.reliable_gps,self.globe,self.mock_logger)
self.assertEqual(vehicle._straightline_angle(47),23.5)
def test_straightline_distance_using_radius_and_turn_angle(self):
vehicle = SimulatedVehicle(self.reliable_gps,self.globe,self.mock_logger)
self.assertLess(percentage_diff(vehicle._straightline_distance(1,90),sqrt(2)),0.0001)
self.assertLess(percentage_diff(vehicle._straightline_distance(1,180),2),0.0001)
self.assertLess(percentage_diff(vehicle._straightline_distance(1,-180),2),0.0001)
self.assertLess(percentage_diff(vehicle._straightline_distance(1,60),1),0.0001)
def test_turn_radius_always_positive_and_inversely_proportional_to_rudder_angle(self):
vehicle = SimulatedVehicle(self.reliable_gps,self.globe,self.mock_logger)
self.assertEqual(vehicle._turn_radius(30),0.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-30),0.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-15),1 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(-10),1.5 + MIN_TURN_RADIUS)
self.assertEqual(vehicle._turn_radius(3),5 + MIN_TURN_RADIUS)
def test_track_delta_based_on_distance_and_radius(self):
vehicle = SimulatedVehicle(self.reliable_gps,self.globe,self.mock_logger)
# arc of length 1 radius subtends 1 radian
self.assertEqual(vehicle._track_delta(2,2),degrees(1))
self.assertEqual(vehicle._track_delta(1,2),degrees(0.5))
self.assertEqual(vehicle._track_delta(3,2),degrees(1.5))
