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, 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, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("")
self.logger.console(LagerLogger.DEBUG)
self.conv_stack = ConversationStack()
self.telemetry_pub = self.ros.Publisher("%s/telemetry" %
self.robot_name,
Telemetry,
queue_size=10)
if config.has("stage",
"enabled") and config["stage"]["enabled"] == "True":
self.logger.info("Using Stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.debug("Using Hardware")
from vacuum_interfaces.fake_interface import FakeInterface
self.robot = FakeInterface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.phone_link.set_visibility(False)
m = self.conv_stack.add_message(
"This robot has been disabled due to problems detect in the onboard electronics. We apologize for the inconvenience."
)
m.add_response("safe", lambda: self.test_reply("safe"))
m.add_response("dangerous", lambda: self.test_reply("dangerous"))
m.add_response("help", lambda: self.test_reply("help"))
m.add_response("off", lambda: self.test_reply("off"))
self.phone_link.set_progression(self.conv_stack)
self._telemetry_timer = Timer(
1, lambda x=fake_telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
def __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, 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, 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()
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,
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):
# 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, 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 __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 __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 __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 __init__(self, behavior_stack):
self.logr = LagerLogger(self.__class__.__name__)
self.logr.console(LagerLogger.DEBUG)
self.behavior_stack = behavior_stack
def __init__(self):
self.logger = LagerLogger("FakePhoneLink")
self.logger.console(LagerLogger.DEBUG)
self.logger.warn("~~~ WARNING: USING FAKE PHONE LINK ~~~")
self._state = "dangerous"
self._enabled = False
def __init__(self):
super(VacuumState, self).__init__()
self.logr = LagerLogger()
self.logr.console(LagerLogger.DEBUG)
self.callbacks = list()
from generic_vacuum import VacuumInterface
import copy
import sys
import time
import threading
import RPi.GPIO as GPIO
from gpio_sensor import gpio_sensor
from timer import Timer
import logging
from threading import Lock
from vacuum_experiment_msgs.telemetry import TelemetryData
from lagerlogger import LagerLogger
logr = LagerLogger("")
logr.console(LagerLogger.DEBUG)
#
# /) ,
# _(/ _ _ ____ _ _ __
# (_(__(_/_)_(__(_) (/___(/_/ (_(_/_
# .-/
# (_/
DROPWHEEL = 29
DUSTBIN = 16
POWER = 33
SPOT = 31
CLEAN = 35
MAX = 37
BASE = 18
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 __init__(self, *args):
dict.__init__(self, args)
self.logger = LagerLogger("RobotConfig")
self.logger.console(LagerLogger.DEBUG)
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 __init__(self):
super(BehaviorStack, self).__init__()
self.logr = LagerLogger("BehaviorStack")
self.logr.console(LagerLogger.DEBUG)
rconfig = RobotConfig()
# Load settings from config file
if mainargs.config:
rconfig.parse_config_file(mainargs.config)
# Load command line args
rconfig.parse_args(others)
# Make sure we have all the mandatory settings
rconfig.verify()
# Setup ROS_MASTER_URI
if "ros_master_uri" in rconfig["robot"]:
os.environ['ROS_MASTER_URI'] = rconfig['robot']['ros_master_uri']
# Load Robot Controller and pass RobotConfig into it
logr = LagerLogger("main")
logr.console(LagerLogger.DEBUG)
logr.debug("=============================================================")
logr.info(rconfig['robot']['controller_file'])
cfg_file_path = os.path.dirname(os.path.realpath(__file__))
cfg_file_path = os.path.join(cfg_file_path,
rconfig['robot']['controller_file'])
imp.load_source("rlib", cfg_file_path)
from rlib import RobotController
robotcontroller = RobotController(rconfig)
print logging.Logger.manager.loggerDict.keys()
# logging.getLogger('ReliableTopicConn').setLevel(LagerLogger.ERROR)
if "loglevels" in rconfig:
for name, level in rconfig["loglevels"].items():
logr.debug("Setting log level for '%s' to '%s'" % (name, level))
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,
def __init__(self, *argv, **kwds):
ReliableTopicConn.__init__(self, rospy.Publisher, *argv, **kwds)
self.logr = LagerLogger("ReliablePublisher")
self.logr.console(LagerLogger.DEBUG)
