Source code for topo.hardware.playerrobot

"""
High-level interface to the Player client libraries.

The Player client libraries allow Python code to communicate with
hardware devices such as robots, cameras, and range sensors.

This is a temporary home for this file until it finds a permanent home
(maybe in the PlayerStage project or in PLASTK?)
"""


import time,array

from threading import RLock, Thread
from Queue import Queue

from operator import eq,ne
from math import pi

# Since this module ships with Topographica only as a sample and is not
# intended to be run, the following SkipTest statement was included to allow
# nose to handle this module (when looking for doctests) without raising an
# import error. If the file is moved elsewhere -- or run on its own, 
# assuming the PlayerStage package has been installed -- the below code should
# be reworked to either import the package directly, or handle the import error
# differently. For more information on how nose works, see topo/tests/README.

try:
    import playerc
    SKIP = False
except ImportError:
    SKIP = True

# JPALERT: Because of the global interpreter lock in Python, using
# Python threads (via the 'thread' or 'threading' modules does not
# necessarily provide low-latency polling of the player process.  In
# particular, long-running native functions (e.g. C/C++ foreign
# functions) will not be pre-empted, so the polling loop won't
# get a timeslice until they complete.  A better solution, especially
# on multicore machines, is true preemptive multiprocessing.  The
# 'processing' module should provide that, but it doesn't yet work
# properly on MacOS, and I haven't tested it yet on linux.  God knows
# what will happen on Windows.

