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()
def __init__(self, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self.controller.ros.Subscriber(
"/%s/fail/dustbin" % self.controller.robot_name, Bool,
self._ros_ctrl)
# Repeats the warning sound every 15 seconds
self.sound_timer = Timer(15, self._sound_timer_cb)
# Flashes the ring light
self.light_timer = Timer(1, self._light_timer_cb)
self.debris_light_on = True
self._active = False
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, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self.controller.ros.Subscriber(
"/%s/fail/reset" % self.controller.robot_name, Bool,
self._ros_ctrl)
self.timer = None #Timer(7, self._timer_cb)
# Repeats the warning sound every 15 seconds
self.sound_timer = Timer(15, self._sound_timer_cb)
# Flashes the ring light
self.light_timer = Timer(1, self._light_timer_cb)
self.ring_light_on = True
self._last_buttons = None
self._behavior_active = False
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, 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 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 BenderInterface(VacuumInterface):
def __init__(self, config):
VacuumInterface.__init__(self)
self.logr = LagerLogger("")
self.logr.console(LagerLogger.DEBUG)
self._dev_path = config['robot']['robot_dev']
self.robot = AdvancedRoomba(config)
self.robot.start(self._dev_path, 115200)
self.robot.passive()
self._telemetry = TelemetryData()
# Keep Alive Code
#setup GPIO to reference pins based the board
GPIO.setmode(GPIO.BOARD)
#device_detect pin is the pin we set low to turn on the robot, we must
#set it to out mode for rasing high and low
GPIO.setup(DEVICE_DETECT, GPIO.OUT)
#keep alive thread
self._keep_alive_timer = Timer(1, self._keep_alive)
self._keep_alive_timer.start()
# Dust-bin detection
gpio_sensor(DUSTBIN, self.gpio_dustbin_cb)
# Callback functions
self._callbacks = dict() # name=(cmp_fun, cb)
self._sensor_update_timer = Timer(.1, self.poll_sensors_cb)
self._sensor_update_timer.start()
def reg_sensor_cb(self, sensor_name, cb, cmp_fun=None):
if sensor_name not in self._callbacks.keys():
self._callbacks[sensor_name] = list()
if cmp_fun is None:
cmp_fun = lambda old, new: not old.__eq__(new)
if (cmp_fun, cb) in self._callbacks[sensor_name]:
self.logr.warn("THIS HAS ALREADY BEEN DONE!?")
self._callbacks[sensor_name] = (cmp_fun, cb)
def gpio_dustbin_cb(self, value):
""" update the dustbin status """
if not value == self._telemetry["dustbin"]:
self._telemetry["dustbin"] = value
def get_sensor(self, key):
return self.robot.get_sensor(key)
def passive(self):
self.robot.passive()
def is_docked(self):
return self._telemetry["docked"]
def is_charging(self):
return self._telemetry["charging"]
def is_cleaning(self):
return self._telemetry["cleaning"]
def dock(self):
self.robot.dock()
def clean(self):
self.robot.clean()
def pause(self):
self.robot.pause()
def _keep_alive(self):
""" Keep alive timer callback. Throws a gpio pin up and down """
GPIO.output(DEVICE_DETECT, GPIO.LOW)
time.sleep(0.1)
GPIO.output(DEVICE_DETECT, GPIO.HIGH)
def poll_sensors_cb(self):
""" Poll sensors from hardware and do callbacks"""
old_sensors = dict()
for sensor_name in self._callbacks.keys():
try:
old_sensors[sensor_name] = self.robot.get_sensor(sensor_name)
except:
old_sensors[sensor_name] = None
try:
self.robot.update_sensors()
except BadDataLengthError as e:
self.logr.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
return
except DriverError as e:
self.logr.warn(e)
self._telemetry["sci_error"] = True
except Exception as e:
self.logri.error(e)
# print "Restarting SCI into control() mode"
# self.robot.control()
# WHEN THIS HAPPENS - the robot seems to have turned off.
# On dirtdog this would happen when the person presses
# the power button while cleaning.
return
# Update telemetry
self._update_telemetry()
# Do callbacks
for sensor_name, (cmp_fun, cb) in self._callbacks.items():
new_sensor_val = self.robot.get_sensor(sensor_name)
if old_sensors[sensor_name] is None or cmp_fun(
old_sensors[sensor_name], new_sensor_val):
cb()
def _update_telemetry(self):
# If sci error has been set (by poll_sensors_cb), undo it
if self._telemetry["sci_error"]:
self._telemetry["sci_error"] = False
# Compare charging status
charging = self.robot.get_sensor("charging_state")
charging = False if charging in ["not_charging", "error"] else True
if not charging == self._telemetry["charging"]:
self._telemetry["charging"] = charging
# See if we are on the dock
sources = self.robot.get_sensor("charging_sources_available")
if not sources["base"] == self._telemetry["docked"]:
self._telemetry["docked"] = sources["base"]
# see if the robot has been picked up
wheel_drops = self.robot.get_sensor("bumps_wheeldrops")
lifted = (wheel_drops["wheeldrop_left"]
or wheel_drops["wheeldrop_right"])
if not lifted == self._telemetry["lifted"]:
self._telemetry["lifted"] = lifted
# Check if the sweeper brush is running (that means we are cleaning)
cleaning = self.robot.get_sensor("main_brush_current") >= 1
if not cleaning == self._telemetry["cleaning"]:
self._telemetry["cleaning"] = cleaning
# Check the status of button presses
buttons = self.robot.get_sensor("buttons")
buttons = [key for key, pressed in buttons.items() if pressed]
if not set(self._telemetry["buttons"]) == set(buttons):
self._telemetry["buttons"] = buttons
# Check the robot voltage
voltage = self.robot.get_sensor("voltage") / 1000.0
if abs(voltage - self._telemetry["battery_voltage"]) >= 0.1:
self._telemetry["battery_voltage"] = voltage
def set_telemetry_cb(self, fun):
self._telemetry.set_telemetry_update_cb(fun)
def terminate(self):
self._keep_alive_timer.cancel()
self.logr.info("keep alive killed")
self._sensor_update_timer.cancel()
self.logr.info("killed sensor update timer")
time.sleep(1)
GPIO.cleanup()
self.robot.close()
self.logr.info("Clean shutdown achieved - cheers!")
class RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("controller")
self.logger.console(LagerLogger.DEBUG)
self.conv_stack = ConversationStack()
self.telemetry_pub = self.ros.Publisher("%s/telemetry" % self.robot_name, Telemetry, latch=True, queue_size=10)
self.ros_cmd_sub = self.ros.Subscriber("%s/cmd" % self.robot_name, String, self.ros_cmd_cb)
if config.has("stage","enabled") and config["stage"]["enabled"] == "True":
self.logger.warn("using stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.info("Using hardware")
from vacuum_interfaces.roomba500_interface import Roomba500Interface
self.robot = Roomba500Interface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.phone_link.set_visibility(False)
self._telemetry_timer = Timer(5, lambda x=self.robot._telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
self.robot.passive()
self.logger.info("Robot Initialized")
self.ros.enable()
self.phone_link.enable()
def experiment_init(self):
""" Callbed by robot controller after a new config has been loaded """
self.logger.info("%s Loading Behavior Profile: %s" % (self.robot_name, self.behavior_profile))
if self.behavior_profile.is_disabled():
self.phone_link.set_visibility(False)
elif self.behavior_profile.is_easy():
self.set_stack_idle()
elif self.behavior_profile.is_help():
self.phone_link.set_state("help")
else:
self.logger.error("Received unexpected behavior profile")
return
# Initialize controller
if self.experiment_state:
if self.experiment_state.get() in [ExperimentState.STANDBY, ExperimentState.READY]:
self.controller_state.set_ready()
elif self.experiment_state.is_running():
self.logger.warn("Received configuration after experiment has started")
self.logger.warn("Set to running")
self.start_run()
# If we are coming online while paused, start the robot and set to paused
elif self.experiment_state.is_paused():
self.logger.warn("Received configuration after experiment has started")
self.logger.warn("Starting, then setting to pause")
self.start_run()
self.controller_state.set_paused()
else:
self.controller_state.set_standby()
else:
self.logger.warn("Received configuration, but no experiment state")
def experiment_state_standby(self):
self.reset_controller()
def experiment_state_ready(self): pass
def experiment_state_running(self):
if not self.behavior_profile:
self.logger.warn("Experiment Running, but still waiting for config")
return
# If we are in ready state, start things off!
if self.controller_state.is_ready():
self.controller_state.set_running()
self.start_run()
elif self.controller_state.is_paused():
self.controller_state.set_running()
def experiment_state_paused(self):
self.controller_state.set_paused()
def experiment_state_complete(self):
self.phone_link.set_visibility(False)
self.controller_state.set_paused()
def shutdown(self):
""" Runs when the robot controller is shutting down """
# self.charging_timer.cancel()
self.logger.warning("Shutting down controller")
self._telemetry_timer.cancel()
self.phone_link.set_visibility(False)
# self.robot.pause()
self.ros.shutdown()
self.robot.terminate()
time.sleep(2)
self.phone_link.disable()
################################################################################
def reset_controller(self):
self.logger.warn("Resetting controller")
def start_run(self):
self.logger.info("starting run")
def ros_cmd_cb(self, data):
""" Receive commands from ROS """
if data.data == "clean":
self.logger.warn("ROS: clean()")
self.robot.clean()
elif data.data == "dock":
self.logger.warn("ROS: dock()")
self.robot.dock()
elif data.data == "pause":
self.logger.warn("ROS: pause()")
self.robot.pause()
def telemetry_cb(self, values):
""" Sends telemetry information """
t = Telemetry()
t.battery_voltage = values["battery_voltage"]
t.lifted = values["lifted"]
t.charging = values["charging"]
t.docked = values["docked"]
t.dustbin = values["dustbin"]
t.cleaning = values["cleaning"]
t.buttons = values["buttons"]
t.sci_error = values["sci_error"]
t.api_mode = values["api_mode"]
self.telemetry_pub.publish(t)
class 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 RobotController(BaseRobotController):
def __init__(self, config):
BaseRobotController.__init__(self, config)
self.logger = LagerLogger("")
self.logger.console(LagerLogger.DEBUG)
self.conv_stack = ConversationStack()
self.telemetry_pub = self.ros.Publisher("%s/telemetry" %
self.robot_name,
Telemetry,
queue_size=10)
if config.has("stage",
"enabled") and config["stage"]["enabled"] == "True":
self.logger.info("Using Stage")
from vacuum_interfaces.stage import StageInterface
self.robot = StageInterface(None, config)
else:
self.logger.debug("Using Hardware")
from vacuum_interfaces.fake_interface import FakeInterface
self.robot = FakeInterface(self.config)
self.robot.set_telemetry_cb(self.telemetry_cb)
self.phone_link.set_visibility(False)
m = self.conv_stack.add_message(
"This robot has been disabled due to problems detect in the onboard electronics. We apologize for the inconvenience."
)
m.add_response("safe", lambda: self.test_reply("safe"))
m.add_response("dangerous", lambda: self.test_reply("dangerous"))
m.add_response("help", lambda: self.test_reply("help"))
m.add_response("off", lambda: self.test_reply("off"))
self.phone_link.set_progression(self.conv_stack)
self._telemetry_timer = Timer(
1, lambda x=fake_telemetry: self.telemetry_cb(x))
self._telemetry_timer.start()
def initialize(self):
""" This is the only function executed explicitly by the robot controller """
self.logger.info("Robot Initialized")
self.phone_link.set_visibility(
True
) # Start showing the robot even when the experiment is not running
self.phone_link.enable()
self.ros.enable()
def experiment_init(self):
""" Callbed by robot controller after a new config has been loaded """
self.logger.info("%s Loaded Behavior Profile: %s" %
(self.robot_name, self.behavior_profile))
# if self.behavior_profile.is_dead():
# # Queue up the state for this robot, but don't show it yet
# self.phone_link.enable()
# self.phone_link.set_state("off")
def experiment_state_standby(self):
self.reset_controller()
def experiment_state_ready(self):
pass
def experiment_state_running(self):
# If we are in ready state, start things off!
if self.controller_state.is_ready():
self.start_run()
elif self.controller_state.is_paused():
self.controller_state.set_running()
def experiment_state_paused(self):
self.phone_link.set_visibility(True)
self.controller_state.set_paused()
def experiment_state_complete(self):
self.phone_link.set_visibility(False)
self.controller_state.set_paused()
def shutdown(self):
""" Runs when the robot controller is shutting down """
# self.charging_timer.cancel()
# time.sleep(.25)
# self.robot.pause()
self._telemetry_timer.cancel()
time.sleep(.25)
self.ros.shutdown()
self.robot.terminate()
self.phone_link.disable()
################################################################################
def reset_controller(self):
self.logger.warn("Resetting controller")
self.phone_link.set_state("safe")
self.phone_link.set_visibility(False)
self.behavior_profile.unset()
self.controller_state.set_standby()
def start_run(self):
self.logger.info("starting run")
self.phone_link.set_visibility(True)
def test_reply(self, state):
self.phone_link.set_state(state)
def clean(self):
self.logger.warn("CLEAN?")
def telemetry_cb(self, values):
""" Sends telemetry information """
self.logger.debug("telemetry")
t = Telemetry()
t.lifted = values["lifted"]
t.charging = values["charging"]
t.docked = values["docked"]
t.dustbin = values["dustbin"]
t.cleaning = values["cleaning"]
t.buttons = values["buttons"]
self.telemetry_pub.publish(t)
self.logger.debug(str(t))
def _init_timer(self):
if self.timer:
self.timer.cancel()
self.timer = None
self.timer = Timer(7, self._timer_cb)
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)
def __init__(self, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self._cleaning_timer = Timer(120, self._cleaning_timeout)
self._buttons_lock = Lock()
self._last_buttons = list()
class NormalBehavior(RobotBehavior):
def __init__(self, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self._cleaning_timer = Timer(120, self._cleaning_timeout)
self._buttons_lock = Lock()
self._last_buttons = list()
def cancel(self):
self._cleaning_timer.cancel()
RobotBehavior.cancel(self)
def vacuum_state_cb(self, vacuum_state):
""" Listen to state from the robot and set the phone response """
if vacuum_state.is_idle() or vacuum_state.is_docked():
self.logr.debug("canceling the cleaning timer")
self._cleaning_timer.cancel()
self.controller.set_stack_idle()
elif vacuum_state.is_cleaning():
self.controller.set_stack_cleaning()
self.logr.debug("starting the cleaning timer")
self._cleaning_timer.start()
elif vacuum_state.is_docking():
self.logr.debug("canceling the cleaning timer")
self._cleaning_timer.cancel()
self.controller.set_stack_docking()
def dustbin_cb(self, detected):
pass
def hw_buttons_cb(self, buttons):
with self._buttons_lock:
if (("clean" in buttons and "clean" not in self._last_buttons) or
("spot" in buttons and "spot" not in self._last_buttons)
) and self.controller.robot.is_charging():
self.logr.debug("hw press: making sure we leave the dock")
import threading
threading.Timer(1, self._check_clean_started).start()
self._last_buttons = buttons
def _cleaning_timeout(self):
self.logr.info("Cleaning timeout hit!")
if self.controller.behavior_profile.is_easy():
self.controller.behavior_dustbin.start()
else:
self.controller.robot.dock()
self.logr.debug("canceling the cleaning timer")
self._cleaning_timer.cancel()
def ros_cmd_cb(self, data):
""" Receive commands from ROS """
if data.data == "clean":
self.logr.warn("ROS: clean()")
self.cmd_clean()
elif data.data == "dock":
self.logr.warn("ROS: dock()")
self.cmd_dock()
def cmd_dock(self):
""" Send the robot to its docking station """
self.logr.info("dock()")
# Do not call passive first. it stops a cleaning robot, which then will not dock
self.controller.robot.dock(
) # This will cause the idle stack to get set
self.controller.set_stack_docking()
def docked_cb(self):
self.logr.info("Robot has docked")
def cmd_pause(self):
""" Tell the robot to pause what it is doing """
self.logr.info("cmd_pause()")
self.controller.robot.pause()
def cmd_clean(self):
""" Tell the robot to clean the floor """
self.logr.info("clean()")
self.controller.robot.passive()
time.sleep(.50)
self.controller.robot.clean()
# Roomba600 fix for going quite while charging
# The problem is we have to literally press the button twice, sometimes.
# If we are charging, check a second later to see if we are not longer charging.
# If the clean command was successful, we won't be charging. If it was
# not successful, we will still be charging and need to press clean again.
if self.controller.robot.is_charging():
self.logr.debug("making sure we leave the dock")
import threading
threading.Timer(1, self._check_clean_started).start()
def _check_clean_started(self):
self.logr.debug("Checking that we left the dock!")
if self.controller.robot.is_charging():
self.logr.info("Robot is still charging, so pressing clean again!")
self.controller.robot.clean(no_state_change=True)
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()
class ResetBehavior(RobotBehavior):
def __init__(self, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self.controller.ros.Subscriber(
"/%s/fail/reset" % self.controller.robot_name, Bool,
self._ros_ctrl)
self.timer = None #Timer(7, self._timer_cb)
# Repeats the warning sound every 15 seconds
self.sound_timer = Timer(15, self._sound_timer_cb)
# Flashes the ring light
self.light_timer = Timer(1, self._light_timer_cb)
self.ring_light_on = True
self._last_buttons = None
self._behavior_active = False
def _init_timer(self):
if self.timer:
self.timer.cancel()
self.timer = None
self.timer = Timer(7, self._timer_cb)
def start(self):
if not self._behavior_active:
# if self.timer:
# self.timer.cancel()
# self.timer = None
# self.timer = Timer(10, self._timer_cb)
self._init_timer()
self._behavior_active = True
self.cmd_reset()
self.controller.robot.robot.set_leds(["check"], 255, 255)
self.controller.robot.robot.play_song(0)
self.sound_timer.start()
self.light_timer.start()
RobotBehavior.start(self)
else:
self.logr.warn("Reset behavior already started")
def cancel(self):
self._behavior_active = False
self.sound_timer.cancel()
self.light_timer.cancel()
RobotBehavior.cancel(self)
self.controller.robot.pause()
def _ros_ctrl(self, data):
self.logr.debug("Heard Command from ROS: %s" % str(data.data))
if data.data:
self.start()
else:
self.cancel()
def hw_buttons_cb(self, buttons):
# Clean pressed down
if ((not self._last_buttons) or
(not (self._last_buttons["clean"] or self._last_buttons["spot"]))
) and (buttons["clean"] or buttons["spot"]):
self.logr.debug("Button Pressed, Starting Timer!")
self.timer.start()
self.light_timer.cancel()
self.sound_timer.cancel()
self.controller.robot.robot.set_leds(["check"], 255, 255)
# Clean button lifted
elif self._last_buttons and (
(self._last_buttons["clean"] and not buttons["clean"]) or
(self._last_buttons["spot"] and not buttons["spot"])):
self.logr.debug("Button Released, Killing Timer!")
self.timer.cancel()
if not self._behavior_active:
# End this behavior!
self.cancel()
else:
try:
self.sound_timer.start()
except:
self.sound_timer.cancel()
self.sound_timer.start()
try:
self.light_timer.start()
except:
self.light_timer.cancel()
self.light_timer.start()
self._last_buttons = buttons
def _timer_cb(self):
""" This happens when the button has been held down for the correct length of time """
self.logr.debug("Reset Achieved!")
self.sound_timer.cancel()
self.light_timer.cancel()
if self._behavior_active:
self.controller.robot.robot.play_song(2)
self.controller.robot.robot.set_leds([], 0, 255)
self.controller.phone_link.set_state("ok", update=False)
if self.controller.conv_stack[-1].tag == "reset":
self.controller.conv_stack[-1].set_selection("0")
m = self.controller.conv_stack.add_message(
"Thank you for resetting me!")
self.controller.phone_link.set_progression(
self.controller.conv_stack)
self._behavior_active = False
self.timer._time = 1
# self.controller.conv_stack.clear()
# self.controller.conv_stack.add_message("Robot reset!")
# self.controller.phone_link.set_state("ok", update=False)
# self.controller.phone_link.set_progression(self.controller.conv_stack)
# self.timer.cancel()
def _sound_timer_cb(self):
self.controller.robot.robot.play_song(0)
def _light_timer_cb(self):
if self.ring_light_on:
self.controller.robot.robot.set_leds(["check"], 255, 0)
self.ring_light_on = False
else:
self.controller.robot.robot.set_leds(["check"], 255, 255)
self.ring_light_on = True
def vacuum_state_cb(self, vacuum_state):
self.cmd_reset()
def cmd_clean(self):
self.cmd_reset()
def cmd_pause(self):
self.cmd_reset()
def cmd_dock(self):
self.cmd_reset()
def cmd_reset(self):
self.controller.robot.control()
if not self.controller.conv_stack or not self.controller.conv_stack[
0].tag == "reset":
self.controller.conv_stack.clear()
# m = self.controller.conv_stack.add_message("Reset Required.<br/><br/>Please hold down the 'Clean' button for 10 seconds")
m = self.controller.conv_stack.add_message(
"Hardware Reset Required. Could you please help me?<br/><br/>I need to be reset! Could you please hold down the 'Clean' button for 10 seconds."
)
m.tag = "reset"
m.set_popup()
m.add_response("Ok", self.reply_ok)
m.add_response("Cancel", self.reply_cancel)
self.controller.phone_link.set_state("help", update=False)
self.controller.phone_link.set_progression(self.controller.conv_stack)
def reply_ok(self):
""" person replied 'ok' to request for help """
if self.controller.conv_stack[-1].tag == "cancel":
self.controller.conv_stack.pop()
if self.controller.conv_stack[-1].tag == "reset":
self.controller.conv_stack[-1].unset_popup()
m = self.controller.conv_stack.add_message(
"Thank you! Waiting for reset...")
m.tag = "ok"
self.controller.phone_link.set_progression(self.controller.conv_stack)
def reply_cancel(self):
""" person replied 'cancel' to request for help """
if self.controller.conv_stack[-1].tag == "ok":
self.controller.conv_stack.pop()
if self.controller.conv_stack[-1].tag == "reset":
self.controller.conv_stack[-1].unset_popup()
self.controller.phone_link.set_progression(self.controller.conv_stack)
class DustbinBehavior(RobotBehavior):
def __init__(self, controller):
RobotBehavior.__init__(self, controller.behavior_stack)
self.controller = controller
self.controller.ros.Subscriber(
"/%s/fail/dustbin" % self.controller.robot_name, Bool,
self._ros_ctrl)
# Repeats the warning sound every 15 seconds
self.sound_timer = Timer(15, self._sound_timer_cb)
# Flashes the ring light
self.light_timer = Timer(1, self._light_timer_cb)
self.debris_light_on = True
self._active = False
def _ros_ctrl(self, data):
self.logr.debug("Heard Command from ROS: %s" % str(data.data))
if data.data:
self.start()
else:
self.cancel()
def start(self):
super(DustbinBehavior, self).start()
if self.controller.robot.is_docked():
self.begin_dustbin()
elif not self.controller.robot.is_docking():
self.logr.info("Telling robot to dock")
self.controller.robot.dock()
def cancel(self):
self._active = False
RobotBehavior.cancel(self)
self.controller.robot.pause()
self.sound_timer.cancel()
self.light_timer.cancel()
def begin_dustbin(self):
""" called after the robot is on its dock, to start asking the user
to empty the dustbin """
if self._active:
self.logr.warn("dustbin already begun!")
return
self._active = True
if not self.controller.robot.is_docked():
self.logr.error("oops")
self.set_stack_idle_help()
self.controller.robot.control()
self.controller.robot.robot.set_leds(["debris"], 128, 255)
self.controller.robot.robot.play_song(1)
self.sound_timer.start()
self.light_timer.start()
def vacuum_state_cb(self, vacuum_state):
if vacuum_state.is_cleaning():
self.logr.warn(
"Intercepting Cleaning Activity, Telling robot to dock")
self.controller.robot.pause()
elif vacuum_state.is_idle():
if self.controller.robot.is_lifted():
self.begin_dustbin()
else:
self.controller.robot.dock()
elif vacuum_state.is_docking():
self.set_stack_docking()
# elif vacuum_state.is_docked():
# self.begin_dustbin()
# self.controller.robot.control()
# self.set_stack_idle_help()
def docked_cb(self):
if not self._active:
self.begin_dustbin()
# self.controller.robot.control()
# self.set_stack_idle_help()
def cmd_clean(self):
self.logr.info("HW CLEAN BUTTON HEARD!")
# self.controller.set_stack_cleaning(False)
# self.cmd_dustbin()
def cmd_dustbin(self):
self.logr.debug("cmd_dustbin()")
if self.controller.conv_stack and not self.controller.conv_stack[
-1].tag in ["dustbin", "dustbin_response"]:
m = self.controller.conv_stack.add_message(
"Cleaning Paused.<br/><br/>Please empty my dustbin.")
m.tag = "dustbin"
m.add_response("Ok", self.heard_ok)
m.add_response("Cancel", self.controller.set_stack_idle)
self.controller.phone_link.set_state("help", update=False)
self.controller.phone_link.set_progression(
self.controller.conv_stack)
def heard_ok(self):
self.logr.info("User agreed!")
if not self.controller.conv_stack[-1].tag == "dustbin":
self.logr.error(
"That last thing on the conversation stack was not the dustbin"
)
self.controller.phone_link.set_progression(
self.controller.conv_stack)
return
m = self.controller.conv_stack.add_message(
"Waiting for dustbin to be emptied.")
m.tag = "dustbin_response"
self.controller.phone_link.set_progression(self.controller.conv_stack)
def dustbin_cb(self, detected):
if not detected:
self.logr.debug("dustbin removed")
self.sound_timer.cancel()
self.light_timer.cancel()
self.controller.robot.robot.set_leds([], 0, 255)
return
self.logr.debug("dustbin replaced")
self.controller.robot.robot.play_song(2)
m = self.controller.conv_stack.add_message(
"Thank you! Begin Cleaning?")
m.add_response("Ok", self.controller.cmd_clean)
m.add_response("Cancel", self.controller.set_stack_idle)
self.controller.phone_link.set_state("ok", update=False)
self.controller.phone_link.set_progression(self.controller.conv_stack)
self._active = False
self.cancel()
def set_stack_idle_help(self):
self.logr.debug("set_stack_idle()")
self.controller.conv_stack.clear()
docked = self.controller.robot.is_docked()
state = "Docked" if docked else "Idle"
m = self.controller.conv_stack.add_message(
"%s.<br/><br/>Please empty my dustbin." % state)
m.tag = "dustbin"
m.add_response("Ok", self.heard_ok)
m.add_response("Cancel", self.controller.set_stack_idle)
# if not docked:
# m.add_response("Dock", self.controller.behavior_stack.cmd_dock)
self.controller.phone_link.set_state("help", update=False)
self.controller.phone_link.set_progression(self.controller.conv_stack)
def set_stack_docking(self):
""" special docking message that prevents re-cleaning """
self.logr.debug("set_stack_docking()")
self.controller.conv_stack.clear()
m = self.controller.conv_stack.add_message(
"Looking for base station...")
self.controller.phone_link.set_state("ok", update=False)
self.controller.phone_link.set_progression(self.controller.conv_stack)
def _sound_timer_cb(self):
self.controller.robot.robot.play_song(1)
def _light_timer_cb(self):
if self.debris_light_on:
self.controller.robot.robot.set_leds(["debris"], 64, 128)
self.debris_light_on = False
else:
self.controller.robot.robot.set_leds([], 64, 128)
self.debris_light_on = True