def test_is_expired(timeout_checks):
timeout_ms, first_check_s, second_check_s = timeout_checks
t = Timeout(timeout_ms)
time.sleep(first_check_s)
assert t.is_expired() is False
time.sleep(second_check_s)
assert t.is_expired()
class RomiUsonicEscape(Behaviour):
"""
Escape behaviour. Moves the robot back then turns.
"""
def __init__(self, priority):
super(RomiUsonicEscape, self).__init__(priority, 'usonic_escape')
self._state = State.WAITING_TO_START
self._timeout = Timeout()
self._clear_sensors()
def _clear_sensors(self):
self._f_set = 0.0
def _set_sensors(self, sensors):
self._clear_sensors()
for sensor in sensors:
if sensor.name == 'distance':
self._f_set = sensor.value
def _is_sensor_active(self):
return self._f_set < 0.2
def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
if not self._is_sensor_active():
return None
return MotionCommand(-0.2, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.2, 0)
elif self._state == State.TURNING:
return MotionCommand(0, 1.5)
return None
def winner(self, winner=False):
if not winner:
self._state = State.WAITING_TO_START
elif self._state == State.WAITING_TO_START:
self._timeout.start(1)
self._state = State.REVERSING
elif self._state == State.REVERSING and self._timeout.is_expired():
self._timeout.start(0.5)
self._state = State.TURNING
elif self._state == State.TURNING and self._timeout.is_expired():
self._state = State.WAITING_TO_START
class RomiImuEscape(Behaviour):
"""
Escape behaviour. Moves the robot back then turns.
"""
def __init__(self, priority):
super(RomiImuEscape, self).__init__(priority, 'imu_escape')
self._state = State.WAITING_TO_START
self._timeout = Timeout()
self._clear_sensors()
self._turn_right = False
def _clear_sensors(self):
self._pitch = 0.0
self._roll = 0.0
def _set_sensors(self, sensors):
self._clear_sensors()
for sensor in sensors:
if sensor.name == 'imu':
self._pitch = sensor.value[0]
self._roll = sensor.value[1]
def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
# Flat, do nothing
if abs(self._pitch) < 0.2 and abs(self._roll) < 0.2:
return None
# Not flat, use roll to set direction of turn.
# We're going to always head down hill if tilted
if self._roll < -0.2:
self._turn_right = False
elif self._roll > 0.2:
self._turn_right = True
return MotionCommand(-0.2, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.2, 0)
elif self._state == State.TURNING and self._turn_right:
return MotionCommand(0, -1.5)
elif self._state == State.TURNING and not self._turn_right:
return MotionCommand(0, 1.5)
return None
def winner(self, winner=False):
if not winner:
self._state = State.WAITING_TO_START
elif self._state == State.WAITING_TO_START:
self._timeout.start(1)
self._state = State.REVERSING
elif self._state == State.REVERSING and self._timeout.is_expired():
self._timeout.start(0.5)
self._state = State.TURNING
elif self._state == State.TURNING and self._timeout.is_expired():
self._state = State.WAITING_TO_START
class SmartCarEscape(Behaviour):
"""
Escape behaviour. Moves the robot back then turns.
"""
def __init__(self, priority):
super(SmartCarEscape, self).__init__(priority, 'escape')
self._state = State.WAITING_TO_START
self._timeout = Timeout()
self._clear_sensors()
self._turn_right = False
def _clear_sensors(self):
self._fl_set = False
self._fr_set = False
self._f_set = False
def _set_sensors(self, sensors):
self._clear_sensors()
for sensor in sensors:
if sensor.name == 'front_left_ir':
self._fl_set = sensor.value
elif sensor.name == 'front_right_ir':
self._fr_set = sensor.value
elif sensor.name == 'front_ir':
self._f_set = sensor.value
def _random_turn(self):
return random() > 0.5
def _is_sensor_active(self):
return self._fl_set or self._fr_set or self._f_set
def get_action(self, sensors=None):
self._set_sensors(sensors)
if self._state == State.WAITING_TO_START:
if not self._is_sensor_active():
return None
if self._fl_set:
self._turn_right = True
elif self._fr_set:
self._turn_right = False
elif self._f_set:
self._turn_right = self._random_turn()
return MotionCommand(-0.4, 0)
elif self._state == State.REVERSING:
return MotionCommand(-0.4, 0)
elif self._state == State.TURNING and self._turn_right:
return MotionCommand(0, -2.5)
elif self._state == State.TURNING and not self._turn_right:
return MotionCommand(0, 2.5)
return None
def winner(self, winner=False):
if not winner:
self._state = State.WAITING_TO_START
elif self._state == State.WAITING_TO_START:
self._timeout.start(1)
self._state = State.REVERSING
elif self._state == State.REVERSING and self._timeout.is_expired():
self._timeout.start(0.5)
self._state = State.TURNING
elif self._state == State.TURNING and self._timeout.is_expired():
self._state = State.WAITING_TO_START
