class Response(dict):
def __init__(self, val, disp_val):
dict.__init__(self, {"id": val, "value": disp_val})
self.logr = LagerLogger("%sResponse" % disp_val)
self.logr.console(LagerLogger.DEBUG)
self._cb_fun = None
def _do_cb(self):
self.logr.debug("Start callback")
sys.stdout.flush()
self._cb_fun()
self.logr.debug("End callback")
sys.stdout.flush()
def __call__(self):
""" Calls this responses callback """
if self._cb_fun:
self._do_cb()
# [db] Removed threading, replaced with threading in payload pipe
# t = Thread(target=self._cb_fun)
# t = Thread(target=self._do_cb)
# t.start()
def set_callback(self, fun):
""" sets a callback function to associate with this response """
self._cb_fun = fun
def cast(self):
""" returns the simplified version of this python object -
in other words in gets rid of the extra functionality we got from subclassing"""
return dict(self)
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 ReliablePublisher(ReliableTopicConn):
def __init__(self, *argv, **kwds):
ReliableTopicConn.__init__(self, rospy.Publisher, *argv, **kwds)
self.logr = LagerLogger("ReliablePublisher")
self.logr.console(LagerLogger.DEBUG)
def publish(self, *args, **kwds):
with self.lock:
if self._conn:
self._conn.publish(*args, **kwds)
else:
self.logr.debug("ROS IS DOWN RIGHT NOW")
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 Message(dict):
def __init__(self, msgid, content):
dict.__init__(self, {})
self.logr = LagerLogger("Message")
self.logr.console(LagerLogger.DEBUG)
self.tag = None # This is a free object you can set to identify your message with
self["msgid"] = msgid
self["content"] = content
self["popup"] = False
def set_content(self, message):
self["content"] = message
def set_selection(self, rspid):
""" set the selected response id if a response has been given """
if "responses" not in self.keys():
self.logr.warn(
"tried to set response selection '%s' for message with no response '%s'"
% (rspid, self["content"]))
return
if int(rspid) not in range(len(self["responses"])):
self.logr.warn(
"tried to set response invalid selection '%s' for message with response '%s', %s"
% (rspid, self["content"], str(self["responses"])))
return
self["selection"] = rspid
def unset_selection(self):
if "selection" in self.keys():
self.pop("selection")
def set_popup(self):
self["popup"] = True
def unset_popup(self):
self["popup"] = False
def add_response(self, disp_val, cb_fun):
""" add a possible response"""
if "responses" not in self.keys():
self["responses"] = list()
r = Response(str(len(self["responses"])), disp_val)
r.set_callback(cb_fun)
self["responses"].append(r)
return r
class StateManager(object):
def __init__(self, pipe, robot_state):
self.logr = LagerLogger()
self.logr.console(LagerLogger.DEBUG)
self._pipe = pipe
self._robot_state = robot_state
self.connected_client = None
pipe.subscribe_to_connect(self.on_connect_cb)
pipe.subscribe_to_disconnect(self.on_disconnect_cb)
def on_connect_cb(self, who):
"""Determines if a state update is necessary upon client connection."""
self.connected_client = who
def on_disconnect_cb(self):
self.connected_client = None
# def update_client_state(self, robot_state=None):
# """Sends the currently connected client a state update. Also will change
# the StateManager's internal state if robot_state != None.
# """
# if robot_state != None:
# self._robot_state = robot_state
# state_msg = self._robot_state.update_msg()
# self._pipe.set_adv_cache_data(state_msg)
# self._pipe.write(state_msg)
def update_client_state(self, robot_state):
""" Accepts a new state to publish, sets the checksum, copies data into pipe """
self.logr.info("update_client_state()")
self._robot_state = robot_state
state_msg = self._robot_state.update_msg()
self._pipe.set_adv_cache_data(state_msg)
self._pipe.write(state_msg)
def subscribe_to_receive(self, on_receive_cb):
self._pipe.subscribe_to_receive(on_receive_cb)
def robot_state(self):
return self._robot_state.copy()
class ReliableTopicConn(object):
""" This wraps topic connections (Publisher / Subscriber), and adds methods
for _start and _stop which can be called by a ReliableNode whenever ROSMaster
comes or goes.
"""
def __init__(self, topic_conn, *argv, **kwds):
self.logr = LagerLogger("ReliableTopicConn")
self.logr.console(LagerLogger.DEBUG)
self.lock = threading.Lock()
self._topic_conn = topic_conn
self._argv = argv
self._kwds = kwds
self._conn = None
self._name = argv[0]
def _start(self):
with self.lock:
if self._conn:
raise Exception("%s has already been initialized" %
self._topic_conn.__name__)
self.logr.debug("Starting %s %s" %
(self._topic_conn.__name__, self._name))
self._conn = self._topic_conn(*self._argv, **self._kwds)
def _stop(self):
if not self._conn:
raise Exception("%s doesn't exist yet" % self._topic_conn.__name__)
self.unregister()
def get_num_connections(self):
with self.lock:
if not self._conn:
raise Exception("%s not started" % self._topic_conn.__name__)
return self._conn.get_num_connections()
def unregister(self):
with self.lock:
self.logr.warn("Stopping %s %s" %
(self._topic_conn.__name__, self._name))
self._conn.unregister()
self._conn = None
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 FakePhoneLink(object):
""" Implements the same phone interface as teh real phone link """
def __init__(self):
self.logger = LagerLogger("FakePhoneLink")
self.logger.console(LagerLogger.DEBUG)
self.logger.warn("~~~ WARNING: USING FAKE PHONE LINK ~~~")
self._state = "dangerous"
self._enabled = False
def is_enabled(self):
return self._enabled
def set_visibility(self, val): pass
def enable(self):
self.logger.debug("enable()")
self.logger.warn("~~~ WARNING: USING FAKE PHONE LINK ~~~")
self._enabled = True
def disable(self):
self.logger.debug("disable()")
self.logger.warn("~~~ WARNING: USING FAKE PHONE LINK ~~~")
self._enabled = False
def set_name(self, value): pass
def set_model(self, value): pass
def set_state(self, value, update=True): self._state = value
def set_progression(self, progression, update=True): pass
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 RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("")
self.logger.console(LagerLogger.DEBUG)
self.conv_stack = ConversationStack()
self.telemetry_pub = self.ros.Publisher("%s/telemetry" %
self.robot_name,
Telemetry,
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.fake_interface import FakeInterface
self.robot = FakeInterface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.phone_link.set_visibility(False)
m = self.conv_stack.add_message(
"This robot has been disabled due to problems detect in the onboard electronics. We apologize for the inconvenience."
)
m.add_response("safe", lambda: self.test_reply("safe"))
m.add_response("dangerous", lambda: self.test_reply("dangerous"))
m.add_response("help", lambda: self.test_reply("help"))
m.add_response("off", lambda: self.test_reply("off"))
self.phone_link.set_progression(self.conv_stack)
self._telemetry_timer = Timer(
1, lambda x=fake_telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
self.logger.info("Robot Initialized")
self.phone_link.set_visibility(
True
) # Start showing the robot even when the experiment is not running
self.phone_link.enable()
self.ros.enable()
def experiment_init(self):
""" Callbed by robot controller after a new config has been loaded """
self.logger.info("%s Loaded Behavior Profile: %s" %
(self.robot_name, self.behavior_profile))
# if self.behavior_profile.is_dead():
# # Queue up the state for this robot, but don't show it yet
# self.phone_link.enable()
# self.phone_link.set_state("off")
def experiment_state_standby(self):
self.reset_controller()
def experiment_state_ready(self):
pass
def experiment_state_running(self):
# If we are in ready state, start things off!
if self.controller_state.is_ready():
self.start_run()
elif self.controller_state.is_paused():
self.controller_state.set_running()
def experiment_state_paused(self):
self.phone_link.set_visibility(True)
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()
# time.sleep(.25)
# self.robot.pause()
self._telemetry_timer.cancel()
time.sleep(.25)
self.ros.shutdown()
self.robot.terminate()
self.phone_link.disable()
################################################################################
def reset_controller(self):
self.logger.warn("Resetting controller")
self.phone_link.set_state("safe")
self.phone_link.set_visibility(False)
self.behavior_profile.unset()
self.controller_state.set_standby()
def start_run(self):
self.logger.info("starting run")
self.phone_link.set_visibility(True)
def test_reply(self, state):
self.phone_link.set_state(state)
def clean(self):
self.logger.warn("CLEAN?")
def telemetry_cb(self, values):
""" Sends telemetry information """
self.logger.debug("telemetry")
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"]
self.telemetry_pub.publish(t)
self.logger.debug(str(t))
class ButtonTracker(object):
""" Tracks changes to button states, reports them to user callbacks. ALso used to remove API button presses
from robot data before their values ever get reported """
def __init__(self):
self._api_callbacks = dict()
self._user_callbacks = list()
self.logr = LagerLogger("ButtonTracker")
self.logr.console(LagerLogger.DEBUG)
self._lock = Lock()
# This stores the values we will pass to callbacks
self._button_dict = dict()
# Cache what the last data looked like (e.g. ['clean','spot'])
# This is different from _button_dict in that button dict can changed by removing values, while
# this is a pure snapshot of the incoming data
self._last_data = list()
def update(self, button_dict):
""" called just after the hardware reports what buttons have been pressed, removes buttons that
were 'pressed through the API' so that we only report hardware presses. """
new_data = [k for k, v in button_dict.items() if v]
changed = True if not set(new_data) == set(self._last_data) else False
if not changed:
return
cb_value = None
with self._lock:
# Save the snapshot so we will know when it changes again, even if we modify the data.
self._last_data = new_data
for name, pressed in button_dict.items():
if pressed and name in self._api_callbacks:
# Do callback and pretend button was never pressed
self._api_callbacks.pop(name)()
self.logr.debug("discarding anticapated button '%s'" %
name)
pressed = False
# Save the value of new or changed buttons
if (name not in self._button_dict) or (
not self._button_dict[name] == pressed):
self._button_dict[name] = pressed
cb_value = copy.copy(self._button_dict)
cb_threads = list()
for cb in self._user_callbacks:
cb_threads.append(Thread(target=cb, args=(cb_value, )))
cb_threads[-1].start()
# cb(cb_value)
wait = 2.0
for cb in cb_threads:
cb.join(wait)
if cb.is_alive():
self.logr.warn("Long running hw button callback!")
# else:
# self.logr.debug("dead")
# import traceback
# traceback.print_stack()
# sys.stdout.flush()
wait = .1
def get_buttons(self):
with self._lock:
return copy.copy(self._button_dict)
def consume_event(self, button_name, cbfun):
""" register a button name to consume, does so in the form of removing it from list to pass on and
calling this associated callback (takes no arguments). Used by APIButtonCall """
with self._lock:
if button_name in self._api_callbacks:
raise Exception("Button '%s' already registered!" %
button_name)
self._api_callbacks[button_name] = cbfun
def cancel_consume(self, button_name):
self.logr.debug("canceling consume event for '%s'" % button_name)
with self._lock:
if button_name in self._api_callbacks:
self._api_callbacks.pop(button_name)
else:
self.logr.warn("no api_callback for '%s' for found" %
button_name)
def reg_update_callback(self, fun):
""" register a user callback to call whenever a button gets pressed """
self._user_callbacks.append(fun)
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)
from generic_vacuum import VacuumInterface
import copy
import sys
import time
import threading
import RPi.GPIO as GPIO
from gpio_sensor import gpio_sensor
from timer import Timer
import logging
from threading import Lock
from vacuum_experiment_msgs.telemetry import TelemetryData
from lagerlogger import LagerLogger
logr = LagerLogger("")
logr.console(LagerLogger.DEBUG)
#
# /) ,
# _(/ _ _ ____ _ _ __
# (_(__(_/_)_(__(_) (/___(/_/ (_(_/_
# .-/
# (_/
DROPWHEEL = 29
DUSTBIN = 16
POWER = 33
SPOT = 31
CLEAN = 35
MAX = 37
BASE = 18
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 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 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 ReliableNode(object):
class Timer(object):
""" Timer fires on an interval, but next cb does not fire until the
callback has completed. Can also be restarted without reinstantiating """
def __init__(self, time, cb):
self._time = time
self._cb = cb
self._timer = None
self._running = True
self._spin = True # This is for the spin() function
def start(self):
self._running = True
self._start_timer()
def cancel(self):
self._running = False
self._stop_timer()
def _start_timer(self):
if not self._running:
return
if self._timer:
raise Exception("Timer already started")
self._timer = threading.Timer(self._time, self._cbfun)
self._timer.start()
def _stop_timer(self):
if self._timer:
self._timer.cancel()
self._timer = None
def _cbfun(self):
self._stop_timer()
self._cb()
self._start_timer()
def __init__(self,
name,
argv=None,
anonymous=False,
log_level=None,
disable_rostime=False,
disable_rosout=False,
xmlrpc_port=0,
tcpros_port=0):
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._enabled = False
self._timer = ReliableNode.Timer(.1, self._timer_cb)
self._name = name
# State Information: Whether or not we are connected to the master
self._state = "disconnected"
self._states = {
"connected": self._state_connected,
"disconnected": self._state_disconnected
}
# Topics to keep track of
self._topic_connections = list()
self._timer_connections = list()
# Node settings
self._settings = {
"argv": argv,
"anonymous": anonymous,
"log_level": log_level,
"disable_rostime": disable_rostime,
"disable_rosout": disable_rosout,
"xmlrpc_port": xmlrpc_port,
"tcpros_port": tcpros_port
}
# Check for ROS Master, and connect if you see it
# Start a timer so that we keep checking to make sure it is around.
if self._check_connected():
self._try_connect()
self._timer.start()
def enable(self):
""" Allow node to connect to ROS if available """
self.logr.info("Enabling RosNode")
self._enabled = True
def disable(self):
""" Prevent node from connecting with ROS """
self.logr.info("Disabling RosNode")
self._enabled = False
def _timer_cb(self):
""" Timer callback to check whether or not the Master still exists """
self._states[self._state]()
def is_connected(self):
if not self._state == "connected":
return False
return True
def _check_connected(self):
""" Checks to see if a roscore instance is running """
if not self._enabled:
return
global rospy
return rospy.core.rosgraph.is_master_online()
def _try_connect(self):
""" Attempt to establish a connection to ROS master """
# The following three lines seem to work. Dont change them
global rospy
rospy = None
rospy = importlib.import_module("rospy")
self.logr.debug("Connecting to ROS")
rospy.init_node(self._name,
argv=self._settings["argv"],
anonymous=self._settings["anonymous"],
log_level=self._settings["log_level"],
disable_rostime=self._settings["disable_rostime"],
disable_rosout=self._settings["disable_rosout"],
disable_signals=True,
xmlrpc_port=self._settings["xmlrpc_port"],
tcpros_port=self._settings["tcpros_port"])
for conn in self._topic_connections:
conn._start()
self._state = "connected"
self.logr.info("Connected to ROS")
def _state_disconnected(self):
""" Disconnected State
Check to see if a master is available, if so try to connect.
"""
if not self._check_connected():
return
# We found a connection!
self._try_connect()
def _state_connected(self):
if rospy.is_shutdown():
#We are shutting down ROS. Close everything up and hibernate
self.logr.warn("Heard ros shutdown call")
self._shutdown_ros()
self._state = "disconnected"
return
if not self._check_connected():
# We have been disconnected
self.logr.info("We have been disconnected")
self._shutdown_ros()
self._state = "disconnected"
def _shutdown_ros(self):
""" Shutdown all topics"""
global rospy
self.logr.warn("Shutting down ros topics")
# Unregister Topics
for conn in self._topic_connections:
try:
conn._stop()
except Exception:
pass
rospy = None
rospy = importlib.import_module("rospy")
# The following command to signal shutdown should NOT be called.
# We will never be able to undo it
# rospy.core.signal_shutdown("reliable down!")
def shutdown(self):
""" Shutdown the reliable node """
self._timer.cancel()
self._shutdown_ros()
for t in self._timer_connections:
t.cancel()
rospy.core.signal_shutdown("reliable down!")
def Publisher(self, *argv, **kwds):
""" replacement for rospy.Publisher() interface.
ReliablePublishers "inherit" (not really) from rospy.Publisher(), but have a link back
to the ReliableNode. They have the following characteristics
- un-register themselves when the ROSMaster goes away or shutsdown
- re-register themselves when the ROSMaster reappears
- throw exceptions when you try to publish and ROSMaster is not around.
"""
pub = ReliablePublisher(*argv, **kwds)
self._topic_connections.append(pub)
if self.is_connected():
pub._start()
return pub
def Subscriber(self, *argv, **kwds):
""" replacement call for rospy.Subscriber() """
sub = ReliableSubscriber(*argv, **kwds)
self._topic_connections.append(sub)
if self.is_connected():
sub._start()
return sub
def ReliableTimer(self, duration, callback):
""" Makes a Timer that will be automatically killed when the process goes down"""
t = ReliableNode.Timer(duration, callback)
self._timer_connections.append(t)
return t
def spin(self):
""" Blocks until keyboard interrupt or stop_spin() """
self._spin = True
try:
while self._spin:
time.sleep(0.5)
except KeyboardInterrupt:
pass
def stop_spin(self):
""" Can be used to programatically stop the spinning """
self._spin = False
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 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 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()
