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 __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()
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__()
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 __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 __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 __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 __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 __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 __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()
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
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 __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 __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 __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 __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 __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)
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()
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()
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)
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()
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 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 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 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)
Corporation. Many of the docstrings from openinterface.py, particularly those which describe the specification, are also used here. Also, docstrings may contain specification text copied directly from the Roomba SCI Spec Manual and the Turtlebot Open Interface specification. Since SCI is a subset of OI, PyRobot first defines the Roomba's functionality in the Roomba class and then extends that with the Turtlebot's additional functionality in the Turtlebot class. In addition, since OI is built on SCI the SerialCommandInterface class is also used for OI. """ __author__ = "[email protected] (Damon Kohler)" import logging from lagerlogger import LagerLogger logging = LagerLogger() logging.console(LagerLogger.DEBUG) import math import serial import struct import time import threading import traceback import rospy ROOMBA_OPCODES = dict( start=128, reset=7, baud=129, control=130, safe=131,
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 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)