[docs]def use_processing(): """ Configure the module to use the processing library for asynchronous process support. Use of the processing library requires the use of queues for communication with robot devices. """ import processing global RLock, Thread, Queue RLock = processing.RLock # pyflakes:ignore (optional alternative) Thread = processing.Process # pyflakes:ignore (optional alternative) Queue = processing.Queue # pyflakes:ignore (optional alternative)
[docs]def use_threading(): """ Configure the module to use the threading library for asynchronous process support. (the default) """ import threading global RLock, Thread, Queue RLock = threading.RLock # pyflakes:ignore (optional alternative) Thread = threading.Thread # pyflakes:ignore (optional alternative) Queue = Queue.Queue # pyflakes:ignore (optional alternative) # JPALERT This is a HACK for the CVS version of Player, this value # should be defined in the playerc module:
if not SKIP: playerc.PLAYERC_OPEN_MODE = 1 class PlayerException(Exception): pass
[docs]def player_fn(error_op=ne,error_val=0): """ Player function decorator. Adds error checking. Takes an operator and a value, and compares the result of the function call with the value using the operator. If the result is true, a PlayerException is raised. The default error condition is error_op = ne, error_value = 0, which raises an exception if fn(*args) != 0. """ def wrap(fn): def new_fn(*args): if error_op(fn(*args),error_val): raise PlayerException(playerc.playerc_error_str()) return new_fn return wrap
[docs]def synchronized(lock): """ Simple synchronization decorator. Takes an existing lock and synchronizes a function or method on that lock. Code taken from the Python Wiki PythonDecoratorLibrary: http://wiki.python.org/moin/PythonDecoratorLibrary """ def wrap(f): def newFunction(*args, **kw): lock.acquire() try: return f(*args, **kw) finally: lock.release() return newFunction return wrap
[docs]def synched_method(f): """ Synchronized method decorator. Like synchronized() decorator, except synched_method assumes that the first argument of the function is an instance containing a Lock object, and this lock is used for synchronization. """ def newFunction(self,*args,**kw): self._lock.acquire() try: return f(self,*args, **kw) finally: self._lock.release() return newFunction
[docs]class PlayerObject(object): """ A generic threadsafe wrapper for client and proxy objects from the playerc library. PlayerObject wrappers are constructed automatically by PlayerRobot objects. Each PlayerObject instance wraps a playerc device proxy or client object, and publishes a thread-safe version of each of proxy's methods, that is synchronized with the PlayerRobot instance's run-loop thread, and that catches playerc error conditions and raises them as PlayerExceptions. The original playerc proxy object is available via the attribute .proxy. Specialized subclasses of PlayerObject can have additional interfaces for getting device state or setting commands specific to that device. Developer note: the PlayerObject base class __init__ method automatically wraps each method on the proxy that (a) doesn't begin with '__' and (b) is not already in dir(self). This way, subclasses can override the wrapping process by defining their own wrappers *before* the base class __init__ method is called. """ def __init__(self,proxy,lock): self._lock = lock self.proxy = proxy for name in dir(proxy): attr = getattr(proxy,name) if name not in dir(self) and name[:2] != '__' and callable(attr): setattr(self,name,synchronized(lock)(player_fn()(attr))) self.cmd_queue = Queue() def process_queues(self): while not self.cmd_queue.empty(): name,args = self.cmd_queue.get() try: print "Doing command:",name,args getattr(self,name)(*args) finally: self.cmd_queue.task_done()
[docs]class PlayerClient(PlayerObject): """ Player object wrapper for playerc.client objects. """ def __init__(self,proxy,lock): # Override the wrapper on playerc.client.read, because # it returns None for errors, instead of returning 0 for # "no error." self.read = synchronized(lock)(player_fn(eq,None)(proxy.read)) super(PlayerClient,self).__init__(proxy,lock) def process_queues(self): pass
[docs]class PlayerDevice(PlayerObject): """ Generic Player device object. Overrides the default proxy .subscribe method so that the mode defaults to PLAYERC_OPEN_MODE. """ @synched_method @player_fn() def subscribe(self, mode=None): mode = playerc.PLAYERC_OPEN_MODE if None else mode return self.proxy.subscribe(mode)
[docs]class PTZDevice(PlayerDevice): """ Player Pan/Tilt/Zoom (PTZ) device. Adds the following to the original proxy interface: state = The tuple (pan,tilt,zoom) indicating the current state of the PTZ device. state_deg = Same as state, but returns values in degrees instead of radians set_deg() and set_ws_deg() methods. Same as .set() and .set_ws(), using degrees instead of radians. """ def get_state(self): return self.proxy.pan, self.proxy.tilt, self.proxy.zoom state = property(get_state) def get_state_deg(self): return self.proxy.pan*180/pi, \ self.proxy.tilt*180/pi, \ self.proxy.zoom*180/pi state_deg = property(get_state_deg) def set_deg(self,pan,tilt,zoom): self.set(pan*pi/180, tilt*pi/180, zoom*pi/180) def set_ws_deg(self,pan,tilt,zoom,pan_speed,tilt_speed): self.set_ws(pan*pi/180, tilt*pi/180, zoom*pi/180,pan_speed*pi/180,tilt_speed*pi/180)
[docs]class CameraDevice(PlayerDevice): """ A Player camera device. The synchronized method get_image grabs an uncompressed snapshot, along with the additional formatting information needed to make an image. """ def __init__(self,proxy,lock): self.decompress = synchronized(lock)(player_fn(ne,None)(proxy.decompress)) super(CameraDevice,self).__init__(proxy,lock) self.image_queue = Queue() def process_queues(self): im = self.image # check to make sure it's really an image if im[1] > 0: self.image_queue.put(im) super(CameraDevice,self).process_queues() # @synched_method
[docs] def get_image(self): """ Returns the tuple: (format,width,height,bpp,fdiv,data) Where data is a copy of the uncompressed image data. """ if self.proxy.compression: self.decompress() im_array = array.array('B') im_array.fromstring(self.proxy.image[:self.proxy.image_count]) return self.proxy.format, \ self.proxy.width, \ self.proxy.height, \ self.proxy.bpp, \ self.proxy.fdiv, \ im_array
image = property(get_image) ################## # DEVICE TABLE # # This table contains the mapping from device type names # to specialized device object types. Types not indexed in this table # should default to type PlayerDevice.
device_table = {'ptz' :PTZDevice, 'camera' :CameraDevice, }
[docs]class PlayerRobot(object): """ Player Robot interface. A PlayerRobot instance encapsulates an interface to a Player robot. It creates and manages a playerc.client object and a set of device proxies wrapped in PlayerDevice objects. In addition, it maintains a run-loop in a separate thread that calls the client's .read() method at regular intervals. The devices are published through standard interfaces on the PlayerRobot instance, and their methods and properties are synchronized with the run thread through a mutex. Example: # set up a robot object with position, laser, and camera objects robot = PlayerRobot(host='myrobot.mydomain.edu',port=6665, devices = [('position2d',0), ('laser',0), ('camera',1)]) # start the run thread, devices will be subscribed # automatically. robot.start() # start the robot turning at 30 deg/sec robot.position2d[0].set_cmd_vel(0, 0, 30*pi/180) # wait for a while time.sleep(5.0) # all stop robot.position2d[0].set_cmd_vel(0,0,0) # shut down the robot's thread, unsubscribing all devices and # disconnecting the client robot.stop() """ def __init__(self,host='localhost',port=6665,speed=20, devices=[]): self._thread = None self._running = False self._lock = RLock() self.speed = speed self._client = PlayerClient(playerc.playerc_client(None,host,port),self._lock) self._queues_running = False self._devices = [] for devname,devnum in devices: self.add_device(devname,devnum=devnum) def start(self): assert self._thread is None self._thread = Thread(target=self.run_loop,name="PlayerRobot Run Loop") self._thread.setDaemon(True) self._thread.start() def stop(self): self._running = False self._thread.join() self._thread = None def run_loop(self): self._client.connect() self._running = True self.subscribe_all() try: while self._running: self._client.read() if self._queues_running: self.process_queues() time.sleep(1.0/self.speed) finally: self.unsubscribe_all() self._client.disconnect()
[docs] def run_queues(self,run_state): """ When using queues for communication with devices, this method toggles queue processing. It is often useful to turn off queue processing, e.g. when a client does not plan on using queued data for a while. """ self._queues_running = run_state
def process_queues(self): for d in self._devices: d.process_queues() def subscribe_all(self): for dev in self._devices: dev.subscribe() def unsubscribe_all(self): for dev in self._devices: dev.unsubscribe() def add_device(self,devname,devnum=0): if devname not in dir(self): setattr(self,devname,{}) proxy_constr = getattr(playerc,'playerc_'+devname) devtype = device_table.get(devname,PlayerDevice) try: self._lock.acquire() dev = devtype(proxy_constr(self._client.proxy,devnum),self._lock) finally: self._lock.release() self._devices.append(dev) getattr(self,devname)[devnum] = dev if self._running: dev.subscribe()