class RobotBehavior(object):
def __init__(self, behavior_stack):
self.logr = LagerLogger(self.__class__.__name__)
self.logr.console(LagerLogger.DEBUG)
self.behavior_stack = behavior_stack
def start(self):
if self in self.behavior_stack:
self.logr.error("start() already started - stack position %d" %
self.behavior_stack.index(self))
return
self.behavior_stack.append(self)
self.logr.info("start() %s" %
str([x.__class__.__name__
for x in self.behavior_stack]))
def cancel(self):
try:
n = self.behavior_stack.index(self)
self.behavior_stack.pop(n)
self.logr.info(
"cancel() %s" %
str([x.__class__.__name__ for x in self.behavior_stack]))
except ValueError:
self.logr.error("cancel() not started yet")
def __nonzero__(self):
return self.__bool__()
def __bool__(self):
return self in self.behavior_stack
def active(self):
return self.__bool__()
class TelemetryData(object):
def __init__(self):
self._telemetry_cb = None
self.logr = LagerLogger("TelemetryData")
self.logr.console(LagerLogger.DEBUG)
self._telemetry_lock = Lock()
self._telemetry = {
"charging": False,
"docked": False,
"lifted": False,
"dustbin": True,
"cleaning": False,
"battery_voltage": 0,
"sci_error": False,
"api_mode": "unknown",
"buttons": list()
}
def __getitem__(self, key):
if key not in self._telemetry.keys():
raise Exception("MemberValue not found in TelemetryData: %s" % key)
with self._telemetry_lock:
return copy.copy(self._telemetry[key])
def __setitem__(self, key, value):
if key not in self._telemetry.keys():
raise Exception("MemberValue not found in TelemetryData: %s" % key)
# print key,value
with self._telemetry_lock:
self._telemetry[key] = copy.copy(value)
# value = copy.copy(self._telemetry)
# print value
try:
if self._telemetry_cb:
self._telemetry_cb(self)
except Exception as e:
self.logr.error("%s: %s" % (str(type(e)), e))
def set_telemetry_update_cb(self, fun):
self._telemetry_cb = fun
def get_ros_msg(self):
""" returns the ros Telemetry message """
t = Telemetry()
with self._telemetry_lock:
for key, value in self._telemetry.items():
t.__setattr__(key, value)
return t
class VacuumState(
gen_enum_class("VacuumStateEnum", "idle", "cleaning", "spot_cleaning",
"docking", "docked", "pausing")):
def __init__(self):
super(VacuumState, self).__init__()
self.logr = LagerLogger()
self.logr.console(LagerLogger.DEBUG)
self.callbacks = list()
def set(self, state):
old = super(VacuumState, self).get()
super(VacuumState, self).set(state)
if not old:
old = "unknown"
self.logr.info("%s >> %s" % (old, state))
if self.callbacks:
for cb in self.callbacks:
try:
cb(copy.copy(self))
except Exception as e:
self.logr.error(e)
class BehaviorStack(list):
def __init__(self):
super(BehaviorStack, self).__init__()
self.logr = LagerLogger("BehaviorStack")
self.logr.console(LagerLogger.DEBUG)
def __getattr__(self, name):
try:
return super(BehaviorStack, self).__getattr__(name)
except AttributeError:
self.logr.debug("Trying to call '%s' from the stack" % name)
if len(self) == 0:
self.logr.error(
"Could not call '%s', no behaviors are on the stack. %s: %s" %
(name, str(type(e)), e))
return
for i in range(1, len(self) + 1):
try:
return self[-i].__getattribute__(name)
except IndexError as e:
self.logr.debug(
"Could not call '%s', no behaviors are on the stack. %s: %s"
% (name, str(type(e)), e))
except AttributeError as e:
self.logr.debug("Active behavior: %s" % e)
except Exception as e:
self.logr.debug("Could not call '%s' on behavior. %s: %s" %
(name, str(type(e)), e))
self.logr.warn(
"Behavior '%s' undefined for %s, lowering the bar." %
(name, self[-i].__class__.__name__))
self.logr.error("Behavior '%s' undefined for all robot behaviors" %
name)
return BehaviorStack.__noop__
@staticmethod
def __noop__(*args, **kwargs):
pass
class APIButtonPress(object):
def __init__(self, button_name, button_tracker):
self.button_name = button_name
self.button_tracker = button_tracker
self.logr = LagerLogger("APIButtonPress(%s)" % button_name)
self.logr.console(LagerLogger.DEBUG)
self._waiting_for_event = False
self._lock = Lock()
self._timeout = 40 # Should work out to be a full two seconds
def __call__(self, button_fun):
# Notify button tracker to give us the next instance of the following command
if not self._lock.acquire(False):
self.logr.fatal("Cant acquire lock!")
return
self._waiting_for_event = True
try:
self.button_tracker.consume_event(self.button_name, self.button_cb)
except Exception as e:
self.logr.error(e)
self._lock.release()
return
# Press button over API
button_fun()
# Wait to consume the button press
timeout = 0
while self._waiting_for_event:
if timeout >= self._timeout:
self.logr.error("Timeout waiting to hear button")
self._waiting_for_event = False
self.button_tracker.cancel_consume(self.button_name)
break
timeout += 1
time.sleep(.05)
self._lock.release()
def button_cb(self):
self._waiting_for_event = False
class Loggerator(object):
def __init__(self, ros, config):
self.logger = LagerLogger("Loggerator")
self.logger.console(LagerLogger.DEBUG)
self.ros = ros
if not "logging" in config:
self.logger.error("!!!!! NO LOGGING DEFINED !!!!!")
self.publishers = dict()
try:
topics = [
x.strip() for x in config["logging"]["topics"].split(",")
]
self.logger.info("Configuring logging on: %s" % topics)
for topic in topics:
try:
if config["logging"][topic] == "string":
topic_type = String
elif config["logging"][topic] == "bool":
topic_type = Bool
else:
topic_type = Bool
except:
topic_type = Bool
self.publishers[topic] = self.ros.Publisher(
"/logging/%s/%s" % (config["robot"]["name"], topic),
topic_type,
queue_size=10)
except:
self.logger.error(
"!!!!!!!!!!Error while parsing logging config!!!!!!!!!")
def event(self, name, value=None):
if value is None:
value = True
self.logger.debug("LOGGING: %s: %s" % (name, str(value)))
self.publishers[name].publish(Bool(value))
class RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("controller")
self.logger.console(LagerLogger.DEBUG)
self.telemetry_pub = self.ros.Publisher("%s/telemetry" %
self.robot_name,
Telemetry,
latch=True,
queue_size=10)
self.phonebutton_pub = self.ros.Publisher("%s/phonereply" %
self.robot_name,
PhoneReply,
latch=True,
queue_size=10)
self.ros_cmd_sub = self.ros.Subscriber("%s/cmd" % self.robot_name,
String, self.ros_cmd_cb)
if config.has("stage",
"enabled") and config["stage"]["enabled"] == "True":
self.logger.warn("using stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.info("Using hardware")
from vacuum_interfaces.roomba500_interface import Roomba500Interface
self.robot = Roomba500Interface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.robot.reg_sensor_cb(
"charging_sources_available", self.docked_cb, lambda old, new:
(not old["base"] == new["base"]) and new["base"])
self.robot.reg_dustbin_cb(self.dustbin_cb)
self.robot.reg_buttons_cb(self.hw_buttons_cb)
self.robot.reg_vacuum_state_cb(self.vacuum_state_cb)
self._telemetry_timer = Timer(
5, lambda x=self.robot._telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
self.behavior_stack = BehaviorStack()
self.behavior_normal = NormalBehavior(self)
self.behavior_dustbin = DustbinBehavior(self)
self.behavior_reset = ResetBehavior(self)
self.behavior_normal.start()
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
super(RobotController, self).initialize()
self.robot.passive()
self.logger.info("Robot Initialized")
self.robot.robot.set_song(0, [(79, 4), (78, 4), (76, 4), (62, 16),
(0, 32), (91, 8), (91, 8), (91, 8),
(91, 8), (91, 8), (91, 8), (91, 8),
(91, 8)])
self.robot.robot.set_song(1, [(79, 4), (78, 4), (76, 4), (62, 16),
(0, 32), (90, 4), (91, 4), (93, 4),
(0, 4), (90, 4), (91, 4), (93, 4)])
self.robot.robot.set_song(2, [(84, 4)])
def shutdown(self):
""" Runs when the robot controller is shutting down """
self.logger.warning("Shutting down controller")
super(RobotController, self).shutdown()
self._telemetry_timer.cancel()
if self.behavior_dustbin:
self.behavior_dustbin.cancel()
if self.behavior_reset:
self.behavior_reset.cancel()
self.behavior_normal.cancel()
self.robot.terminate()
def reset_controller(self):
self.logger.warn("Resetting controller")
if self.behavior_dustbin:
self.behavior_dustbin.cancel()
if self.behavior_reset:
self.behavior_reset.cancel()
self.behavior_normal.cancel()
self.behavior_normal.start()
super(RobotController, self).reset_controller()
def start_run(self):
self.logger.info("starting run")
# We assume that the robot needs
# [db] removed because it undoes the reset behavior on boot
# self.robot.passive()
if self.phone_link.is_stealth():
# [db] this is a hack to work around the robotlink software showing
# popups even when visibility is set to false
self.logger.info(
"Shutting down bluetooth because we are in stealth")
self.phone_link.disable()
if self.behavior_profile.is_disabled():
# phone link should already be set to invisible, this is redundant
self.phone_link.set_visibility(False)
self.robot.passive()
self.phone_link.disable()
self.logger.info("Ignoring start command")
elif self.behavior_profile.is_easy():
self.set_stack_idle()
self.controller_state.set_running()
self.robot.passive()
self.phone_link.set_visibility(True)
elif self.behavior_profile.is_help():
self.behavior_reset.start()
self.controller_state.set_running()
self.phone_link.set_visibility(True)
else:
self.logger.error("Received unexpected behavior profile")
return
################################################################################
def telemetry_cb(self, values):
""" Sends telemetry information """
t = Telemetry()
t.battery_voltage = values["battery_voltage"]
t.lifted = values["lifted"]
t.charging = values["charging"]
t.docked = values["docked"]
t.dustbin = values["dustbin"]
t.cleaning = values["cleaning"]
t.buttons = values["buttons"]
t.sci_error = values["sci_error"]
t.api_mode = values["api_mode"]
self.telemetry_pub.publish(t)
def vacuum_state_cb(self, vacuum_state):
""" Listen to state from the robot and set the phone response """
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.behavior_stack.vacuum_state_cb(vacuum_state)
# If the robot is supposed to be paused and gets jostled causing it to
# turn on, make it stop
elif self.controller_state.is_standby(
) or self.controller_state.is_ready():
if vacuum_state.is_cleaning():
self.hw_pause()
def hw_buttons_cb(self, buttons):
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.behavior_stack.hw_buttons_cb(buttons)
def dustbin_cb(self, detected):
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.behavior_stack.dustbin_cb(detected)
def ros_cmd_cb(self, data):
""" Receive commands from ROS """
self.logger.debug("ros_cmd_cb(%s)" % data.data)
if data.data == "sci":
self.robot.keep_alive()
elif data.data == "baud":
self.robot.baud_fix()
elif data.data == "play_song0":
self.robot.robot.play_song(0)
elif data.data == "play_song1":
self.robot.robot.play_song(1)
# elif data.data == "bump":
# self.robot.robot.sci.wake()
else:
self.behavior_stack.ros_cmd_cb(data)
def cmd_dock(self):
""" Send the robot to its docking station """
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.pub_phone_buttons("dock")
self.behavior_stack.cmd_dock()
def docked_cb(self):
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.behavior_stack.docked_cb()
def cmd_pause(self):
""" Tell the robot to pause what it is doing - this is a behavior so what this means is up to interpertation"""
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.behavior_stack.cmd_pause()
def hw_pause(self):
""" Tell the robot hardware to stop moving """
self.logger.debug("hw_pause()")
self.robot.pause()
def cmd_clean(self):
""" Tell the robot to clean the floor """
if self.controller_state.is_running(
) or self.controller_state.is_paused():
self.pub_phone_buttons("clean")
self.behavior_stack.cmd_clean()
def cmd_noop(self):
self.logger.debug("cmd_noop()")
def pub_phone_buttons(self, button=None):
data = PhoneReply()
if button:
data.buttons.append(button)
self.phonebutton_pub.publish(data)
data = None
data = PhoneReply()
self.phonebutton_pub.publish(data)
####################$$$$$$$$$$$$$$$$$$$$$$$$$$####################
def set_stack_idle(self):
self.logger.debug("set_stack_idle()")
self.conv_stack.clear()
docked = self.robot.is_docked()
state = "Docked" if docked else "Idle"
m = self.conv_stack.add_message("%s. Waiting for Job." % state)
m.add_response("Clean", self.cmd_clean)
if not docked:
m.add_response("Dock", self.cmd_dock)
self.phone_link.set_state("ok", update=False)
self.phone_link.set_progression(self.conv_stack)
def set_stack_cleaning(self, update=True):
self.logger.debug("set_stack_cleaning()")
self.conv_stack.clear()
m = self.conv_stack.add_message("Received command: Cleaning")
m.add_response("Dock", self.cmd_dock)
m.add_response("Pause", self.cmd_pause)
self.phone_link.set_state("safe", update=False)
self.phone_link.set_progression(self.conv_stack, update=update)
def set_stack_docking(self):
self.logger.debug("set_stack_docking()")
self.conv_stack.clear()
m = self.conv_stack.add_message("Looking for base station...")
m.add_response("Clean", self.cmd_clean)
m.add_response("Pause", self.cmd_pause)
self.phone_link.set_state("ok", update=False)
self.phone_link.set_progression(self.conv_stack)
class NeatoInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = NeatoDriver(self._dev_path)
self.robot.passive()
self._callbacks = dict()
self.reg_sensor_cb(
"buttons", self._buttons_cb,
lambda old, new: not set([k for k, v in old.items() if v]) == set(
[k for k, v in new.items() if v]))
self._telemetry = TelemetryData()
self._sensor_update_timer = Timer(.001, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except Exception as e:
self.logr.error(e)
return
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, (cmp_fun, cb) in self._callbacks.items():
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def get_sensor(self, key):
return self.robot.get_sensor(key)
def passive(self):
self.robot.passive()
def is_docked(self):
#return bool(self.robot.get_sensor("ExtPwrPresent")) and not\
# bool(self.robot.get_sensor("SNSR_DC_JACK_CONNECT"))
voltage = self.robot.get_sensor("VExtV")
return (voltage > 20 and voltage < 23.5)
def is_charging(self):
#return bool(self.robot.get_sensor("ExtPwrPresent"))
return (self.robot.get_sensor("VExtV") > 20)
def is_cleaning(self):
return False
def dustbin_in(self):
return bool(self.robot.get_sensor("SNSR_DUSTBIN_IS_IN"))
def _update_telemetry(self):
charging = self.is_charging()
if not charging == self._telemetry["charging"]:
self._telemetry["charging"] = charging
# See if we are on the dock
# sources = self.robot.get_sensor("charging_sources_available")
docked = self.is_docked()
if not docked == self._telemetry["docked"]:
self._telemetry["docked"] = docked
# see if the robot has been picked up
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
dustbin = self.dustbin_in()
if not dustbin == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = dustbin
# Check if the sweeper brush is running (that means we are cleaning)
cleaning = False
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Check the status of button presses
buttons = self.robot.get_sensor("buttons")
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
# Check the robot voltage
# voltage = self.robot.get_sensor("BatteryVoltageInmV")
voltage = self.robot.get_sensor("VBattV")
if voltage is None:
voltage = 0
#voltage /= 1000
if abs(voltage - self._telemetry["battery_voltage"]) >= .2: #40.0 :
self._telemetry["battery_voltage"] = voltage
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._sensor_update_timer.cancel()
time.sleep(.5)
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
def _buttons_cb(self):
# Save the last button or buttons to be pressed.
# Note: This gets called when buttons are released too, so make
# sure something is actually pressed we didn't just release
buttons = self.robot.get_sensor("buttons")
new_buttons = [k for k, v in buttons.items() if v]
if new_buttons:
self._lastbuttons = new_buttons
class PayloadPipe(object):
"""Interacts with the bga layer to send and receive information."""
def __init__(self):
"""Basic initialization."""
self._batch_index = 0
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._lower_level = BgSerial(self)
self._lower_level.subscribe_to_connect(self._on_notify_connect_cb)
self._lower_level.subscribe_to_disconnect(self._on_disconnect_cb)
self._lower_level.subscribe_to_transfer(self._on_transfer_cb)
self._lower_level.subscribe_to_receive_batch(self._on_receive_batch_cb)
self._last_batch_sent = None
self._who_connected = None
self._current_payload_out = None
self._next_payload_out = None
self._payload_builder_in = None
self._on_receive_cbs = []
self._on_connect_cbs = []
self._on_disconnect_cbs = []
self._spin_timer = Timer(0.01, self._lower_level.spin_once)
self._write_in_progress_lock = Lock()
self._next_payload_lock = Lock()
self._payload_builder_in_lock = Lock()
def write(self, raw_payload, who=None):
"""Writes payload to the _current_payload_out which is
automatically transferred once the client requests the size of
the payload.
"""
with self._next_payload_lock:
self._next_payload_out = Payload(raw_payload)
def send_next_payload(self):
"""Uses the bga layer to send the batches in the payload."""
if self._current_payload_out != None:
for batch in (self._current_payload_out.segmented_payload()):
self._last_batch_sent = batch
if self._lower_level.write_batch(batch) == False:
# Then the payload's transfer failed to write because there
# was a disconnect. So we don't pop the last message from
# the queue.
self.logr.error("Disconnect occurred in the middle of a write.")
self._write_in_progress_lock.release()
return
self._current_payload_out = None
self._write_in_progress_lock.release()
def start_spinning(self):
"""Starts a thread to processes the bga-layer's callback queue."""
self._spin_timer.start()
def stop_spinning(self):
"""Stops processing the bga-layer's callback queue."""
self._spin_timer.cancel()
def set_visibility(self, visibility):
"""Signals the bga-layer to enable/disable its visibility to clients at
a software level."""
self._lower_level.set_adv_visibility_data(visibility)
# expects cache to be a string
def set_adv_cache_data(self, cache):
cache_hex = "%08x" % (binascii.crc32(cache) & 0xFFFFFFFF)
self.logr.info("NEW CHECKSUM set_adv_cache_data(%s)" % cache_hex)
# import traceback
# traceback.print_stack()
byte_array = []
while len(cache_hex) != 0:
byte_array.insert(0, int(cache_hex[-2:], 16))
cache_hex = cache_hex[:-2]
self._lower_level.set_adv_cache_data(byte_array)
def get_last_batch_sent(self):
"""Required by the lower-level when they are resending lost packets."""
return self._last_batch_sent
def prepare_next_payload(self):
"""Called when a read is performed on the lower level."""
with self._next_payload_lock:
permission_acquired = self._write_in_progress_lock.acquire(False)
if (self._next_payload_out != None and permission_acquired):
self._current_payload_out = copy.deepcopy(self._next_payload_out)
return permission_acquired;
def get_payload_size(self):
"""Retrieves the number of frames that make up the current payload."""
if self._current_payload_out != None:
return self._current_payload_out.size()
else:
return 0
def total_frames_remaining(self, who):
"""Retrieves the amount of frames needed to complete the newest payload
from 'who'."""
with self._payload_builder_in_lock:
if self._payload_builder_in != None:
return self._payload_builder_in.final_size() - self._payload_builder_in.size()
else:
return 0
def enable(self):
"""Signals the bg_serial layer to stop advertising."""
self._lower_level._start_advertising()
def disable(self):
"""Signals the bg_serial layer to stop advertising."""
self._lower_level.stop()
def _on_notify_connect_cb(self, who):
""" This begins the client reading data from the phone """
self.logr.debug("%s sent notify connect" % who)
self._who_connected = who
for on_connect_cb in self._on_connect_cbs:
on_connect_cb(who)
def _on_disconnect_cb(self):
self.logr.debug("disconnected")
for on_disconnect_cb in self._on_disconnect_cbs:
on_disconnect_cb()
def _on_transfer_cb(self, who, payload_size):
with self._payload_builder_in_lock:
self.logr.debug("Phone wants to send data of size %d" % payload_size)
self._payload_builder_in = PayloadBuilder(payload_size)
sys.stdout.flush()
def _on_receive_batch_cb(self, who, batch):
with self._payload_builder_in_lock:
self.logr.debug("Received batch - %s" % binascii.crc32(str(batch)))
sys.stdout.flush()
if not self._payload_builder_in:
self.logr.error("F**K ME SIDEWAYS")
sys.stdout.flush()
return
self._payload_builder_in.add_batch(batch)
if self._payload_builder_in.is_complete():
# Tell bg_serial to not accept batch data anymore
self._lower_level._reading_batch = False
self.logr.debug("Payload Complete - running callbacks")
sys.stdout.flush()
json_obj = self._payload_builder_in.build()
threads = list()
self.logr.debug("Starting %d callback threads" % len(self._on_receive_cbs))
for receive_cb in self._on_receive_cbs:
# receive_cb(json_obj)
threads.append(threaderized(receive_cb, json_obj))
for t in threads:
t.join(3.0)
if t.is_alive():
self.logr.warn("Long running callback, can't wait!")
self._payload_builder_in = None
self.logr.debug("Client data send completed - payloadbuilder = none")
self.logr.info("Signaling allclear to phone")
sys.stdout.flush()
def subscribe_to_receive(self, receive_cb):
self._on_receive_cbs.append(receive_cb)
def subscribe_to_connect(self, on_connect_cb):
self._on_connect_cbs.append(on_connect_cb)
def subscribe_to_disconnect(self, on_disconnect_cb):
self._on_disconnect_cbs.append(on_disconnect_cb)
class BgSerial(object):
pp = pprint.PrettyPrinter(indent=4)
def __init__(self, payload_pipe, dev='/dev/bluegiga', debug=False):
self._lock = Lock()
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._attributes_status = 0
self._ble = bglib.BGLib()
self._ble.packet_mode = False
self._connected_addr = None
self._conn_handle = None
self._notified_handle = None
self.adv_data = list(
ADV_DATA) # clones list, initially set to be hidden
# SHOULD BE STATELESS, WHAT IS THIS???????????
# Used to retrieve:
# (for writing)
# - The total payload size, we need this at the low-level because the
# client requests this before every payload transfer.
# - The last batch sent. We need this because we need to resend missed
# packets, but we have no knowledge of the payload.
#
# (for receiving)
# - sends the received messages
self._payload_pipe = payload_pipe
# SHOULD BE STATELESS, WHAT IS THIS???????????
# Used to signal when the (bga server -> client) client signals the
# entire batch was received successfully.
self._is_batch_transfer_complete = False
self._reading_batch = False
self._batch_builder = None
self._ser = serial.Serial(port=dev, baudrate=115200, timeout=1)
self._ser.flushInput()
self._ser.flushOutput()
self._on_connect_cbs = []
self._on_transfer_cbs = []
self._on_receive_batch_cbs = []
self._on_disconnect_cbs = []
#self._ble.ble_evt_attributes_user_read_request += self._read_cb
#self._ble.ble_evt_attributes_value += self._write_cb
#self._ble.ble_evt_attributes_status += self._attributes_status_cb
#self._ble.ble_evt_connection_disconnected += self._disconnect_cb
#self._ble.ble_evt_connection_status += self._conn_interval_cb
self._ble.ble_evt_attributes_user_read_request += lambda x, y: threaderized(
self._read_cb, x, y)
self._ble.ble_evt_attributes_value += lambda x, y: threaderized(
self._write_cb, x, y)
self._ble.ble_evt_attributes_status += lambda x, y: threaderized(
self._attributes_status_cb, x, y)
self._ble.ble_evt_connection_disconnected += lambda x, y: threaderized(
self._disconnect_cb, x, y)
self._ble.ble_evt_connection_status += lambda x, y: threaderized(
self._conn_interval_cb, x, y)
# if debug == True:
# self._setup_debug_callbacks()
self.stop()
self._start_advertising()
def _start_advertising(self):
"""Sets necessary attributes for advertising and begins advertising."""
self._ble.send_command(
self._ser,
self._ble.ble_cmd_gap_set_adv_parameters(ADV_MIN, ADV_MAX, 7))
time.sleep(bga_constants.NOTIFY_RATE)
self.update_adv_data()
self.logr.debug("Entering advertisement mode...")
sys.stdout.flush()
self._ble.send_command(self._ser,
self._ble.ble_cmd_gap_set_mode(0x04, 0x02))
time.sleep(bga_constants.NOTIFY_RATE)
# takes a list of 4 bytes, with each byte represented in hex.
def set_adv_cache_data(self, cache):
if len(cache) == 4:
for i in range(0, 4):
self.adv_data[CACHE_BIT_INDEX + i] = cache[i]
self.update_adv_data()
def set_adv_visibility_data(self, visible):
self.adv_data[VISIBILITY_BIT_INDEX] = int(not visible)
self.update_adv_data()
def update_adv_data(self):
self._ble.send_command(
self._ser, self._ble.ble_cmd_gap_set_adv_data(0, self.adv_data))
time.sleep(bga_constants.NOTIFY_RATE)
# try:
# self.check_activity(self._ser, 1)
# except serial.SerialException:
# print "Warning: Unable to set advertising data at this time."
def spin(self):
while True:
self.spin_once()
def spin_once(self):
# spinner.spin()
self.check_activity(self._ser)
# Timeout - are we missing frames?
if self._reading_batch and self._batch_builder and self._batch_builder.is_waiting(
):
self._request_missed_frames_or_proceed()
def check_activity(self, *args):
with self._lock:
self._ble.check_activity(*args)
def write_batch(self, batch):
"""Writes a batch out to the currently connected client. Blocks
the thread until the client signals the entire batch was
received successfully.
"""
self.logr.debug("Writing Batch")
sys.stdout.flush()
for frame in batch:
if self._notified_handle != None:
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_write(self._notified_handle,
0, frame))
#self._ble.check_activity(self._ser)
time.sleep(bga_constants.NOTIFY_RATE * 2)
self.logr.debug("Waiting for transfer to complete")
sys.stdout.flush()
while (self._is_batch_transfer_complete == False
and self._connected_addr != None):
time.sleep(bga_constants.NOTIFY_RATE)
#self._ble.check_activity(self._ser)
self.logr.debug("transfer to complete")
sys.stdout.flush()
self._is_batch_transfer_complete = False
return self._connected_addr != None
def stop(self):
"""Stops connections, advertising, and scanning."""
# Disconnects
self._ble.send_command(self._ser,
self._ble.ble_cmd_connection_disconnect(0))
time.sleep(bga_constants.NOTIFY_RATE)
# Stops advertising mode
self._ble.send_command(self._ser, self._ble.ble_cmd_gap_set_mode(0, 0))
time.sleep(bga_constants.NOTIFY_RATE)
# Stops scanning
self._ble.send_command(self._ser,
self._ble.ble_cmd_gap_end_procedure())
time.sleep(bga_constants.NOTIFY_RATE)
def _read_cb(self, sender, args):
""""When the client requests to read from the server, this is fired."""
self.logr.debug("Got Read Callback")
sys.stdout.flush()
if args['handle'] == bga_constants.PKT_OUT_HANDLE:
self.logr.debug("_read_cb() PKT_OUT_HANDLE")
prepare_permission = self._payload_pipe.prepare_next_payload()
payload_size = self._payload_pipe.get_payload_size()
if prepare_permission and payload_size > 0:
self.logr.debug("_read_cb() Sending payload size")
sys.stdout.flush()
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_user_read_response(
args['connection'], 0,
[ord(c) for c in struct.pack("!I", payload_size)]))
#self.check_activity(self._ser, 1)
time.sleep(bga_constants.NOTIFY_RATE * 2)
self._payload_pipe.send_next_payload()
elif not prepare_permission:
self.logr.debug("_read_cb() tried to read during a transfer")
sys.stdout.flush()
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_user_read_response(
args['connection'], 0, [0, 0, 0, -1]))
time.sleep(bga_constants.NOTIFY_RATE)
else:
self.logr.debug("_read_cb() Doing the other thing")
sys.stdout.flush()
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_user_read_response(
args['connection'], 0, [0, 0, 0, 0]))
time.sleep(bga_constants.NOTIFY_RATE)
def _write_cb(self, sender, args):
"""When the client writes to the server, this is fired."""
# Acknowledges any write_with_response messages.
sys.stdout.flush()
if args['reason'] == 2:
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_user_write_response(
self._conn_handle, 0))
time.sleep(bga_constants.NOTIFY_RATE)
# Stores packets sent from the client.
if args['handle'] == bga_constants.PKT_IN_HANDLE:
if not self._reading_batch:
self.logr.error(
" ~!~!~!~!~ This should never happen ~!~!~!~!~!~")
return
# when batch transfer complete, notify the client
if self._batch_builder.add_frame(args['value']):
self._request_missed_frames_or_proceed()
# Processes a missed_pkt request (from the client)
elif args['handle'] == bga_constants.MISSED_PKT_OUT_HANDLE:
if self._payload_pipe.get_payload_size() > 0:
missed_frames = ''
for b in args['value']:
missed_frames += bin(b)[2:].zfill(8)
self._is_batch_transfer_complete = \
self._resend_missed_frames(missed_frames)
# Reads the size of an upcoming payload (from the client).
elif args['handle'] == bga_constants.PKT_IN_COUNT_HANDLE:
# converts a little endian hex to decimal
total = 0
byte_mult = 1
for b in args['value']:
if b == 0:
continue
else:
total += b * byte_mult
byte_mult <<= 8
self._reading_batch = True
self._batch_builder = BatchBuilder(total)
for on_transfer_cb in self._on_transfer_cbs:
on_transfer_cb(self._connected_addr, total)
def _resend_missed_frames(self, missed_frames):
if len(missed_frames) > 0 and missed_frames[0] == '0':
return True
last_batch = self._payload_pipe.get_last_batch_sent()
frame_index = 0
while frame_index < len(last_batch):
if (missed_frames[frame_index + 1] == '1'):
bit_enabled_missed_frame = last_batch[frame_index]
bit_enabled_missed_frame[0] += 128
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_write(
self._notified_handle, 0, bit_enabled_missed_frame))
time.sleep(bga_constants.NOTIFY_RATE)
frame_index += 1
return False
def _attributes_status_cb(self, sender, args):
"""Fired when we a client subscribes through notify/indicate."""
if args['handle'] == bga_constants.PKT_OUT_HANDLE:
if args['flags'] == 0:
self._notified_handle = None
elif args['flags'] == bga_constants.NOTIFY_FLAG:
self._notified_handle = args['handle']
for on_connect_cb in self._on_connect_cbs:
on_connect_cb(self._connected_addr)
else:
self.logr.error("invalid attribute status flag")
return
self._attributes_status = args['flags']
elif args['handle'] == bga_constants.MISSED_PKT_IN_HANDLE:
if args['flags'] == bga_constants.INDICATE_FLAG:
self._in_missed_frame_handle = args['handle']
def _request_missed_frames_or_proceed(self):
missed_frames_pkt = self._batch_builder.get_missed_frames_pkt()
if missed_frames_pkt[0] == False:
self.logr.debug("Batch Complete")
batch = self._batch_builder.build()
for on_receive_batch_cb in self._on_receive_batch_cbs:
on_receive_batch_cb(self._connected_addr, batch)
total_frames_remaining = self._payload_pipe.total_frames_remaining(
self._connected_addr)
if total_frames_remaining > 0:
self._batch_builder = BatchBuilder(total_frames_remaining)
else:
self._batch_builder = None
self._ble.send_command(
self._ser,
self._ble.ble_cmd_attributes_write(
self._in_missed_frame_handle, 0,
[ord(num) for num in list(missed_frames_pkt.tobytes())]))
time.sleep(bga_constants.NOTIFY_RATE)
def _disconnect_cb(self, sender, args):
"""Called when a connection ends, re-enables peripheral advertising."""
self._ble.send_command(self._ser,
self._ble.ble_cmd_gap_set_mode(0x04, 0x02))
time.sleep(bga_constants.NOTIFY_RATE)
self._conn_handle = None
self._connected_addr = None
self._notified_handle = None
for disconnect_cb in self._on_disconnect_cbs:
disconnect_cb()
def _conn_interval_cb(self, sender, args):
"""Suggests low connection interval, the central ultimately decides."""
low = 6
high = 10
if args['conn_interval'] > high or args['conn_interval'] < low:
self._ble.send_command(
self._ser,
self._ble.ble_cmd_connection_update(0, low, high, 0, 2000))
time.sleep(bga_constants.NOTIFY_RATE)
self._conn_handle = args['connection']
str_address = BgSerial._address_arr_to_str(args['address'])
if self._connected_addr != str_address:
self.logr.debug("Connected (actually)")
self._connected_addr = str_address
@classmethod
def _debug_args_cb(cls, sender, args):
"""Prints all info about a RSP/EVT in a JSON-like format."""
cls.pp.pprint(args)
def subscribe_to_connect(self, on_connect_cb):
if callable(on_connect_cb):
self._on_connect_cbs.append(on_connect_cb)
else:
raise TypeError('on_connect callback provided was not a function.')
def subscribe_to_transfer(self, on_transfer_cb):
if callable(on_transfer_cb):
self._on_transfer_cbs.append(on_transfer_cb)
else:
raise TypeError(
'on_transfer callback provided was not a function.')
def subscribe_to_receive_batch(self, on_receive_batch_cb):
if callable(on_receive_batch_cb):
self._on_receive_batch_cbs.append(on_receive_batch_cb)
else:
raise TypeError(
'on_receive_batch callback provided was not a function.')
def subscribe_to_disconnect(self, on_disconnect_cb):
if callable(on_disconnect_cb):
self._on_disconnect_cbs.append(on_disconnect_cb)
else:
raise TypeError(
'on_disconnect callback provided was not a function.')
@staticmethod
def _address_arr_to_str(address):
str_address = ''
for byte in address:
str_byte = str(hex(byte))[2:]
if len(str_byte) == 1:
str_byte = '0' + str_byte
if str_address != '':
str_byte += ':'
str_address = str_byte + str_address
return str_address
def _setup_debug_callbacks(self):
"""Registers the debug callback for all RSPs and EVTs."""
self._ble.ble_rsp_system_reset += self._debug_args_cb
self._ble.ble_rsp_system_hello += self._debug_args_cb
self._ble.ble_rsp_system_address_get += self._debug_args_cb
self._ble.ble_rsp_system_reg_write += self._debug_args_cb
self._ble.ble_rsp_system_reg_read += self._debug_args_cb
self._ble.ble_rsp_system_get_counters += self._debug_args_cb
self._ble.ble_rsp_system_get_connections += self._debug_args_cb
self._ble.ble_rsp_system_read_memory += self._debug_args_cb
self._ble.ble_rsp_system_get_info += self._debug_args_cb
self._ble.ble_rsp_system_endpoint_tx += self._debug_args_cb
self._ble.ble_rsp_system_whitelist_append += self._debug_args_cb
self._ble.ble_rsp_system_whitelist_remove += self._debug_args_cb
self._ble.ble_rsp_system_whitelist_clear += self._debug_args_cb
self._ble.ble_rsp_system_endpoint_rx += self._debug_args_cb
self._ble.ble_rsp_system_endpoint_set_watermarks += self._debug_args_cb
self._ble.ble_rsp_flash_ps_defrag += self._debug_args_cb
self._ble.ble_rsp_flash_ps_dump += self._debug_args_cb
self._ble.ble_rsp_flash_ps_erase_all += self._debug_args_cb
self._ble.ble_rsp_flash_ps_save += self._debug_args_cb
self._ble.ble_rsp_flash_ps_load += self._debug_args_cb
self._ble.ble_rsp_flash_ps_erase += self._debug_args_cb
self._ble.ble_rsp_flash_erase_page += self._debug_args_cb
self._ble.ble_rsp_flash_write_words += self._debug_args_cb
self._ble.ble_rsp_attributes_write += self._debug_args_cb
self._ble.ble_rsp_attributes_read += self._debug_args_cb
self._ble.ble_rsp_attributes_read_type += self._debug_args_cb
self._ble.ble_rsp_attributes_user_read_response += self._debug_args_cb
self._ble.ble_rsp_attributes_user_write_response += self._debug_args_cb
self._ble.ble_rsp_connection_disconnect += self._debug_args_cb
self._ble.ble_rsp_connection_get_rssi += self._debug_args_cb
self._ble.ble_rsp_connection_update += self._debug_args_cb
self._ble.ble_rsp_connection_version_update += self._debug_args_cb
self._ble.ble_rsp_connection_channel_map_get += self._debug_args_cb
self._ble.ble_rsp_connection_channel_map_set += self._debug_args_cb
self._ble.ble_rsp_connection_features_get += self._debug_args_cb
self._ble.ble_rsp_connection_get_status += self._debug_args_cb
self._ble.ble_rsp_connection_raw_tx += self._debug_args_cb
self._ble.ble_rsp_attclient_find_by_type_value += self._debug_args_cb
self._ble.ble_rsp_attclient_read_by_group_type += self._debug_args_cb
self._ble.ble_rsp_attclient_read_by_type += self._debug_args_cb
self._ble.ble_rsp_attclient_find_information += self._debug_args_cb
self._ble.ble_rsp_attclient_read_by_handle += self._debug_args_cb
self._ble.ble_rsp_attclient_attribute_write += self._debug_args_cb
self._ble.ble_rsp_attclient_write_command += self._debug_args_cb
self._ble.ble_rsp_attclient_indicate_confirm += self._debug_args_cb
self._ble.ble_rsp_attclient_read_long += self._debug_args_cb
self._ble.ble_rsp_attclient_prepare_write += self._debug_args_cb
self._ble.ble_rsp_attclient_execute_write += self._debug_args_cb
self._ble.ble_rsp_attclient_read_multiple += self._debug_args_cb
self._ble.ble_rsp_sm_encrypt_start += self._debug_args_cb
self._ble.ble_rsp_sm_set_bondable_mode += self._debug_args_cb
self._ble.ble_rsp_sm_delete_bonding += self._debug_args_cb
self._ble.ble_rsp_sm_set_parameters += self._debug_args_cb
self._ble.ble_rsp_sm_passkey_entry += self._debug_args_cb
self._ble.ble_rsp_sm_get_bonds += self._debug_args_cb
self._ble.ble_rsp_sm_set_oob_data += self._debug_args_cb
self._ble.ble_rsp_gap_set_privacy_flags += self._debug_args_cb
self._ble.ble_rsp_gap_set_mode += self._debug_args_cb
self._ble.ble_rsp_gap_discover += self._debug_args_cb
self._ble.ble_rsp_gap_connect_direct += self._debug_args_cb
self._ble.ble_rsp_gap_end_procedure += self._debug_args_cb
self._ble.ble_rsp_gap_connect_selective += self._debug_args_cb
self._ble.ble_rsp_gap_set_filtering += self._debug_args_cb
self._ble.ble_rsp_gap_set_scan_parameters += self._debug_args_cb
self._ble.ble_rsp_gap_set_adv_parameters += self._debug_args_cb
self._ble.ble_rsp_gap_set_adv_data += self._debug_args_cb
self._ble.ble_rsp_gap_set_directed_connectable_mode += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_config_irq += self._debug_args_cb
self._ble.ble_rsp_hardware_set_soft_timer += self._debug_args_cb
self._ble.ble_rsp_hardware_adc_read += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_config_direction += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_config_function += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_config_pull += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_write += self._debug_args_cb
self._ble.ble_rsp_hardware_io_port_read += self._debug_args_cb
self._ble.ble_rsp_hardware_spi_config += self._debug_args_cb
self._ble.ble_rsp_hardware_spi_transfer += self._debug_args_cb
self._ble.ble_rsp_hardware_i2c_read += self._debug_args_cb
self._ble.ble_rsp_hardware_i2c_write += self._debug_args_cb
self._ble.ble_rsp_hardware_set_txpower += self._debug_args_cb
self._ble.ble_rsp_hardware_timer_comparator += self._debug_args_cb
self._ble.ble_rsp_test_phy_tx += self._debug_args_cb
self._ble.ble_rsp_test_phy_rx += self._debug_args_cb
self._ble.ble_rsp_test_phy_end += self._debug_args_cb
self._ble.ble_rsp_test_phy_reset += self._debug_args_cb
self._ble.ble_rsp_test_get_channel_map += self._debug_args_cb
self._ble.ble_rsp_test_debug += self._debug_args_cb
self._ble.ble_evt_system_boot += self._debug_args_cb
self._ble.ble_evt_system_debug += self._debug_args_cb
self._ble.ble_evt_system_endpoint_watermark_rx += self._debug_args_cb
self._ble.ble_evt_system_endpoint_watermark_tx += self._debug_args_cb
self._ble.ble_evt_system_script_failure += self._debug_args_cb
self._ble.ble_evt_system_no_license_key += self._debug_args_cb
self._ble.ble_evt_flash_ps_key += self._debug_args_cb
self._ble.ble_evt_attributes_value += self._debug_args_cb
self._ble.ble_evt_attributes_user_read_request += self._debug_args_cb
self._ble.ble_evt_attributes_status += self._debug_args_cb
self._ble.ble_evt_connection_status += self._debug_args_cb
self._ble.ble_evt_connection_version_ind += self._debug_args_cb
self._ble.ble_evt_connection_feature_ind += self._debug_args_cb
self._ble.ble_evt_connection_raw_rx += self._debug_args_cb
self._ble.ble_evt_connection_disconnected += self._debug_args_cb
self._ble.ble_evt_attclient_indicated += self._debug_args_cb
self._ble.ble_evt_attclient_procedure_completed += self._debug_args_cb
self._ble.ble_evt_attclient_group_found += self._debug_args_cb
self._ble.ble_evt_attclient_attribute_found += self._debug_args_cb
self._ble.ble_evt_attclient_find_information_found += self._debug_args_cb
self._ble.ble_evt_attclient_attribute_value += self._debug_args_cb
self._ble.ble_evt_attclient_read_multiple_response += self._debug_args_cb
self._ble.ble_evt_sm_smp_data += self._debug_args_cb
self._ble.ble_evt_sm_bonding_fail += self._debug_args_cb
self._ble.ble_evt_sm_passkey_display += self._debug_args_cb
self._ble.ble_evt_sm_passkey_request += self._debug_args_cb
self._ble.ble_evt_sm_bond_status += self._debug_args_cb
self._ble.ble_evt_gap_mode_changed += self._debug_args_cb
self._ble.ble_evt_hardware_io_port_status += self._debug_args_cb
self._ble.ble_evt_hardware_soft_timer += self._debug_args_cb
self._ble.ble_evt_hardware_adc_result += self._debug_args_cb
self._ble.ble_evt_gap_scan_response += self._debug_args_cb
class RobotConfig(dict):
""" Loads arbitrary settings from a config file and from the command line.
Settings are stored as config['section']['setting'] = value
"""
# All names are automatically converted to lower case.
def __init__(self, *args):
dict.__init__(self, args)
self.logger = LagerLogger("RobotConfig")
self.logger.console(LagerLogger.DEBUG)
def _add_setting(self, section, setting, value):
""" Adds a setting to the RobotConfig """
# section = section.lower()
if section not in self.keys():
self[section] = dict()
self.logger.debug("Adding %s.%s: %s" % (section, setting, value))
self[section][setting] = value
def parse_config_file(self, filename):
""" Load arbitrary settings from config file"""
import ConfigParser
self.cfg_file = ConfigParser.ConfigParser()
self.cfg_file.optionxform = str # Make options case sensitive
path = os.path.dirname(os.path.realpath(__file__))
path = os.path.join(path, filename)
self.logger.debug("config path=%s" % path)
try:
self.cfg_file.readfp(open(path))
except IOError as e:
self.logger.error("Unable to load config file '%s'" % path)
exit(0)
for section in self.cfg_file.sections():
settings = self.cfg_file.options(section)
for setting in settings:
self._add_setting(section, setting,
self.cfg_file.get(section, setting))
def parse_args(self, unknown_args):
""" Parse arbitrary command line args """
arg_list = list()
for arg in unknown_args:
if arg.startswith(("-", "--")):
if "." not in arg:
raise Exception(
"All arguments must have a '.' in their name, like 'Robot.setting'"
)
arg_list.append(arg[2:])
parser.add_argument(arg, type=str)
opt_args = parser.parse_args(unknown_args)
for arg in arg_list:
section, setting = arg.split(".")
self.logger.debug("Adding %s, %s from cmd line" %
(section, setting))
self._add_setting(section, setting, opt_args.__getattribute__(arg))
def has(self, section, setting):
""" test if the config includes a value for section setting """
if section not in self.keys():
return False
if setting not in self[section].keys():
return False
return True
def verify(self):
""" Make sure we have at least the following settings"""
if "robot" not in self.keys():
raise Exception("No Section 'robot' in RobotConfig")
# if "name" not in self["robot"]:
# raise Exception("No robot.name specified in RobotConfig")
if "controller_file" not in self['robot']:
raise Exception(
"No robot.controller_file specified in RobotConfig")
# if "ros_master_uri" not in self['robot']:
# raise Exception("No robot.ros_master_uri specified in RobotConfig")
# if "bluegigga_dev" not in self['robot']:
# raise Exception("No robot.bluegigga_dev specified in RobotConfig")
if "robot_dev" not in self['robot']:
raise Exception("No robot.robot_dev specified in RobotConfig")
class BenderInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
#keep alive thread
self._keep_alive_timer = Timer(1, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def gpio_dustbin_cb(self, value):
""" update the dustbin status """
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
def get_sensor(self, key):
return self.robot.get_sensor(key)
def passive(self):
self.robot.passive()
def is_docked(self):
return self._telemetry["docked"]
def is_charging(self):
return self._telemetry["charging"]
def is_cleaning(self):
return self._telemetry["cleaning"]
def dock(self):
self.robot.dock()
def clean(self):
self.robot.clean()
def pause(self):
self.robot.pause()
def _keep_alive(self):
""" Keep alive timer callback. Throws a gpio pin up and down """
GPIO.output(DEVICE_DETECT, GPIO.LOW)
time.sleep(0.1)
GPIO.output(DEVICE_DETECT, GPIO.HIGH)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except BadDataLengthError as e:
self.logr.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
return
except DriverError as e:
self.logr.warn(e)
self._telemetry["sci_error"] = True
except Exception as e:
self.logri.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
# WHEN THIS HAPPENS - the robot seems to have turned off.
# On dirtdog this would happen when the person presses
# the power button while cleaning.
return
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, (cmp_fun, cb) in self._callbacks.items():
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def _update_telemetry(self):
# If sci error has been set (by poll_sensors_cb), undo it
if self._telemetry["sci_error"]:
self._telemetry["sci_error"] = False
# Compare charging status
charging = self.robot.get_sensor("charging_state")
charging = False if charging in ["not_charging", "error"] else True
if not charging == self._telemetry["charging"]:
self._telemetry["charging"] = charging
# See if we are on the dock
sources = self.robot.get_sensor("charging_sources_available")
if not sources["base"] == self._telemetry["docked"]:
self._telemetry["docked"] = sources["base"]
# see if the robot has been picked up
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
# Check if the sweeper brush is running (that means we are cleaning)
cleaning = self.robot.get_sensor("main_brush_current") >= 1
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Check the status of button presses
buttons = self.robot.get_sensor("buttons")
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
# Check the robot voltage
voltage = self.robot.get_sensor("voltage") / 1000.0
if abs(voltage - self._telemetry["battery_voltage"]) >= 0.1:
self._telemetry["battery_voltage"] = voltage
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._keep_alive_timer.cancel()
self.logr.info("keep alive killed")
self._sensor_update_timer.cancel()
self.logr.info("killed sensor update timer")
time.sleep(1)
GPIO.cleanup()
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
class PhoneLink(object):
def __init__(self):
# Robot Data
self._name = "unknown"
self._model = "unknown"
self._state = "dangerous"
self._progression = None
self.logger = LagerLogger("")
self.logger.console(LagerLogger.DEBUG)
# Self state
self._enabled = False
self._stealth = False
# Components
self._pipe = PayloadPipe()
self._pipe._lower_level.stop()
self._chil_state = self._build_state()
self._sm = CHILStateManager(self._pipe, self._chil_state)
self._sm.subscribe_to_receive(self._receiver_cb)
def is_enabled(self):
""" checks to see if we are advertising ourselves as a robot """
return self._enabled
def is_stealth(self):
return self._stealth
def stealth_mode(self, enabled):
""" Do not ever let robot be visible when stealth is enabled """
self.logger.info("stealth_mode(%s)" % str(enabled))
self._stealth = enabled
if enabled:
self._pipe.set_visibility(False)
def set_visibility(self, val):
""" Sets whether or not the phone interface will display the data being sent"""
if self._stealth:
self.logger.info("bluetooth set_visibility(%s/steathmode)" % str(val))
self._pipe.set_visibility(False)
return
else:
self.logger.info("bluetooth set_visibility(%s)" % str(val))
sys.stdout.flush()
self._pipe.set_visibility(val)
# TODO: This is a hack to force visibility updates to make robots disappear...
# This should really be fixed on the phone side?
# if val == False:
# self.set_state("dangerous")
def enable(self):
""" Enables advertising and connecting to us """
if self._enabled:
self.logger.debug("bluetooth enabled (again)")
return
else:
self.logger.info("bluetooth enable()")
sys.stdout.flush()
self._pipe.enable()
self._pipe.start_spinning()
self._enabled = True
def disable(self):
""" disables advertising and connecting to us """
if not self._enabled:
self.logger.debug("Already disabled")
return
else:
self.logger.debug("bluetooth disable()")
sys.stdout.flush()
self._pipe.set_visibility(False)
self._pipe.disable()
self._pipe.stop_spinning()
self._enabled = False
def set_name(self, name):
""" sets the robots name """
self.logger.debug("set_name(%s)" % name)
self._name = name
self._chil_state = self._build_state()
def set_model(self, model):
""" set the robots model """
self.logger.debug("set_model(%s)" % model)
self._model = model
self._chil_state = self._build_state()
def set_state(self, state, update=True):
""" set the robots state"""
self.logger.debug("set_state(%s)" % state)
if state not in ["ok", "safe", "help", "dangerous", "off"]:
raise Exception("Unknown state for chil: %s" % state)
self._state = state
self._chil_state = self._build_state()
if update:
self._sm.update_client_state(self._chil_state)
def set_progression(self, progression, update=True):
self.logger.debug("set_progression()")
# self.logger.debug("set_progression(%s)" % str(progression))
self._progression = progression if progression else None
self._chil_state = self._build_state()
if update:
self._sm.update_client_state(self._chil_state)
def _build_state(self):
""" constructs and returns a CHILRobotState object """
return CHILRobotState(self._name, self._model, self._state, self._progression)
def _receiver_cb(self, payload):
""" receive data coming back from phone """
if 'msgtype' in payload and payload['msgtype'] == bga_msgs.REPLY_TYPE:
bga_schema.validate_object(payload, bga_schema.reply_schema)
msgid = int(payload["responses"][0]["id"])
rspid = int(payload["responses"][0]["value"])
self.logger.info("Phone Sent Response '%s'" % self._progression[msgid]["responses"][rspid]["value"])
sys.stdout.flush()
self._progression[msgid].set_selection(str(rspid))
self._progression.run_cb(msgid,rspid)
# handle the reply value:
# e.g. update our progression with that selection and handle
else:
self.logger.error("received malformed payload '%s'." % payload)
sys.stdout.flush()
class Roomba500Interface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self._vacuum_state = VacuumState()
self._vacuum_state.set_idle()
self._spinner = Spinner()
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
# Tracks button presses to prevent API calls from slipping out
self._button_tracker = ButtonTracker()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
self._keep_alive()
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
self._button_tracker = ButtonTracker()
self._api_button_clean = APIButtonPress("clean", self._button_tracker)
self._api_button_dock = APIButtonPress("dock", self._button_tracker)
#keep alive thread
self._keep_alive_timer = Timer(60, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
self._dustbin_user_cb_funs = list()
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._cleaning_user_cb_funs = list()
# Prevent Baud change
# self._clean_button_safety = CleanButtonSafety(self)
# Use button presses to track vacuum state
self._button_tracker.reg_update_callback(self._hw_buttons_cb)
self.reg_cleaning_cb(self._sweeper_cb)
# Detect Docking
self.reg_sensor_cb("charging_sources_available", self._docked_cb,
lambda old, new: (not old["base"] == new["base"]))
# Detect Lifts
self.reg_sensor_cb(
"bumps_wheeldrops", self._lifted_cb, lambda old, new: any(
[((not old[x]) and new[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self.reg_sensor_cb(
"bumps_wheeldrops", self._dropped_cb, lambda old, new: any(
[((not new[x]) and old[x])
for x in ["wheeldrop_right", "wheeldrop_left"]]))
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_vacuum_state_cb(self, cb):
""" register a callback for when the vacuum state changes """
self._vacuum_state.callbacks.append(cb)
def reg_buttons_cb(self, cb):
self._button_tracker.reg_update_callback(cb)
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name == "buttons":
self.logr.error(
"cannot log buttons using this callback - use dedicated reg_button_cb!"
)
return
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name].append((cmp_fun, cb))
def reg_dustbin_cb(self, cb):
self._dustbin_user_cb_funs.append(cb)
def reg_cleaning_cb(self, cb):
"""" callbacks need to take an argument """
self._cleaning_user_cb_funs.append(cb)
def _docked_cb(self):
base = self.robot.get_sensor("charging_sources_available")["base"]
if base:
self._vacuum_state.set_docked()
def _lifted_cb(self):
""" gets called each time a wheel leaves the ground """
self.logr.debug("Lifted!")
if not self._vacuum_state.is_idle():
self._vacuum_state.set_idle()
def _dropped_cb(self):
""" gets called every time a wheel is set back down """
self.logr.debug("droped!")
if not self._vacuum_state.is_idle():
self._vacuum_state.set_idle()
def baud_fix(self):
# self.robot.sci = SerialCommandInterface(self._dev_path, 19200)
with self.robot.sci.lock:
self.logr.warn("baud_fix()!")
self._keep_alive()
time.sleep(0.20)
self.robot.sci.ser.baudrate = 19200
self.robot.sci.start()
time.sleep(0.20)
try:
self.robot.change_baud_rate(115200)
except Exception as e:
self.logr.error("change_baud_rate %s: %s" % (str(type(e)), e))
try:
self.robot.sci.start()
except Exception as e:
self.logr.error("start %s: %s" % (str(type(e)), e))
time.sleep(.15)
self.logr.warn("baud_fix() done.")
sys.stdout.flush()
def _hw_buttons_cb(self, values):
""" called whenever a person presses one of the hardware buttons on the robot """
buttons = [k for k, v in values.items() if v]
# self._clean_button_safety.set_value(True if "clean" in buttons else False)
# Dont do anything if we are just lifting up off a button
# This isn't quite right, but is probably good enough
if not buttons:
return
self.logr.debug("HW Button Pressed: %s" % str(buttons))
oi_mode = self.get_sensor("oi_mode")
if not oi_mode == "passive":
self.logr.debug("Not setting vacuum_state in control mode '%s'" %
oi_mode)
return
if self._telemetry["lifted"]:
self.logr.debug("Ignoring buttons because lifted")
return
if len(buttons) > 1:
self.logr.error(
"Heard too many button states at once %s, auto cancelling." %
str(buttons))
self.robot.pause()
if self._vacuum_state.is_idle() or self._vacuum_state.is_docked():
if "clean" in buttons:
self._vacuum_state.set_cleaning()
elif "spot" in buttons:
self._vacuum_state.set_cleaning()
# [db] we don't seperately track spot cleaning anymore
# self._vacuum_state.set_spot_cleaning()
elif "dock" in buttons:
self._vacuum_state.set_docking()
# [db] this happens if "day" or "hour" gets pressed...
else:
self.logr.warn("Unhandled last button: %s" % str(buttons))
else:
if self.is_docked():
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
def _sweeper_cb(self, cleaning):
""" gets called when the sweeper either starts or stops """
self.logr.debug("sweeper_cb(%s)" % str(cleaning))
if not self._vacuum_state:
self.logr.warn(
"Sweeper changed state, but vacuum_state is unknown")
return
vacuum_state = self._vacuum_state.get()
if cleaning and (vacuum_state
not in ["cleaning", "spot_cleaning", "docking"]):
self.logr.error(
"Sweeper started, but was in state '%s'. Assuming Cleaning." %
vacuum_state)
self._vacuum_state.set_cleaning()
elif not cleaning and (vacuum_state
not in ["idle", "docking", "docked"]):
if self._telemetry["docked"]:
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
def gpio_dustbin_cb(self, value):
""" update the dustbin status """
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
if self._dustbin_user_cb_funs:
for cb in self._dustbin_user_cb_funs:
cb(value)
def get_sensor(self, key):
if key == "buttons":
return self._button_tracker.get_buttons()
return self.robot.get_sensor(key)
def passive(self):
# This gets called when the controller boots up, and just before the clean cmd
# Do NOT set the vacuum state explicitly here, because it should get overwritten
# moments later and will just cause problems. If you want vacuum_state set, use
# pause() instead.
self.logr.info("passive mode")
self.robot.passive()
def control(self):
self.keep_alive()
self.logr.info("control mode")
self.robot.control()
def is_docking(self):
return self._vacuum_state.is_docking()
def is_docked(self):
return self._vacuum_state.is_docked()
# def is_on_dock(self):
# return self._telemetry["docked"]
def is_lifted(self):
drops = self.robot.get_sensor("bumps_wheeldrops")
return (drops["wheeldrop_right"] or drops["wheeldrop_left"])
def is_charging(self):
return self._telemetry["charging"]
def is_cleaning(self):
return self._vacuum_state.is_cleaning(
) or self._vacuum_state.is_spot_cleaning()
def dock(self):
""" Sends robot to the dock. Send twice if cleaning, once if not cleaning"""
if self.is_cleaning():
self.logr.info("dock() {while cleaning}")
self._vacuum_state.set_docking()
self.robot.pause()
time.sleep(0.5)
else:
self.logr.info("dock() {while paused}")
self._api_button_dock(self.robot.dock)
self._vacuum_state.set_docking()
def clean(self, no_state_change=None):
self.logr.info("clean()")
if (self._vacuum_state.is_cleaning()
or self._vacuum_state.is_spot_cleaning()
) and no_state_change is None:
self.logr.error("Already cleaning. Ignoring command")
return
self._api_button_clean(self.robot.clean)
if no_state_change is None:
self._vacuum_state.set_cleaning()
def pause(self):
self.logr.info("pause()")
if self._telemetry["docked"]:
self._vacuum_state.set_docked()
else:
self._vacuum_state.set_idle()
self.robot.pause()
def keep_alive(self):
self._keep_alive()
def _keep_alive(self):
""" Keep alive timer callback. Throws a gpio pin up and down """
self.logr.debug("SCI Keep Alive")
GPIO.output(DEVICE_DETECT, GPIO.LOW)
time.sleep(0.1)
GPIO.output(DEVICE_DETECT, GPIO.HIGH)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
self._spinner.spin()
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except BadDataLengthError as e:
self.logr.error(e)
return
except DriverError as e:
self.logr.warn(e)
self._telemetry["sci_error"] = True
self._keep_alive()
return
except Exception as e:
self.logr.error(e)
return
# Update Buttons
self._button_tracker.update(self.robot.get_sensor("buttons"))
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, cb_list in self._callbacks.items():
for (cmp_fun, cb) in cb_list:
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def _update_telemetry(self):
# If sci error has been set (by poll_sensors_cb), undo it
if self._telemetry["sci_error"]:
self._telemetry["sci_error"] = False
# Compare charging status
try:
charging = self.robot.get_sensor("charging_state")
chargingbool = False if charging in [
"waiting", "not_charging", "error"
] else True
if not chargingbool == self._telemetry["charging"]:
self._telemetry["charging"] = chargingbool
if charging == "error":
self.logr.error("Charging: Fault")
elif charging == "waiting":
self.logr.warn("Charging: waiting")
elif charging == "trickle_charging":
self.logr.info("Charging: trickle")
elif charging == "charging":
self.logr.info("Charging: Charging")
elif charging == "charge_recovery":
self.logr.info("Charging: Recovery")
elif charging == "not_charging":
self.logr.info("Charging: Not Charging")
else:
self.logr.error("Charging: Received unknown state '%s'" %
str(charging))
except SensorStoreException as e:
self.logr.warn(e)
except Exception as e:
self.logr.warn(e)
self.logr.warn("Not updating telemetry this time")
# See if we are on the dock
try:
sources = self.robot.get_sensor("charging_sources_available")
if not sources["base"] == self._telemetry["docked"]:
self._telemetry["docked"] = sources["base"]
except SensorStoreException as e:
self.logr.warn(e)
# see if the robot has been picked up
try:
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
except SensorStoreException as e:
self.logr.warn(e)
# Check if the sweeper brush is running (that means we are cleaning)
try:
# Eva has a lower current than the other robots - dips into 80s sometimes
# so this is a good threshold, I think
cleaning = self.robot.get_sensor("main_brush_current") >= 65
# if cleaning:
# print self.robot.get_sensor("main_brush_current")
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Do cleaning cb if it is registered.
if self._cleaning_user_cb_funs:
for cb in self._cleaning_user_cb_funs:
cb(cleaning)
except SensorStoreException as e:
self.logr.warn(e)
# Check the status of button presses
try:
buttons = self._button_tracker.get_buttons()
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
except SensorStoreException as e:
self.logr.warn(e)
# Check the robot voltage
try:
voltage = self.robot.get_sensor("voltage") / 1000.0
if abs(voltage - self._telemetry["battery_voltage"]) >= 0.1:
self._telemetry["battery_voltage"] = voltage
except SensorStoreException as e:
self.logr.warn(e)
# Set api mode
try:
oi_mode = self.robot.get_sensor("oi_mode")
if not oi_mode == self._telemetry["api_mode"]:
self._telemetry["api_mode"] = oi_mode
except SensorStoreException as e:
self.logr.warn(e)
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._keep_alive_timer.cancel()
self.logr.info("keep alive killed")
self._sensor_update_timer.cancel()
self.logr.info("killed sensor update timer")
time.sleep(1)
GPIO.cleanup()
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
class RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger()
self.logger.console(LagerLogger.DEBUG)
self.telemetry_pub = self.ros.Publisher("%s/telemetry" % self.robot_name, Telemetry, latch=True, queue_size=10)
if config.has("stage","enabled") and config["stage"]["enabled"] == "True":
self.logger.info("Using Stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.debug("Using Hardware")
from vacuum_interfaces.discovery_interface import DiscoveryInterface
self.robot = DiscoveryInterface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
m = self.conv_stack.add_message("This robot has been disabled due to problems"
" detected in the onboard electronics.<br/><br/>We apologize for the inconvenience.")
self.phone_link.set_progression(self.conv_stack)
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
super(RobotController, self).initialize()
self.logger.info("Robot Initialized")
def shutdown(self):
""" Runs when the robot controller is shutting down """
self.logger.warning("Shutting down controller")
super(RobotController, self).shutdown()
self.robot.terminate()
def reset_controller(self):
self.logger.warn("Resetting controller")
super(RobotController, self).reset_controller()
def start_run(self):
self.logger.info("starting run")
# We assume that the robot needs
if self.behavior_profile.is_disabled():
self.phone_link.disable()
self.logger.info("Ignoring start command")
elif self.behavior_profile.is_dead():
self.phone_link.set_state("off")
self.phone_link.set_visibility(True)
self.controller_state.set_running()
else:
self.logger.error("Received unexpected behavior profile")
return
################################################################################
def telemetry_cb(self, values):
""" Sends telemetry information """
t = Telemetry()
t.lifted = values["lifted"]
t.charging = values["charging"]
t.docked = values["docked"]
t.dustbin = values["dustbin"]
t.cleaning = values["cleaning"]
t.buttons = values["buttons"]
t.api_mode = "unknown"
self.telemetry_pub.publish(t)
class RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("controller")
self.logger.console(LagerLogger.DEBUG)
self.conv_stack = ConversationStack()
self.telemetry_pub = self.ros.Publisher("%s/telemetry" % self.robot_name, Telemetry, latch=True, queue_size=10)
self.ros_cmd_sub = self.ros.Subscriber("%s/cmd" % self.robot_name, String, self.ros_cmd_cb)
if config.has("stage","enabled") and config["stage"]["enabled"] == "True":
self.logger.warn("using stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.info("Using hardware")
from vacuum_interfaces.roomba500_interface import Roomba500Interface
self.robot = Roomba500Interface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.phone_link.set_visibility(False)
self._telemetry_timer = Timer(5, lambda x=self.robot._telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
self.robot.passive()
self.logger.info("Robot Initialized")
self.ros.enable()
self.phone_link.enable()
def experiment_init(self):
""" Callbed by robot controller after a new config has been loaded """
self.logger.info("%s Loading Behavior Profile: %s" % (self.robot_name, self.behavior_profile))
if self.behavior_profile.is_disabled():
self.phone_link.set_visibility(False)
elif self.behavior_profile.is_easy():
self.set_stack_idle()
elif self.behavior_profile.is_help():
self.phone_link.set_state("help")
else:
self.logger.error("Received unexpected behavior profile")
return
# Initialize controller
if self.experiment_state:
if self.experiment_state.get() in [ExperimentState.STANDBY, ExperimentState.READY]:
self.controller_state.set_ready()
elif self.experiment_state.is_running():
self.logger.warn("Received configuration after experiment has started")
self.logger.warn("Set to running")
self.start_run()
# If we are coming online while paused, start the robot and set to paused
elif self.experiment_state.is_paused():
self.logger.warn("Received configuration after experiment has started")
self.logger.warn("Starting, then setting to pause")
self.start_run()
self.controller_state.set_paused()
else:
self.controller_state.set_standby()
else:
self.logger.warn("Received configuration, but no experiment state")
def experiment_state_standby(self):
self.reset_controller()
def experiment_state_ready(self): pass
def experiment_state_running(self):
if not self.behavior_profile:
self.logger.warn("Experiment Running, but still waiting for config")
return
# If we are in ready state, start things off!
if self.controller_state.is_ready():
self.controller_state.set_running()
self.start_run()
elif self.controller_state.is_paused():
self.controller_state.set_running()
def experiment_state_paused(self):
self.controller_state.set_paused()
def experiment_state_complete(self):
self.phone_link.set_visibility(False)
self.controller_state.set_paused()
def shutdown(self):
""" Runs when the robot controller is shutting down """
# self.charging_timer.cancel()
self.logger.warning("Shutting down controller")
self._telemetry_timer.cancel()
self.phone_link.set_visibility(False)
# self.robot.pause()
self.ros.shutdown()
self.robot.terminate()
time.sleep(2)
self.phone_link.disable()
################################################################################
def reset_controller(self):
self.logger.warn("Resetting controller")
def start_run(self):
self.logger.info("starting run")
def ros_cmd_cb(self, data):
""" Receive commands from ROS """
if data.data == "clean":
self.logger.warn("ROS: clean()")
self.robot.clean()
elif data.data == "dock":
self.logger.warn("ROS: dock()")
self.robot.dock()
elif data.data == "pause":
self.logger.warn("ROS: pause()")
self.robot.pause()
def telemetry_cb(self, values):
""" Sends telemetry information """
t = Telemetry()
t.battery_voltage = values["battery_voltage"]
t.lifted = values["lifted"]
t.charging = values["charging"]
t.docked = values["docked"]
t.dustbin = values["dustbin"]
t.cleaning = values["cleaning"]
t.buttons = values["buttons"]
t.sci_error = values["sci_error"]
t.api_mode = values["api_mode"]
self.telemetry_pub.publish(t)
class BaseRobotController(object): #pylint: disable=abstract-class-not-used
""" Manages ROS connection, experiment configuration, robot state, etc """
def __init__(self, config):
self.config = config
self.robot_name = config['robot']['name']
self.robot_model = config['bluegigga']['model']
self.ros = ReliableNode(self.robot_name)
# DEBUG LOGGING
self.printer = LagerLogger("RobotBaseController")
self.printer.console(LagerLogger.DEBUG)
# ROS Logging
self.logging = Loggerator(self.ros, config)
# Robot Controller State
self._controller_state_pub = self.ros.Publisher(
"/experiment/%s/state" % self.robot_name, String, queue_size=10)
self._behavior_profile_pub = self.ros.Publisher(
"/experiment/%s/profile" % self.robot_name, String, queue_size=10)
self._phone_support_pub = self.ros.Publisher(
"/experiment/%s/phone_support" % self.robot_name,
Bool,
queue_size=10)
self.controller_state = ControllerState() # manual, ready, running
self.controller_state.set_standby()
# Robot Behavior Experiment Profile
self.behavior_profile = BehaviorProfile()
self.behavior_profile.unset()
# Startup bluetooth phone link
if config.has('bluegigga',
'enabled') and config['bluegigga']['enabled'] == "True":
self.printer.info("Using Bluetooth Hardware")
self.phone_link = PhoneLink()
else:
self.printer.info("Faking bluetooth")
self.phone_link = FakePhoneLink()
self.conv_stack = ConversationStack()
self.phone_link.set_name(self.robot_name)
self.phone_link.set_model(config['bluegigga']['model'])
self.phone_link.set_state("safe")
self.phone_link.set_visibility(False)
self.experiment_state = ExperimentState()
# These need to come after the controller_state has been declared
self.ros.Subscriber('/experiment/config', Configuration,
self._experiment_config_cb)
self.ros.Subscriber('/experiment/state', String,
self.experiment_state_cb)
self._controller_state_timer = self.ros.ReliableTimer(
0.5, self._controller_state_timer_cb)
self._controller_state_timer.start()
def initialize(self):
""" Called once the controller's __init__ has been finished, to give
the subclasses time to do things before firing up ros and the phone """
self.ros.enable()
self.phone_link.enable()
def _controller_state_timer_cb(self):
""" heatbeat that publishes the controller_state (overall state of controller) """
self._controller_state_pub.publish(str(self.controller_state))
self._behavior_profile_pub.publish(str(self.behavior_profile))
self._phone_support_pub.publish(not self.phone_link.is_stealth())
def experiment_state_cb(self, data):
""" Receives experiment state from experiment controller """
self.printer.info("Heard Experiment State: %s" % data.data)
self.experiment_state.set(data.data)
if self.experiment_state.is_standby():
self.reset_controller()
elif self.experiment_state.is_ready():
pass # Wait until we get a configuration
elif self.experiment_state.is_running():
# If we are in ready state, start things off!
# Make sure we know what behavior to perform
if not self.behavior_profile:
self.printer.warn(
"Experiment Running, but still waiting for config")
return
if self.controller_state.is_standby(
) or self.controller_state.is_ready():
self.start_run()
elif self.controller_state.is_paused():
self.controller_state.set_running()
elif self.experiment_state.is_paused():
if self.controller_state.is_standby(
) or self.controller_state.is_ready():
self.start_run()
self.controller_state.set_paused()
elif self.experiment_state.is_complete():
# self.phone_link.set_visibility(False)
# self.controller_state.set_paused()
self.reset_controller()
# Do controller specific callbacks
state_funs = {
ExperimentState.STANDBY: self.experiment_state_standby,
ExperimentState.READY: self.experiment_state_ready,
ExperimentState.RUNNING: self.experiment_state_running,
ExperimentState.PAUSED: self.experiment_state_paused,
ExperimentState.COMPLETE: self.experiment_state_complete
}
state_funs[data.data]()
def experiment_state_standby(self):
pass
def experiment_state_ready(self):
pass
def experiment_state_running(self):
pass
def experiment_state_paused(self):
pass
def experiment_state_complete(self):
pass
def _experiment_config_cb(self, config_msg):
""" Receives the experiment configuration,
calls user callback,
configures the phone interface,
sets controller state """
# [db] this was causing a race condition when the controller would start up after experiment was running
# if not (self.controller_state.is_standby() or self.controller_state.is_ready()):
if (not (self.controller_state\
or self.controller_state.is_standby()\
or self.controller_state.is_ready() )\
and self.behavior_profile):
# We have received an experiment configuration at a strange time, ignore.
self.printer.error(
"Ignoring received configuration while in state %s" %
str(self.controller_state))
return
self.printer.info("Received Configuration: P=%d, C=%d, R=%d" %
(config_msg.participant_id, config_msg.condition_id,
config_msg.run_id))
try:
self.behavior_profile.get_robot_config(self.robot_name,
config_msg.condition_id,
config_msg.run_id)
except KeyError as e:
self.printer.error(
"No Behavior Profile defined for '%s' in Cond '%d', Run '%d'" %
(self.robot_name, config_msg.condition_id, config_msg.run_id))
return
self.logger.info("%s Loading Behavior Profile: %s" %
(self.robot_name, self.behavior_profile))
# Check phone support mode
phone_support = {
(1, 1): True,
(1, 2): False,
(2, 1): True,
(2, 2): False,
(3, 1): False,
(3, 2): True,
(4, 1): False,
(4, 2): True
}
if phone_support[(config_msg.condition_id, config_msg.run_id)]:
self.phone_link.stealth_mode(False)
else:
self.phone_link.stealth_mode(True)
# Initialize controller
# self.experiment_init()
if self.experiment_state:
if self.experiment_state.get() in [
ExperimentState.STANDBY, ExperimentState.READY
]:
self.controller_state.set_ready()
elif self.experiment_state.is_running():
self.logger.warn(
"Received configuration after experiment has started")
self.logger.warn("Set to running")
self.start_run()
# If we are coming online while paused, start the robot and set to paused
elif self.experiment_state.is_paused():
self.logger.warn(
"Received configuration after experiment has started")
self.logger.warn("Starting, then setting to pause")
self.start_run()
self.controller_state.set_paused()
else:
self.controller_state.set_standby()
else:
self.logger.warn("Received configuration, but no experiment state")
def experiment_init(self):
""" This is called after behavior_profile has been set! """
raise NotImplementedError("this needs to be implemented!")
def hw_pause(self):
""" Make the robot stop moving """
self.logger.debug("Pause called (no-op)")
def reset_controller(self):
self.hw_pause()
# Enable the phone_link, but set it to invisible
self.phone_link.set_state("safe")
self.phone_link.stealth_mode(False)
self.phone_link.set_visibility(False)
self.phone_link.enable()
# Reset the behavior profile, and put us into standby mode
self.behavior_profile.unset()
self.controller_state.set_standby()
def shutdown(self):
self.hw_pause()
self.phone_link.set_visibility(False)
self.ros.shutdown()
# self.robot.terminate()
time.sleep(2)
self.phone_link.disable()
def start_run(self):
raise NotImplementedError()
