def test_message():
_log = Logger('message-test', Level.INFO)
try:
_log.info('start message test...')
_message_bus = MessageBus(Level.INFO)
_message_factory = MessageFactory(_message_bus, Level.INFO)
_subscriber1 = Subscriber('behaviour', Fore.YELLOW, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber1)
_subscriber2 = Subscriber('infrared', Fore.MAGENTA, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber2)
_subscriber3 = Subscriber('bumper', Fore.GREEN, _message_bus,
Level.INFO)
_message_bus.register_subscriber(_subscriber3)
_message_bus.print_publishers()
_message_bus.print_subscribers()
# ................................
_message = _message_factory.get_message(Event.BRAKE, True)
_log.info('created message {}; event: {}'.format(
_message.name, _message.event.name))
_message.acknowledge(_subscriber1)
_subscriber1.print_message_info('sub1 info for message:', _message,
None)
except Exception as e:
_log.error('error: {}'.format(e))
finally:
_log.info('complete.')
class MockMessageQueue(object):
def __init__(self, level=Level.INFO):
self._log = Logger('flask.wrapper', level)
self._message_factory = MessageFactory(None, level)
self._log.info('ready.')
# ......................................................
def respond(self, event):
'''
Responds to the Event by wrapping it in Message and adding it to the backing queue.
This is only used by FlaskWrapper.
'''
self._log.info('RESPOND to event {}.'.format(event.name))
_message = self._message_factory.get_message(event, None)
self.add(_message)
return jsonify([{
'id': _message.message_id
}, {
'event': event.name
}, {
'priority': event.priority
}])
# ..........................................................................
def add(self, message):
'''
Add a new Message to the queue, then additionally to any consumers.
'''
# if self._queue.full():
# try:
# _dumped = self._queue.get()
# self._log.debug('dumping old message eid#{}/msg#{}: {}'.format(_dumped.eid, _dumped.number, _dumped.description))
# except Empty:
# pass
# self._queue.put(message);
self._log.info('added message id {} with event: {}'.format(
message.message_id, message.event.description))
class IntegratedFrontSensor():
'''
IntegratedFrontSensor: communicates with the integrated front bumpers and
infrared sensors, receiving messages from the IO Expander board or I²C
Arduino slave, sending the messages with its events onto the message bus.
This listens to the Clock and at a frequency of ( TICK % tick_modulo )
calls the _poll() method.
Parameters:
:param config: the YAML based application configuration
:param queue: the message queue receiving activation notifications
:param clock: the clock providing the polling loop trigger
:param message_bus: the message bus to send event messages
:param message_factory: optional MessageFactory
:param level: the logging Level
'''
# ..........................................................................
def __init__(self, config, queue, clock, message_bus, message_factory,
level):
if config is None:
raise ValueError('no configuration provided.')
self._log = Logger("ifs", level)
self._log.info('configuring integrated front sensor...')
_cruise_config = config['ros'].get('cruise_behaviour')
self._cruising_velocity = _cruise_config.get('cruising_velocity')
self._config = config['ros'].get('integrated_front_sensor')
self._clock = clock
self._clock.add_consumer(self)
self._message_bus = message_bus
# _queue = queue
# _queue.add_consumer(self)
self._device_id = self._config.get(
'device_id'
) # i2c hex address of slave device, must match Arduino's SLAVE_I2C_ADDRESS
self._channel = self._config.get('channel')
self._ignore_duplicates = self._config.get('ignore_duplicates')
self._tick_modulo = self._config.get('tick_modulo')
_max_workers = self._config.get('max_workers')
self._log.info('tick modulo: {:d}'.format(self._tick_modulo))
# event thresholds:
self._callback_cntr_min_trigger = self._config.get(
'callback_center_minimum_trigger')
self._callback_side_min_trigger = self._config.get(
'callback_side_minimum_trigger')
self._callback_min_trigger = self._config.get(
'callback_minimum_trigger')
self._port_side_trigger_distance = self._config.get(
'port_side_trigger_distance')
self._port_trigger_distance = self._config.get('port_trigger_distance')
self._center_trigger_distance = self._config.get(
'center_trigger_distance')
self._stbd_trigger_distance = self._config.get('stbd_trigger_distance')
self._stbd_side_trigger_distance = self._config.get(
'stbd_side_trigger_distance')
self._log.info('event thresholds: \t' \
+Fore.RED + ' port side={:>5.2f}; port={:>5.2f};'.format(self._port_side_trigger_distance, self._port_trigger_distance) \
+Fore.BLUE + ' center={:>5.2f};'.format(self._center_trigger_distance) \
+Fore.GREEN + ' stbd={:>5.2f}; stbd side={:>5.2f}'.format(self._stbd_trigger_distance, self._stbd_side_trigger_distance))
# hardware pin assignments
self._port_side_ir_pin = self._config.get('port_side_ir_pin')
self._port_ir_pin = self._config.get('port_ir_pin')
self._center_ir_pin = self._config.get('center_ir_pin')
self._stbd_ir_pin = self._config.get('stbd_ir_pin')
self._stbd_side_ir_pin = self._config.get('stbd_side_ir_pin')
self._log.info('infrared pin assignments:\t' \
+Fore.RED + ' port side={:d}; port={:d};'.format(self._port_side_ir_pin, self._port_ir_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_ir_pin) \
+Fore.GREEN + ' stbd={:d}; stbd side={:d}'.format(self._stbd_ir_pin, self._stbd_side_ir_pin))
self._port_bmp_pin = self._config.get('port_bmp_pin')
self._center_bmp_pin = self._config.get('center_bmp_pin')
self._stbd_bmp_pin = self._config.get('stbd_bmp_pin')
self._log.info('bumper pin assignments:\t' \
+Fore.RED + ' port={:d};'.format(self._port_bmp_pin) \
+Fore.BLUE + ' center={:d};'.format(self._center_bmp_pin) \
+Fore.GREEN + ' stbd={:d}'.format(self._stbd_bmp_pin))
if message_factory:
self._message_factory = message_factory
else:
self._message_factory = MessageFactory(level)
# self._executor = ProcessPoolExecutor(max_workers=_max_workers)
self._log.info(
'creating thread pool executor with maximum of {:d} workers.'.
format(_max_workers))
self._executor = ThreadPoolExecutor(max_workers=_max_workers,
thread_name_prefix='ifs')
# config IO Expander
self._ioe = IoExpander(config, Level.INFO)
# calculating means for IR sensors
self._pot = Potentiometer(config, Level.INFO)
_queue_limit = 2 # larger number means it takes longer to change
self._deque_port_side = Deque([], maxlen=_queue_limit)
self._deque_port = Deque([], maxlen=_queue_limit)
self._deque_cntr = Deque([], maxlen=_queue_limit)
self._deque_stbd = Deque([], maxlen=_queue_limit)
self._deque_stbd_side = Deque([], maxlen=_queue_limit)
# ...
self._last_event = None
self._last_value = None
self._enabled = False
self._suppressed = False
self._closed = False
self._log.info(Fore.MAGENTA + 'ready.')
# ......................................................
def add(self, message):
'''
Reacts to every nth (modulo) TICK message, submitting a _poll Thread
from the thread pool executor, which polls the various sensors and
sending callbacks for each.
Note that if the loop frequency is set this method is disabled.
'''
if self._enabled and message.event is Event.CLOCK_TICK:
if (message.value % self._tick_modulo) == 0:
_future = self._executor.submit(self._poll, message.value,
lambda: self.enabled)
# ......................................................
def _poll(self, count, f_is_enabled):
'''
Poll the various infrared and bumper sensors, executing callbacks for each.
In tests this typically takes 173ms from ItsyBitsy, 85ms from the Pimoroni IO Expander.
We add a messsage for the bumpers immediately (rather than use a callback) after reading
the sensors since their response must be as fast as possible.
'''
if not f_is_enabled():
self._log.warning('[{:04d}] poll not enabled'.format(count))
return
self._log.info(Fore.BLACK + '[{:04d}] ifs poll start.'.format(count))
_current_thread = threading.current_thread()
_current_thread.name = 'poll'
_start_time = dt.datetime.now()
# pin 10: digital bumper sensor ........................................
_port_bmp_data = self.get_input_for_event_type(Event.BUMPER_PORT)
_cntr_bmp_data = self.get_input_for_event_type(Event.BUMPER_CNTR)
_stbd_bmp_data = self.get_input_for_event_type(Event.BUMPER_STBD)
_port_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_PORT_SIDE)
_port_ir_data = self.get_input_for_event_type(Event.INFRARED_PORT)
_cntr_ir_data = self.get_input_for_event_type(Event.INFRARED_CNTR)
_stbd_ir_data = self.get_input_for_event_type(Event.INFRARED_STBD)
_stbd_side_ir_data = self.get_input_for_event_type(
Event.INFRARED_STBD_SIDE)
# self._callback(Event.BUMPER_CNTR, _cntr_bmp_data)
if _cntr_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_CNTR, _cntr_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_CNTR event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 9: digital bumper sensor .........................................
# self._callback(Event.BUMPER_PORT, _port_bmp_data)
if _port_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_PORT, _port_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_PORT event.'.
format(_message.eid))
self._message_bus.handle(_message)
# pin 11: digital bumper sensor ........................................
# self._callback(Event.BUMPER_STBD, _stbd_bmp_data)
if _stbd_bmp_data == 1:
_message = self._message_factory.get_message(
Event.BUMPER_STBD, _stbd_bmp_data)
self._log.info(Fore.BLUE + Style.BRIGHT +
'adding new message eid#{} for BUMPER_STBD event.'.
format(_message.eid))
self._message_bus.handle(_message)
# port side analog infrared sensor .....................................
if _port_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.RED + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 1) + (Fore.RED if (_port_side_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_side_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT_SIDE, _port_side_ir_data)
# port analog infrared sensor ..........................................
if _port_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_PORT, _port_ir_data)
# self._log.info(Fore.RED + Style.BRIGHT + 'adding new message eid#{} for INFRARED_PORT event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 2) + (Fore.RED if (_port_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_port_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_PORT, _port_ir_data)
# center analog infrared sensor ........................................
if _cntr_ir_data > self._callback_cntr_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_CNTR, _cntr_ir_data)
# self._log.info(Fore.BLUE + Style.BRIGHT + 'adding new message eid#{} for INFRARED_CNTR event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(Fore.BLUE + '[{:04d}] ANALOG IR ({:d}): \t'.format(count, 3) + (Fore.RED if (_cntr_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_cntr_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_CNTR, _cntr_ir_data)
# starboard analog infrared sensor .....................................
if _stbd_ir_data > self._callback_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD, _stbd_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info('[{:04d}] ANALOG IR ({:d}): \t'.format(count, 4) + (Fore.RED if (_stbd_ir_data > 100.0) else Fore.YELLOW) \
+ Style.BRIGHT + '{:d}'.format(_stbd_ir_data) + Style.DIM + '\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD, _stbd_ir_data)
# starboard side analog infrared sensor ................................
if _stbd_side_ir_data > self._callback_side_min_trigger:
# _message = self._message_factory.get_message(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
# self._log.info(Fore.GREEN + Style.BRIGHT + 'adding new message eid#{} for INFRARED_STBD_SIDE event.'.format(_message.eid))
# self._message_bus.handle(_message)
self._log.info(
'[{:04d}] ANALOG IR ({:d}): \t'.format(count, 5) +
(Fore.RED if (_stbd_side_ir_data > 100.0) else Fore.YELLOW) +
Style.BRIGHT + '{:d}'.format(_stbd_side_ir_data) + Style.DIM +
'\t(analog value 0-255)')
self._callback(Event.INFRARED_STBD_SIDE, _stbd_side_ir_data)
_delta = dt.datetime.now() - _start_time
_elapsed_ms = int(_delta.total_seconds() * 1000)
self._log.info(Fore.BLACK +
'[{:04d}] poll end; elapsed processing time: {:d}ms'.
format(count, _elapsed_ms))
# return True
# ..........................................................................
def _get_mean_distance(self, orientation, value):
'''
Returns the mean of values collected in the queue for the specified IR sensor.
'''
if value == None or value == 0:
return None
if orientation is Orientation.PORT_SIDE:
_deque = self._deque_port_side
elif orientation is Orientation.PORT:
_deque = self._deque_port
elif orientation is Orientation.CNTR:
_deque = self._deque_cntr
elif orientation is Orientation.STBD:
_deque = self._deque_stbd
elif orientation is Orientation.STBD_SIDE:
_deque = self._deque_stbd_side
else:
raise ValueError('unsupported orientation.')
_deque.append(value)
_n = 0
_mean = 0.0
for x in _deque:
_n += 1
_mean += (x - _mean) / _n
if _n < 1:
return float('nan')
else:
return _mean
# ..........................................................................
def _convert_to_distance(self, value):
'''
Converts the value returned by the IR sensor to a distance in centimeters.
Distance Calculation ---------------
This is reading the distance from a 3 volt Sharp GP2Y0A60SZLF infrared
sensor to a piece of white A4 printer paper in a low ambient light room.
The sensor output is not linear, but its accuracy is not critical. If
the target is too close to the sensor the values are not valid. According
to spec 10cm is the minimum distance, but we get relative variability up
until about 5cm. Values over 150 clearly indicate the robot is less than
10cm from the target.
0cm = unreliable
5cm = 226.5
7.5cm = 197.0
10cm = 151.0
20cm = 92.0
30cm = 69.9
40cm = 59.2
50cm = 52.0
60cm = 46.0
70cm = 41.8
80cm = 38.2
90cm = 35.8
100cm = 34.0
110cm = 32.9
120cm = 31.7
130cm = 30.7 *
140cm = 30.7 *
150cm = 29.4 *
* Maximum range on IR is about 130cm, after which there is diminishing
stability/variability, i.e., it's hard to determine if we're dealing
with a level of system noise rather than data. Different runs produce
different results, with values between 28 - 31 on a range of any more
than 130cm.
See: http://ediy.com.my/blog/item/92-sharp-gp2y0a21-ir-distance-sensors
'''
if value == None or value == 0:
return None
self._use_pot = False
# if self._use_pot:
# _pot_value = self._pot.get_scaled_value()
# _EXPONENT = _pot_value
# else:
# _pot_value = 0.0
_EXPONENT = 1.33
_NUMERATOR = 1000.0
_distance = pow(_NUMERATOR / value, _EXPONENT) # 900
# self._log.debug(Fore.BLACK + Style.NORMAL + 'value: {:>5.2f}; pot value: {:>5.2f}; distance: {:>5.2f}cm'.format(value, _pot_value, _distance))
return _distance
# ..........................................................................
def _callback(self, event, value):
'''
This is the callback method from the I²C Master, whose events are
being returned from the source, e.g., Arduino or IO Expander board.
'''
if not self._enabled or self._suppressed:
self._log.info(Fore.BLACK + Style.DIM +
'SUPPRESSED callback: event {}; value: {:d}'.format(
event.name, value))
return
try:
# self._log.debug(Fore.BLACK + 'callback: event {}; value: {:d}'.format(event.name, value))
_event = None
_value = value
# bumpers ..................................................................................
if event == Event.BUMPER_PORT:
if value == 1:
_event = Event.BUMPER_PORT
_value = 1
elif event == Event.BUMPER_CNTR:
if value == 1:
_event = Event.BUMPER_CNTR
_value = 1
elif event == Event.BUMPER_STBD:
if value == 1:
_event = Event.BUMPER_STBD
_value = 1
# ..........................................................................................
# For IR sensors we rewrite the value with a dynamic mean distance (cm),
# setting the event type only if the value is less than a distance threshold.
elif event == Event.INFRARED_PORT_SIDE:
_value = self._get_mean_distance(
Orientation.PORT_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
if _value != None and _value < self._port_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT SIDE'.format(_value))
elif event == Event.INFRARED_PORT:
_value = self._get_mean_distance(
Orientation.PORT, self._convert_to_distance(value))
self._log.info(
Fore.RED + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tPORT'.format(_value))
if _value != None and _value < self._port_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.RED + 'mean distance: {:5.2f}cm;\tPORT'.format(_value))
elif event == Event.INFRARED_CNTR:
_value = self._get_mean_distance(
Orientation.CNTR, self._convert_to_distance(value))
self._log.info(
Fore.BLUE + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
if _value != None and _value < self._center_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.BLUE + 'mean distance: {:5.2f}cm;\tCNTR'.format(_value))
elif event == Event.INFRARED_STBD:
_value = self._get_mean_distance(
Orientation.STBD, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
if _value != None and _value < self._stbd_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD'.format(_value))
elif event == Event.INFRARED_STBD_SIDE:
_value = self._get_mean_distance(
Orientation.STBD_SIDE, self._convert_to_distance(value))
self._log.info(
Fore.GREEN + Style.BRIGHT +
'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
if _value != None and _value < self._stbd_side_trigger_distance:
_fire_message(event, _value)
# _event = event
# else:
# self._log.info(Fore.GREEN + 'mean distance: {:5.2f}cm;\tSTBD SIDE'.format(_value))
# if _event is not None and _value is not None:
# # if not self._ignore_duplicates or ( _event != self._last_event and _value != self._last_value ):
# _message = self._message_factory.get_message(_event, _value)
# self._log.info(Fore.WHITE + Style.BRIGHT + 'adding new message eid#{} for event {}'.format(_message.eid, _event.description))
# self._message_bus.handle(_message)
# # else:
# # self._log.warning(Fore.CYAN + Style.NORMAL + 'ignoring message for event {}'.format(_event.description))
# self._last_event = _event
# self._last_value = _value
# else:
# self._log.info(Fore.WHITE + Style.BRIGHT + 'NOT ADDING message eid#{} for event {}'.format(_message.eid, _event.description))
except Exception as e:
self._log.error('callback error: {}\n{}'.format(
e, traceback.format_exc()))
# ..........................................................................
def _fire_message(self, event, value):
self._log.info(
Fore.YELLOW + Style.BRIGHT +
'fire message with event {} and value {}'.format(event, value))
if event is not None and value is not None:
_message = self._message_factory.get_message(event, value)
self._log.info(Fore.WHITE + Style.BRIGHT +
'adding new message eid#{} for event {}'.format(
_message.eid, _event.description))
self._message_bus.handle(_message)
else:
self._log.info(Fore.RED + Style.BRIGHT +
'ignoring message with event {} and value {}'.
format(event, value))
# ..........................................................................
def get_input_for_event_type(self, event):
'''
Return the current value of the pin corresponding to the Event type.
'''
if event is Event.INFRARED_PORT_SIDE:
return self._ioe.get_port_side_ir_value()
elif event is Event.INFRARED_PORT:
return self._ioe.get_port_ir_value()
elif event is Event.INFRARED_CNTR:
return self._ioe.get_center_ir_value()
elif event is Event.INFRARED_STBD:
return self._ioe.get_stbd_ir_value()
elif event is Event.INFRARED_STBD_SIDE:
return self._ioe.get_stbd_side_ir_value()
elif event is Event.BUMPER_PORT:
return self._ioe.get_port_bmp_value()
elif event is Event.BUMPER_CNTR:
return self._ioe.get_center_bmp_value()
elif event is Event.BUMPER_STBD:
return self._ioe.get_stbd_bmp_value()
else:
raise Exception('unexpected event type.')
# ..........................................................................
def suppress(self, state):
self._log.info('suppress {}.'.format(state))
self._suppressed = state
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._enabled:
if not self._closed:
self._log.info('enabled.')
self._enabled = True
else:
self._log.warning('cannot enable: already closed.')
else:
self._log.warning('already enabled.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._log.info(Fore.YELLOW +
'shutting down thread pool executor...')
self._executor.shutdown(
wait=False) # python 3.9: , cancel_futures=True)
self._log.info(
Fore.YELLOW +
'disabled: thread pool executor has been shut down.')
else:
self._log.debug('already disabled.')
# ..........................................................................
def close(self):
'''
Permanently close and disable the integrated front sensor.
'''
if not self._closed:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
# ..........................................................................
@staticmethod
def clamp(n, minn, maxn):
return max(min(maxn, n), minn)
# ..........................................................................
@staticmethod
def remap(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
class MockIntegratedFrontSensor(object):
'''
A mock IFS.
'''
def __init__(self, message_bus, exit_on_complete=True, level=Level.INFO):
super().__init__()
self._log = Logger("mock-ifs", level)
self._message_factory = MessageFactory(Level.INFO)
self._message_bus = message_bus
# self._message_bus = MockMessageBus(self, Level.INFO)
self.exit_on_complete = exit_on_complete
# self._rate = Rate(10)
self._thread = None
self._enabled = False
self._verbose = True
self._suppress = False
self._closed = False
self._counter = itertools.count()
# .....
self._triggered_ir_port_side = self._triggered_ir_port = self._triggered_ir_cntr = self._triggered_ir_stbd = \
self._triggered_ir_stbd_side = self._triggered_bmp_port = self._triggered_bmp_cntr = self._triggered_bmp_stbd = 0
self._limit = 3
self._fmt = '{0:>9}'
self._log.info('ready.')
# ..........................................................................
def name(self):
return 'MockIntegratedFrontSensor'
# ..........................................................................
def suppress(self, mode):
'''
Enable or disable capturing characters. Upon starting the loop the
suppress flag is set False, but can be enabled or disabled as
necessary without halting the thread.
'''
self._suppress = mode
# ..........................................................................
def _start_loop(self, f_is_enabled):
self._log.info('start loop:\t' + Fore.YELLOW + 'type the \"' + Fore.RED + 'Delete' + Fore.YELLOW \
+ '\" or \"' + Fore.RED + 'q' + Fore.YELLOW + '\" key to exit sensor loop, the \"' \
+ Fore.RED + 'h' + Fore.YELLOW + '\" key for help.')
print('\n')
while f_is_enabled():
self.sense()
self._rate.wait()
self._log.info(Fore.YELLOW + 'exit loop.')
# ..........................................................................
def sense(self):
'''
See if any sensor (key) has been activated.
'''
if self._suppress: # then just sleep during the loop
time.sleep(0.2)
else:
_count = next(self._counter)
self._log.info('[{:03d}]'.format(_count))
ch = readchar.readchar()
och = ord(ch)
if och == 91 or och == 113 or och == 127: # 'q' or delete
self.disable()
return
elif och == 104: # 'h' for help
self.print_keymap()
return
elif och == 109: # 'j' to toggle messages sent to IFS
self._verbose = not self._verbose
self._log.info(
Fore.YELLOW +
'setting verbose mode to: {}'.format(self._verbose))
_event = self.get_event_for_char(och)
if _event is not None:
# self._log.info('firing message for event {}'.format(event))
# _message = self._message_factory.get_message(event, True)
# self._message_bus.publish(_message)
# await asyncio.sleep(0.1)
self.fire_message(_event)
self._log.info('[{:03d}] "{}" ({}) pressed; event: {}'.format(
_count, ch, och, _event))
if self.exit_on_complete and self.all_triggered:
self._log.info('[{:03d}] COMPLETE.'.format(_count))
self.disable()
return
elif self._verbose:
self.waiting_for_message()
else:
self._log.info(
'[{:03d}] unmapped key "{}" ({}) pressed.'.format(
_count, ch, och))
# ..........................................................................
# async def fire_message(self, event):
def fire_message(self, event):
self._log.info('firing message for event {}'.format(event))
_message = self._message_factory.get_message(event, True)
self._message_bus.publish(_message)
# await asyncio.sleep(0.1)
# message handling .........................................................
# ......................................................
def process_message(self, message):
'''
Processes the message, keeping count and providing a display of status.
'''
_event = message.event
if _event is Event.BUMPER_PORT:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_bmp_port < self._limit:
self._triggered_bmp_port += 1
elif _event is Event.BUMPER_CNTR:
self._print_event(Fore.BLUE, _event, message.value)
if self._triggered_bmp_cntr < self._limit:
self._triggered_bmp_cntr += 1
elif _event is Event.BUMPER_STBD:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_bmp_stbd < self._limit:
self._triggered_bmp_stbd += 1
elif _event is Event.INFRARED_PORT_SIDE:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_ir_port_side < self._limit:
self._triggered_ir_port_side += 1
elif _event is Event.INFRARED_PORT:
self._print_event(Fore.RED, _event, message.value)
if self._triggered_ir_port < self._limit:
self._triggered_ir_port += 1
elif _event is Event.INFRARED_CNTR:
self._print_event(Fore.BLUE, _event, message.value)
if self._triggered_ir_cntr < self._limit:
self._triggered_ir_cntr += 1
elif _event is Event.INFRARED_STBD:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_ir_stbd < self._limit:
self._triggered_ir_stbd += 1
elif _event is Event.INFRARED_STBD_SIDE:
self._print_event(Fore.GREEN, _event, message.value)
if self._triggered_ir_stbd_side < self._limit:
self._triggered_ir_stbd_side += 1
else:
self._log.info(Fore.BLACK + Style.BRIGHT +
'other event: {}'.format(_event.description))
# ......................................................
def _print_event(self, color, event, value):
self._log.info('event:\t' + color + Style.BRIGHT +
'{}; value: {}'.format(event.description, value))
# ..........................................................................
def waiting_for_message(self):
_div = Fore.CYAN + Style.NORMAL + ' | '
self._log.info('waiting for: | ' \
+ self._get_output(Fore.RED, 'PSID', self._triggered_ir_port_side) \
+ _div \
+ self._get_output(Fore.RED, 'PORT', self._triggered_ir_port) \
+ _div \
+ self._get_output(Fore.BLUE, 'CNTR', self._triggered_ir_cntr) \
+ _div \
+ self._get_output(Fore.GREEN, 'STBD', self._triggered_ir_stbd) \
+ _div \
+ self._get_output(Fore.GREEN, 'SSID', self._triggered_ir_stbd_side) \
+ _div \
+ self._get_output(Fore.RED, 'BPRT', self._triggered_bmp_port) \
+ _div \
+ self._get_output(Fore.BLUE, 'BCNT', self._triggered_bmp_cntr) \
+ _div \
+ self._get_output(Fore.GREEN, 'BSTB', self._triggered_bmp_stbd) \
+ _div )
# ......................................................
def _get_output(self, color, label, value):
if (value == 0):
_style = color + Style.BRIGHT
elif (value == 1):
_style = color + Style.NORMAL
elif (value == 2):
_style = color + Style.DIM
else:
_style = Fore.BLACK + Style.DIM
return _style + self._fmt.format(label if
(value < self._limit) else '')
# ......................................................
@property
def all_triggered(self):
return self._triggered_ir_port_side >= self._limit \
and self._triggered_ir_port >= self._limit \
and self._triggered_ir_cntr >= self._limit \
and self._triggered_ir_stbd >= self._limit \
and self._triggered_ir_stbd_side >= self._limit \
and self._triggered_bmp_port >= self._limit \
and self._triggered_bmp_cntr >= self._limit \
and self._triggered_bmp_stbd >= self._limit
# ..........................................................................
@property
def enabled(self):
return self._enabled
# ..........................................................................
def enable(self):
if not self._closed:
if self._enabled:
self._log.warning('already enabled.')
else:
# if we haven't started the thread yet, do so now...
if self._thread is None:
self._enabled = True
# self._thread = Thread(name='mock-ifs', target=MockIntegratedFrontSensor._start_loop, args=[self, lambda: self.enabled], daemon=True)
# self._thread.start()
self._log.info('enabled.')
else:
self._log.warning('cannot enable: thread already exists.')
else:
self._log.warning('cannot enable: already closed.')
# ..........................................................................
def disable(self):
if self._enabled:
self._enabled = False
self._thread = None
self._log.info('disabled.')
else:
self._log.warning('already disabled.')
# ..........................................................................
def close(self):
if not self._closed:
if self._enabled:
self.disable()
self._closed = True
self._log.info('closed.')
else:
self._log.warning('already closed.')
# ..........................................................................
def print_keymap(self):
# 1 2 3 4 5 6 7 8 9 C 1 2
#23456789012345678901234567890123456789012345678901234567890123456789012345678901234567890123456789012345678901234567890
self._log.info('''key map:
o-------------------------------------------------o ---o--------o
| Q | | T | | DEL |
| QUIT | | NOOP | | QUIT |
o--------------------------------------------------------------------------o---------o ---o--------o
| A | S | D | F | G | H | J | K |
| IR_PSID | IR_PORT | IR_CNTR | IR_STBD | IR_SSID | HELP | | |
o-------------------------------------------------------------------------------o------------------------o
| X | C | V | | M | < | > |
| BM_PORT | BM_CNTR | BM_STBD | | TOG_MSG | DN_VELO | UP_VELO |
o-----------------------------o o-----------------------------o
''')
self._log.info('note:\t' + Fore.YELLOW +
'will exit after receiving 3 events on each sensor.')
print('')
# ..........................................................................
def get_event_for_char(self, och):
'''
Below are the mapped characters for IFS-based events, including several others:
oct dec hex char usage
54 44 2C , increase motors speed (both)
56 46 2E . decrease motors speed (both)
141 97 61 a * port side IR
142 98 62 b
143 99 63 c * cntr BMP
144 100 64 d * cntr IR
145 101 65 e
146 102 66 f * stbd IR
147 103 67 g * stbd side IR
150 104 68 h
151 105 69 i
152 106 6A j
153 107 6B k
154 108 6C l
155 109 6D m toggle IFS messaging
156 110 6E n
157 111 6F o
160 112 70 p
161 113 71 q
162 114 72 r
163 115 73 s * port IR
164 116 74 t * noop (test message)
165 117 75 u
166 118 76 v * stbd BMP
167 119 77 w
170 120 78 x * port BMP
171 121 79 y
172 122 7A z
'''
if och == 44: # ,
return Event.DECREASE_SPEED
elif och == 46: # .
return Event.INCREASE_SPEED
elif och == 97: # a
return Event.INFRARED_PORT_SIDE
elif och == 99: # c
return Event.BUMPER_CNTR
elif och == 100: # d
return Event.INFRARED_CNTR
elif och == 102: # f
return Event.INFRARED_STBD
elif och == 103: # g
return Event.INFRARED_STBD_SIDE
elif och == 115: # s
return Event.INFRARED_PORT
elif och == 116: # s
return Event.NOOP
elif och == 118: # v
return Event.BUMPER_STBD
elif och == 120: # x
return Event.BUMPER_PORT
else:
return None