def handle_message(self, message):
message.number = next(self._counter)
# show elapsed time
_delta = dt.datetime.now() - self._start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
# we want to limit the messages to 1/5ms, so if elapsed time is less than that we'll just pad it out
# set pad to -1 to disable feature
if _elapsed_ms < self._min_loop_ms:
time.sleep((self._min_loop_ms - _elapsed_ms) / 1000)
_delta = dt.datetime.now() - self._start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
# self._log.debug('elapsed since last message: {}ms (padded)'.format(_elapsed_ms))
self._log.debug(
Fore.MAGENTA +
'handling message #{}: priority {}: {};\telapsed: {}ms'.format(
message.number, message.priority, message.description,
_elapsed_ms) + " (padded)")
else:
# self._log.debug('elapsed since last message: {}ms'.format(_elapsed_ms))
self._log.debug(
Fore.MAGENTA +
'handling message #{}: priority {}: {};\telapsed: {}ms'.format(
message.number, message.priority, message.description,
_elapsed_ms) + "")
# ........................................
event = message.event
if event is Event.INFRARED_PORT_SIDE:
self._log.debug(Fore.RED + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_PORT:
self._log.debug(Fore.RED + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
# elif event is Event.INFRARED_CNTR:
# _cruising_velocity = self._behaviours.get_cruising_velocity()
# _distance = message.value
# _clamped_distance = Behaviours.clamp(_distance, 20.0, 100.0)
# _mapped_velocity = Behaviours.remap(_clamped_distance, 20.0, 100.0, 0.0, _cruising_velocity)
# self._log.info(Fore.BLUE + Style.NORMAL + 'event: {};'.format(event.description) + Fore.YELLOW + '\tmapped velocity: {:5.2f}cm'.format(message.value ))
# self._log.info(Fore.BLUE + Style.NORMAL + '_distance: {:5.2f}\t'.format(_distance) + Style.BRIGHT + '\tcruising velocity: {:5.2f}cm;'.format(_cruising_velocity) \
# + Fore.YELLOW + Style.NORMAL + '\tvelocity: {:5.2f}'.format(_mapped_velocity))
# pids = self._pid_motor_ctrl.set_max_velocity(_mapped_velocity)
elif event is Event.INFRARED_CNTR:
self._log.debug(Fore.BLUE + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_STBD:
self._log.debug(Fore.GREEN + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
elif event is Event.INFRARED_STBD_SIDE:
self._log.debug(Fore.GREEN + Style.BRIGHT +
'event: {};\tvalue: {:5.2f}cm'.format(
event.description, message.value))
# ........................................
elif event is Event.BUMPER_PORT:
self._log.info(Fore.RED + Style.BRIGHT +
'event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.BUMPER_CNTR:
self._log.info(Fore.BLUE + Style.BRIGHT +
'event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.BUMPER_STBD:
self._log.info(Fore.GREEN + Style.BRIGHT +
'event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.PORT_VELOCITY:
if not self._port_pid.enabled:
self._port_pid.enable()
# if message.value == 0: # on push of button
# _value = message.value # with no hysteresis
_value = self._hysteresis(message.value)
_velocity = Gamepad.convert_range(_value)
self._log.debug(Fore.RED +
'PORT: {};\tvalue: {:>5.2f}; velocity: {:>5.2f};'.
format(event.description, _value, _velocity))
# self._motors.set_motor(Orientation.PORT, _velocity)
self._port_pid.velocity = _velocity * 100.0
elif event is Event.PORT_THETA:
# if message.value == 0: # on push of button
_velocity = -1 * Gamepad.convert_range(message.value)
self._log.debug('{};\tvalue: {:>5.2f}'.format(
event.description, _velocity))
elif event is Event.STBD_VELOCITY:
if not self._stbd_pid.enabled:
self._stbd_pid.enable()
# if message.value == 0: # on push of button
# _value = message.value # with no hysteresis
_value = self._hysteresis(message.value)
_velocity = Gamepad.convert_range(_value)
self._log.info(Fore.GREEN +
'STBD: {};\tvalue: {:>5.2f}; velocity: {:>5.2f};'.
format(event.description, _value, _velocity))
# self._motors.set_motor(Orientation.STBD, _velocity)
self._stbd_pid.velocity = _velocity * 100.0
elif event is Event.STBD_THETA:
# if message.value == 0: # on push of button
_velocity = -1 * Gamepad.convert_range(message.value)
self._log.debug('{};\tvalue: {:>5.2f}'.format(
event.description, _velocity))
elif event is Event.SHUTDOWN:
if message.value == 1: # shutdown on push of button
self._log.info('SHUTDOWN event.')
self._close()
elif event is Event.STANDBY:
self._log.info(Fore.YELLOW + Style.DIM +
'STANDBY event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.HALT:
self._log.info(Fore.RED + Style.BRIGHT +
'HALT event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.BRAKE:
self._log.info(Fore.RED + Style.BRIGHT +
'BRAKE event: {};\tvalue: {:d}'.format(
event.description, message.value))
if message.value == 1: # shutdown on push of button
self._enable_pid(not self._port_pid.enabled)
elif event is Event.STOP:
self._log.info(Fore.RED + Style.BRIGHT +
'STOP event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.ROAM:
# if message.value == 0:
# self._behaviours.roam()
pass
elif event is Event.SNIFF:
# if message.value == 0:
# self._behaviours.one_meter()
pass
elif event is Event.NO_ACTION:
self._log.info(Fore.BLACK + Style.DIM +
'NO_ACTION event: {};\tvalue: {:d}'.format(
event.description, message.value))
elif event is Event.VIDEO:
if message.value == 0:
if self._video:
if self._video.active:
self._log.info(
Fore.GREEN + Style.DIM +
'STOP VIDEO event: {};\tvalue: {:d}'.format(
event.description, message.value))
self._enable_video(False)
else:
self._log.info(
Fore.GREEN + Style.DIM +
'START VIDEO event: {};\tvalue: {:d}'.format(
event.description, message.value))
self._enable_video(True)
# time.sleep(0.5) # debounce
else:
self._log.warning('video not available.')
elif event is Event.EVENT_L2:
if message.value == 0:
self._log.info(Fore.GREEN + Style.DIM +
'BLOB_SENSOR event: {};\tvalue: {:d}'.format(
event.description, message.value))
self._capture_blob()
# time.sleep(0.5) # debounce
elif event is Event.LIGHTS:
if message.value == 0:
self._enable_lights(not self._lights_on)
elif event is Event.EVENT_R1:
if message.value == 0:
self._log.info(
Fore.MAGENTA +
'EVENT_R1 event (cruise): {}'.format(event.description))
if self._cruise_behaviour is None:
# create a new CruiseBehaviour
self._cruise_behaviour = CruiseBehaviour(
self._config, self._pid_motor_ctrl, self._ifs,
Level.INFO)
self._queue.add_consumer(self._cruise_behaviour)
self._cruise_behaviour.enable()
self._log.info(Fore.MAGENTA + 'enabled cruise.')
else:
self._cruise_behaviour.disable()
# self._cruise_behaviour.close()
self._cruise_behaviour = None
self._log.info(Fore.MAGENTA + 'disabled cruise.')
else:
self._log.debug(Fore.RED +
'other event: {}'.format(event.description))
pass
self._start_time = dt.datetime.now()
def main():
_level = Level.INFO
_log = Logger('main', _level)
_loader = ConfigLoader(_level)
_config = _loader.configure('config.yaml')
try:
_motors = Motors(_config, None, Level.WARN)
_pot = Potentiometer(_config, Level.WARN)
_pot.set_output_limits(0.0, 127.0)
# motor configuration: starboard, port or both?
_orientation = Orientation.BOTH
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_motor = _motors.get_motor(Orientation.PORT)
_port_pid = PIDController(_config, _port_motor, level=Level.WARN)
_port_pid.enable()
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_motor = _motors.get_motor(Orientation.STBD)
_stbd_pid = PIDController(_config, _stbd_motor, level=Level.WARN)
_stbd_pid.enable()
ROTATE = True
_rotate = -1.0 if ROTATE else 1.0
# sys.exit(0)
_stbd_velocity = 0.0
_port_velocity = 0.0
_step = 0.5
_min = 0.0
_max = 70.0
_rate = Rate(10)
try:
# for _value in numpy.arange(_min, _max, _step):
while True:
# update RGB LED
# _pot.set_rgb(_pot.get_value())
_value = 127.0 - _pot.get_scaled_value(True)
if _value > 125.0:
_value = 127.0
_velocity = Gamepad.convert_range(_value)
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.setpoint = _velocity * 100.0
_port_velocity = _port_pid.setpoint
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.setpoint = _rotate * _velocity * 100.0
_stbd_velocity = _stbd_pid.setpoint
_log.info(Fore.WHITE + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \
+ Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity))
_rate.wait()
_log.info(Fore.YELLOW + 'resting...')
time.sleep(10.0)
# for _value in numpy.arange(_min, _max, _step):
while True:
# update RGB LED
# _pot.set_rgb(_pot.get_value())
_value = 127.0 - _pot.get_scaled_value(True)
if _value > 125.0:
_value = 127.0
_velocity = Gamepad.convert_range(_value)
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.setpoint = _rotate * _velocity * 100.0
_port_velocity = _port_pid.setpoint
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.setpoint = _velocity * 100.0
_stbd_velocity = _stbd_pid.setpoint
_log.info(Fore.MAGENTA + 'value: {:<5.2f}; set-point: {:5.2f}; velocity: '.format(_value, _velocity) \
+ Fore.RED + ' port: {:5.2f}\t'.format(_port_velocity) + Fore.GREEN + ' stbd: {:5.2f}'.format(_stbd_velocity))
_rate.wait()
_log.info(Fore.YELLOW + 'end of cruising.')
except KeyboardInterrupt:
_log.info(Fore.CYAN + Style.BRIGHT + 'PID test complete.')
finally:
if _orientation == Orientation.BOTH or _orientation == Orientation.PORT:
_port_pid.disable()
if _orientation == Orientation.BOTH or _orientation == Orientation.STBD:
_stbd_pid.disable()
_motors.brake()
except Exception as e:
_log.info(Fore.RED + Style.BRIGHT + 'error in PID controller: {}\n{}'.format(e, traceback.format_exc()))
finally:
_log.info(Fore.YELLOW + Style.BRIGHT + 'C. finally.')
def act(self, message, callback):
'''
Responds to the Event contained within the Message.
The callback method's API is:
callback(self._current_message, _current_power_levels)
'''
if not self._enabled:
self._log.warning('action ignored: controller disabled.')
return
_start_time = dt.datetime.now()
self._current_message = message
# _port_speed = self._motors.get_current_power_level(Orientation.PORT)
# _stbd_speed = self._motors.get_current_power_level(Orientation.STBD)
# if _port_speed is not None and _stbd_speed is not None:
# self._log.debug('last set power port: {:6.1f}, starboard: {:6.1f}'.format(_port_speed, _stbd_speed))
# else:
# self._log.debug('last set power port: {}, starboard: {}'.format(_port_speed, _stbd_speed))
_event = self._current_message.get_event()
self._log.debug(Fore.CYAN + 'act()' + Style.BRIGHT +
' event: {}.'.format(_event) + Fore.YELLOW)
self._current_message.start()
# no action ............................................................
if _event is Event.NO_ACTION:
self._log.info('event: no action.')
pass
# BUTTON ..............................................
# button ...............................................................
elif _event is Event.BUTTON:
# if button pressed we change standby mode but don't do anything else
_value = self._current_message.get_value()
# toggle value
# self.set_standby(not _value)
if _value:
self._log.critical('event: button value: {}.'.format(_value))
self.set_standby(False)
else:
self._log.error('event: button value: {}.'.format(_value))
self.set_standby(True)
# if in standby mode we don't process the event, but we do perform the callback
# # stopping and halting ...................................................
# SHUTDOWN ..............................................
# shutdown ............................................................
elif _event is Event.SHUTDOWN:
self._log.info('event: shutdown.')
self._motors.stop()
self._callback_shutdown()
# STOP ..............................................
# stop .................................................................
elif _event is Event.STOP:
self._log.info('event: stop.')
self._motors.stop()
# HALT ..............................................
# halt .................................................................
elif _event is Event.HALT:
self._log.info('event: halt.')
self._motors.halt()
# BRAKE ...................................
# brake ................................................................
elif _event is Event.BRAKE:
self._log.info('event: brake.')
self._motors.brake()
# STANDBY ..............................................
# standby ..............................................................
elif _event is Event.STANDBY:
# toggle standby state
_value = self._current_message.get_value()
if _value == 1:
if self._standby:
self.set_standby(False)
else:
self.set_standby(True)
else:
pass
# ROAM .................................................
elif _event is Event.ROAM:
self._log.info('event: roam.')
self._roam_behaviour.roam()
# SNIFF .................................................
elif _event is Event.SNIFF:
self._log.info('event: sniff.')
self._behaviours.sniff()
time.sleep(0.5) # debounce gamepad
# D-PAD HORIZONTAL .................................................
elif _event is Event.FORWARD_VELOCITY:
_direction = message.get_value()
_speed = 80.0
if _direction == -1: # then ahead
self._log.info('event: d-pad movement: ahead.')
# self._motors.change_velocity(0.5, 0.5, SlewRate.SLOW, -1)
self._motors.ahead(_speed)
time.sleep(2.0)
elif _direction == 1: # then astern
self._log.info('event: d-pad movement: astern.')
# self._motors.change_velocity(-0.5, -0.5, SlewRate.SLOW, -1)
self._motors.astern(_speed)
time.sleep(2.0)
else:
self._log.info('event: d-pad movement: halt.')
# D-PAD VERTICAL .................................................
elif _event is Event.THETA:
_direction = message.get_value()
if _direction == -1: # then ahead
self._log.info('event: d-pad rotate: counter-clockwise.')
self._motors.step(-100.0, 100.0, -1, -1)
time.sleep(2.0)
elif _direction == 1: # then astern
self._log.info('event: d-pad rotate: clockwise.')
self._motors.step(100.0, -100.0, -1, -1)
time.sleep(2.0)
else:
self._log.info('event: d-pad rotate: none.')
# PORT_VELOCITY ................................................
elif _event is Event.PORT_VELOCITY:
_velocity = Gamepad.convert_range(message.get_value())
self._log.info(
Fore.GREEN + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
self._port_motor.set_motor_power(_velocity)
# PORT_THETA ...................................................
elif _event is Event.PORT_THETA:
_velocity = -1 * Gamepad.convert_range(message.get_value())
self._log.info(
Fore.MAGENTA + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
# STBD_VELOCITY ................................................
elif _event is Event.STBD_VELOCITY:
_velocity = Gamepad.convert_range(message.get_value())
self._log.info(
Fore.GREEN + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
self._stbd_motor.set_motor_power(_velocity)
# STBD_THETA ...................................................
elif _event is Event.STBD_THETA:
_velocity = -1 * Gamepad.convert_range(message.get_value())
self._log.info(
Fore.MAGENTA + Style.BRIGHT +
'{};\tvalue: {:>5.2f}'.format(_event.description, _velocity))
# # bumper .............................................................
# BUMPER_PORT ..............................................
elif _event is Event.BUMPER_PORT:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._ifs.disable()
self._motors.stop()
# self._motors.astern(Speed.HALF.value)
self._motors.turn_astern(Speed.THREE_QUARTER.value,
Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._ifs.enable()
self._log.info('action complete: port bumper.')
else:
self._log.info('no action required (not moving): port bumper.')
# BUMPER_CNTR ..............................................
elif _event is Event.BUMPER_CNTR:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._ifs.disable()
self._motors.stop()
self._motors.astern(Speed.HALF.value)
time.sleep(0.5)
self._motors.brake()
self._ifs.enable()
self._log.info('action complete: center bumper.')
else:
self._log.info(
'no action required (not moving): center bumper.')
# BUMPER_STBD ..............................................
elif _event is Event.BUMPER_STBD:
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard bumper.' + Style.RESET_ALL)
if self._motors.is_in_motion(): # if we're moving then halt
self._motors.stop()
# self._motors.astern(Speed.FULL.value)
self._motors.turn_astern(Speed.HALF.value,
Speed.THREE_QUARTER.value)
time.sleep(0.5)
self._motors.brake()
self._log.info('action complete: starboard bumper.')
else:
self._log.info(
'no action required (not moving): starboard bumper.')
# # infrared ...........................................................
# INFRARED_PORT_SIDE ..............................................
elif _event is Event.INFRARED_PORT_SIDE:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port side infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
pass
# INFRARED_PORT ..............................................
elif _event is Event.INFRARED_PORT:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.RED +
' port infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.PORT)
self._log.info('action complete: port infrared.')
else:
self._log.info(
'no action required (not moving): port infrared.')
pass
# INFRARED_CNTR ..............................................
elif _event is Event.INFRARED_CNTR:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL + Fore.BLUE +
' center infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.CNTR)
self._log.info('action complete: center infrared.')
else:
self._log.info(
'no action required (not moving): center infrared.')
pass
# INFRARED_STBD ...................................................
elif _event is Event.INFRARED_STBD:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard infrared; ' + Fore.YELLOW +
'value: {}'.format(_value))
if self._motors.is_in_motion(): # if we're moving then avoid
self._avoid(Orientation.STBD)
self._log.info('action complete: starboard infrared.')
else:
self._log.info(
'no action required (not moving): starboard infrared.')
pass
# INFRARED_STBD_SIDE ...........................................
elif _event is Event.INFRARED_STBD_SIDE:
_value = message.get_value()
self._log.info(Style.BRIGHT + 'event:' + Style.NORMAL +
Fore.GREEN + ' starboard side infrared; ' +
Fore.YELLOW + 'value: {}'.format(_value))
pass
# EVENT UNKNOWN: FAILURE ...............................................
else:
self._log.error('unrecognised event: {}'.format(_event))
pass
_current_power_levels = self._motors.get_current_power_levels()
if callback is not None:
self._log.debug(
Fore.MAGENTA + 'callback message: {}; '.format(
self._current_message.get_value()) + Fore.CYAN +
'current power levels: {}'.format(_current_power_levels))
callback(self._current_message, _current_power_levels)
self._clear_current_message()
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.debug(Fore.MAGENTA + Style.DIM +
'elapsed: {}ms'.format(_elapsed_ms) + Style.DIM)