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