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 __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 setUp(self):
super(TestHelmOscillation, self).setUp()
# Sensors
mock_angle = PropertyMock(return_value=3.0)
self.time = 0
self.gps = StubGPS()
self.windsensor = Mock()
self.compass = Mock()
self.compass.bearing = 0
type(self.windsensor).angle = mock_angle
self.logger = Mock()
self.sensors = Sensors(self.gps, self.windsensor, self.compass,
self.mock_time, self.exchange, self.logger,
SENSOR_CONFIG)
self.rudder_servo = RudderSimulator()
self.steerer = Steerer(self.rudder_servo, self.logger, STEERER_CONFIG)
self.helm = Helm(self.exchange, self.sensors, self.steerer,
self.logger, HELM_CONFIG)
self.helm.previous_heading = 0
def setUp(self):
self.servo = Mock()
self.logger = Mock()
self.steerer = Steerer(self.servo, self.logger, TEST_CONFIG)
class TestSteerer(unittest.TestCase):
def setUp(self):
self.servo = Mock()
self.logger = Mock()
self.steerer = Steerer(self.servo, self.logger, TEST_CONFIG)
def test_on_course_if_heading_and_rate_of_turn_below_threshold(self):
course = 277
self.assertTrue(
self.steerer.on_course(course, course + MAX_DEVIATION - 1,
MAX_RATE_OF_TURN - 1))
def test_not_on_course_if_only_heading_below_threshold(self):
course = 188
self.assertFalse(
self.steerer.on_course(course, course - MAX_DEVIATION + 1,
MAX_RATE_OF_TURN + 1))
def test_not_on_course_if_only_rate_of_turn_below_threshold(self):
course = 92
self.assertFalse(
self.steerer.on_course(course, course + MAX_DEVIATION + 1,
MAX_RATE_OF_TURN - 1))
def test_should_not_change_direction_if_within_five_degrees_of_right_course_and_rate_of_turn_less_that_five_degrees(
self):
self.steerer.steer(requested_heading=196, heading=200, rate_of_turn=4)
self.assertEqual(self.servo.set_position.call_count, 0)
def test_should_move_rudder_right_by_difference_between_heading_and_course_with_factor_if_no_rate_of_turn(
self):
self.steerer.steer(requested_heading=209, heading=200, rate_of_turn=0)
self.servo.set_position.assert_called_with(-9 * DEVIATION_FACTOR)
def test_should_move_rudder_right_by_difference_between_heading_and_course_with_factor_subtracting_rate_of_turn(
self):
self.steerer.steer(requested_heading=209, heading=200, rate_of_turn=50)
self.servo.set_position.assert_called_with(50 * 0.1 -
9 * DEVIATION_FACTOR)
def test_should_move_rudder_left_by_difference_between_heading_and_course_with_factor_subtracting_larger_rate_of_turn(
self):
self.steerer.steer(requested_heading=209,
heading=205,
rate_of_turn=150)
self.servo.set_position.assert_called_with(150 * 0.1 - (209 - 205) *
DEVIATION_FACTOR)
def test_should_move_rudder_left_by_difference_between_heading_and_course_subtracting_rate_of_turn(
self):
self.steerer.steer(requested_heading=355,
heading=10,
rate_of_turn=-100)
self.servo.set_position.assert_called_with(-100 * 0.1 -
(-15 * DEVIATION_FACTOR))
def test_rudder_movements_should_be_relative_to_current_rudder_position(
self):
self.steerer.rudder_angle = 5
self.steerer.steer(requested_heading=355,
heading=10,
rate_of_turn=-100)
self.servo.set_position.assert_called_with(5 + 15 * 0.5 - 100 * 0.1)
def test_rudder_movements_should_be_limited_to_full_deflection_left(self):
self.steerer.rudder_angle = 5
self.steerer.steer(requested_heading=330,
heading=50,
rate_of_turn=-100)
self.servo.set_position.assert_called_with(FULL_DEFLECTION)
def test_rudder_movements_should_be_limited_to_full_deflection_right(self):
self.steerer.rudder_angle = -20
self.steerer.steer(requested_heading=20, heading=355, rate_of_turn=00)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_steer_left_if_difference_is_less_than_180(self):
self.steerer.steer(requested_heading=45, heading=200, rate_of_turn=00)
self.servo.set_position.assert_called_with(FULL_DEFLECTION)
def test_should_steer_right_if_difference_is_less_than_180(self):
self.steerer.steer(requested_heading=10, heading=200, rate_of_turn=0)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_steer_right_if_difference_is_exactly_180(self):
self.steerer.steer(requested_heading=20, heading=200, rate_of_turn=0)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_work_with_fractional_parts(self):
self.steerer.steer(requested_heading=29.4, heading=0.9, rate_of_turn=0)
self.servo.set_position.assert_called_with(-28.5 * DEVIATION_FACTOR)
def test_should_centralise_rudder_if_sensor_returns_NaN(self):
self.steerer.steer(requested_heading=57.23,
heading=NaN,
rate_of_turn=0)
self.servo.set_position.assert_called_with(0)
def test_should_log_steering_calculation_and_status_to_debug(self):
self.steerer.steer(requested_heading=355,
heading=10,
rate_of_turn=-100)
self.logger.debug.assert_called_with(
"Steerer, steering 355.0, heading 10.0, rate of turn -100.0, rudder +0.0, new rudder -2.5"
)
self.servo.set_position.assert_called_with(-2.5)
def test_should_use_reduction_factor_for_deflection_to_allow_minor_long_term_corrections(
self):
self.steerer.steer(requested_heading=209,
heading=200,
rate_of_turn=50,
rudder_deflection_factor=0.1)
self.servo.set_position.assert_called_with(
0.1 * (50 * 0.1 - 9 * DEVIATION_FACTOR))
def setUp(self):
self.servo = Mock()
self.logger = Mock()
self.steerer = Steerer(self.servo,self.logger, TEST_CONFIG)
class TestSteerer(unittest.TestCase):
def setUp(self):
self.servo = Mock()
self.logger = Mock()
self.steerer = Steerer(self.servo,self.logger, TEST_CONFIG)
def test_on_course_if_heading_and_rate_of_turn_below_threshold(self):
course = 277
self.assertTrue(self.steerer.on_course(course,course + MAX_DEVIATION - 1,MAX_RATE_OF_TURN -1))
def test_not_on_course_if_only_heading_below_threshold(self):
course = 188
self.assertFalse(self.steerer.on_course(course,course - MAX_DEVIATION + 1,MAX_RATE_OF_TURN + 1))
def test_not_on_course_if_only_rate_of_turn_below_threshold(self):
course = 92
self.assertFalse(self.steerer.on_course(course,course + MAX_DEVIATION + 1,MAX_RATE_OF_TURN - 1))
def test_should_not_change_direction_if_within_five_degrees_of_right_course_and_rate_of_turn_less_that_five_degrees(self):
self.steerer.steer(requested_heading=196,heading=200,rate_of_turn=4)
self.assertEqual(self.servo.set_position.call_count,0)
def test_should_move_rudder_right_by_difference_between_heading_and_course_with_factor_if_no_rate_of_turn(self):
self.steerer.steer(requested_heading=209,heading=200,rate_of_turn=0)
self.servo.set_position.assert_called_with(-9 * DEVIATION_FACTOR)
def test_should_move_rudder_right_by_difference_between_heading_and_course_with_factor_subtracting_rate_of_turn(self):
self.steerer.steer(requested_heading=209,heading=200,rate_of_turn=50)
self.servo.set_position.assert_called_with(50 * 0.1 - 9 * DEVIATION_FACTOR)
def test_should_move_rudder_left_by_difference_between_heading_and_course_with_factor_subtracting_larger_rate_of_turn(self):
self.steerer.steer(requested_heading=209,heading=205,rate_of_turn=150)
self.servo.set_position.assert_called_with(150 * 0.1 - (209 - 205) * DEVIATION_FACTOR )
def test_should_move_rudder_left_by_difference_between_heading_and_course_subtracting_rate_of_turn(self):
self.steerer.steer(requested_heading=355,heading=10,rate_of_turn=-100)
self.servo.set_position.assert_called_with(-100 * 0.1 - (-15 * DEVIATION_FACTOR))
def test_rudder_movements_should_be_relative_to_current_rudder_position(self):
self.steerer.rudder_angle = 5
self.steerer.steer(requested_heading=355,heading=10,rate_of_turn=-100)
self.servo.set_position.assert_called_with(5 + 15*0.5 - 100*0.1)
def test_rudder_movements_should_be_limited_to_full_deflection_left(self):
self.steerer.rudder_angle = 5
self.steerer.steer(requested_heading=330,heading=50,rate_of_turn=-100)
self.servo.set_position.assert_called_with(FULL_DEFLECTION)
def test_rudder_movements_should_be_limited_to_full_deflection_right(self):
self.steerer.rudder_angle = -20
self.steerer.steer(requested_heading=20,heading=355,rate_of_turn=00)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_steer_left_if_difference_is_less_than_180(self):
self.steerer.steer(requested_heading=45,heading=200,rate_of_turn=00)
self.servo.set_position.assert_called_with(FULL_DEFLECTION)
def test_should_steer_right_if_difference_is_less_than_180(self):
self.steerer.steer(requested_heading=10,heading=200,rate_of_turn=0)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_steer_right_if_difference_is_exactly_180(self):
self.steerer.steer(requested_heading=20,heading=200,rate_of_turn=0)
self.servo.set_position.assert_called_with(-FULL_DEFLECTION)
def test_should_work_with_fractional_parts(self):
self.steerer.steer(requested_heading=29.4,heading=0.9,rate_of_turn=0)
self.servo.set_position.assert_called_with(-28.5 * DEVIATION_FACTOR)
def test_should_centralise_rudder_if_sensor_returns_NaN(self):
self.steerer.steer(requested_heading=57.23,heading=NaN,rate_of_turn=0)
self.servo.set_position.assert_called_with(0)
def test_should_log_steering_calculation_and_status_to_debug(self):
self.steerer.steer(requested_heading=355,heading=10,rate_of_turn=-100)
self.logger.debug.assert_called_with("Steerer, steering 355.0, heading 10.0, rate of turn -100.0, rudder +0.0, new rudder -2.5")
self.servo.set_position.assert_called_with(-2.5)
def test_should_use_reduction_factor_for_deflection_to_allow_minor_long_term_corrections(self):
self.steerer.steer(requested_heading=209,heading=200,rate_of_turn=50,rudder_deflection_factor = 0.1)
self.servo.set_position.assert_called_with(0.1 * (50 * 0.1 - 9 * DEVIATION_FACTOR))
