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)
