topo.misc.playerrobot

1 """ 2 High-level interface to the Player client libraries. 3 4 The Player client libraries allow Python code to communicate with 5 hardware devices such as robots, cameras, and range sensors. 6 7 This is a temporary home for this file until it finds a permanent home 8 (maybe in the PlayerStage project or in PLASTK?) 9 10 $Id: playerrobot.py 8356 2008-04-11 19:51:05Z jprovost $ 11 """ 12 __version__='$Revision: 8356 $' 13 14 15 import time,array 16 import playerc 17 18 from threading import RLock, Thread 19 from Queue import Queue 20 21 from operator import eq,ne 22 from copy import copy 23 from math import pi

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

38 """ 39 Configure the module to use the processing library for asynchronous 40 process support. Use of the processing library requires the use of 41 queues for communication with robot devices. 42 """ 43 import processing 44 global RLock, Thread, Queue 45 RLock = processing.RLock 46 Thread = processing.Process 47 Queue = processing.Queue

48

49 -def use_threading():

50 """ 51 Configure the module to use the threading library for asynchronous 52 process support. (the default) 53 """ 54 import threading, Queue 55 global RLock, Thread, Queue 56 RLock = threading.RLock 57 Thread = threading.Thread 58 Queue = Queue.Queue

59

60 # JPALERT This is a HACK for the CVS version of Player, this value 61 # should be defined in the playerc module: 62 playerc.PLAYERC_OPEN_MODE = 1 63 64 65 -class PlayerException(Exception):

66 pass

67

68 69 -def player_fn(error_op=ne,error_val=0):

70 """ 71 Player function decorator. Adds error checking. 72 73 Takes an operator and a value, and compares the result 74 of the function call with the value using the operator. 75 If the result is true, a PlayerException is raised. The 76 default error condition is error_op = ne, error_value = 0, 77 which raises an exception if fn(*args) != 0. 78 """ 79 def wrap(fn): 80 def new_fn(*args): 81 if error_op(fn(*args),error_val): 82 raise PlayerException(playerc.playerc_error_str())

83 return new_fn 84 return wrap 85

86 87 -def synchronized(lock):

88 """ 89 Simple synchronization decorator. 90 91 Takes an existing lock and synchronizes a function or 92 method on that lock. Code taken from the Python Wiki 93 PythonDecoratorLibrary: 94 95 http://wiki.python.org/moin/PythonDecoratorLibrary 96 """ 97 def wrap(f): 98 def newFunction(*args, **kw): 99 lock.acquire() 100 try: 101 return f(*args, **kw) 102 finally: 103 lock.release()

104 return newFunction 105 return wrap 106

107 108 -def synched_method(f):

109 """ 110 Synchronized method decorator. 111 112 Like synchronized() decorator, except synched_method assumes 113 that the first argument of the function is an instance containing 114 a Lock object, and this lock is used for synchronization. 115 """ 116 def newFunction(self,*args,**kw): 117 self._lock.acquire() 118 try: 119 return f(self,*args, **kw) 120 finally: 121 self._lock.release()

122 return newFunction 123

124 125 126 -class PlayerObject(object):

127 """ 128 A generic threadsafe wrapper for client and proxy objects 129 from the playerc library. 130 131 PlayerObject wrappers are constructed automatically by PlayerRobot 132 objects. Each PlayerObject instance wraps a playerc device proxy 133 or client object, and publishes a thread-safe version of each of 134 proxy's methods, that is synchronized with the PlayerRobot 135 instance's run-loop thread, and that catches playerc error 136 conditions and raises them as PlayerExceptions. The original 137 playerc proxy object is available via the attribute .proxy. 138 Specialized subclasses of PlayerObject can have additional 139 interfaces for getting device state or setting commands specific 140 to that device. 141 142 Developer note: the PlayerObject base class __init__ method 143 automatically wraps each method on the proxy that (a) doesn't 144 begin with '__' and (b) is not already in dir(self). This way, 145 subclasses can override the wrapping process by defining their own 146 wrappers *before* the base class __init__ method is called. 147 """

148 - def __init__(self,proxy,lock):

149 150 self._lock = lock 151 self.proxy = proxy 152 for name in dir(proxy): 153 attr = getattr(proxy,name) 154 if name not in dir(self) and name[:2] != '__' and callable(attr): 155 setattr(self,name,synchronized(lock)(player_fn()(attr))) 156 157 self.cmd_queue = Queue()

158

159 - def process_queues(self):

160 while not self.cmd_queue.empty(): 161 name,args = self.cmd_queue.get() 162 try: 163 print "Doing command:",name,args 164 getattr(self,name)(*args) 165 finally: 166 self.cmd_queue.task_done()

167

168 169 -class PlayerClient(PlayerObject):

170 """ 171 Player object wrapper for playerc.client objects. 172 """

173 - def __init__(self,proxy,lock):

174 175 # Override the wrapper on playerc.client.read, because 176 # it returns None for errors, instead of returning 0 for 177 # "no error." 178 self.read = synchronized(lock)(player_fn(eq,None)(proxy.read)) 179 super(PlayerClient,self).__init__(proxy,lock)

180

181 - def process_queues(self):

182 pass

183

184 185 -class PlayerDevice(PlayerObject):

186 """ 187 Generic Player device object. 188 189 Overrides the default proxy .subscribe method so that the mode defaults 190 to PLAYERC_OPEN_MODE. 191 """ 192 193 @synched_method 194 @player_fn()

197

198 199 200 -class PTZDevice(PlayerDevice):

201 """ 202 Player Pan/Tilt/Zoom (PTZ) device. 203 204 Adds the following to the original proxy interface: 205 206 state = The tuple (pan,tilt,zoom) indicating the current state of 207 the PTZ device. 208 209 state_deg = Same as state, but returns values in degrees instead 210 of radians 211 212 set_deg() and set_ws_deg() methods. Same as .set() and .set_ws(), 213 using degrees instead of radians. 214 """

215 - def get_state(self):

216 return self.proxy.pan, self.proxy.tilt, self.proxy.zoom

217 state = property(get_state) 218

219 - def get_state_deg(self):

220 return self.proxy.pan*180/pi, \ 221 self.proxy.tilt*180/pi, \ 222 self.proxy.zoom*180/pi

223 state_deg = property(get_state_deg) 224

225 - def set_deg(self,pan,tilt,zoom):

226 self.set(pan*pi/180, tilt*pi/180, zoom*pi/180)

227

228 - def set_ws_deg(self,pan,tilt,zoom,pan_speed,tilt_speed):

229 self.set_ws(pan*pi/180, tilt*pi/180, zoom*pi/180,pan_speed*pi/180,tilt_speed*pi/180)

230

231 232 233 -class CameraDevice(PlayerDevice):

234 """ 235 A Player camera device. 236 237 The synchronized method get_image grabs an uncompressed snapshot, 238 along with the additional formatting information needed to make an 239 image. 240 """ 241

242 - def __init__(self,proxy,lock):

243 self.decompress = synchronized(lock)(player_fn(ne,None)(proxy.decompress)) 244 super(CameraDevice,self).__init__(proxy,lock) 245 self.image_queue = Queue()

246

247 - def process_queues(self):

248 im = self.image 249 # check to make sure it's really an image 250 if im[1] > 0: 251 self.image_queue.put(im) 252 super(CameraDevice,self).process_queues()

253 254 255 # @synched_method

256 - def get_image(self):

257 """ 258 Returns the tuple: 259 (format,width,height,bpp,fdiv,data) 260 Where data is a copy of the uncompressed image data. 261 """ 262 if self.proxy.compression: 263 self.decompress() 264 im_array = array.array('B') 265 im_array.fromstring(self.proxy.image[:self.proxy.image_count]) 266 return self.proxy.format, \ 267 self.proxy.width, \ 268 self.proxy.height, \ 269 self.proxy.bpp, \ 270 self.proxy.fdiv, \ 271 im_array

272 273 image = property(get_image)

274 275 276 277 ################## 278 # DEVICE TABLE 279 # 280 # This table contains the mapping from device type names 281 # to specialized device object types. Types not indexed in this table 282 # should default to type PlayerDevice. 283 284 device_table = {'ptz' :PTZDevice, 285 'camera' :CameraDevice, 286 }

287 288 289 290 -class PlayerRobot(object):

291 """ 292 Player Robot interface. 293 294 A PlayerRobot instance encapsulates an interface to a Player 295 robot. It creates and manages a playerc.client object and a set of 296 device proxies wrapped in PlayerDevice objects. In addition, it 297 maintains a run-loop in a separate thread that calls the client's 298 .read() method at regular intervals. The devices are published 299 through standard interfaces on the PlayerRobot instance, and their 300 methods and properties are synchronized with the run thread 301 through a mutex. 302 303 Example: 304 305 # set up a robot object with position, laser, and camera objects 306 robot = PlayerRobot(host='myrobot.mydomain.edu',port=6665, 307 devices = [('position2d',0), 308 ('laser',0), 309 ('camera',1)]) 310 311 # start the run thread, devices will be subscribed 312 # automatically. 313 robot.start() 314 315 # start the robot turning at 30 deg/sec 316 robot.position2d[0].set_cmd_vel(0, 0, 30*pi/180) 317 318 # wait for a while 319 time.sleep(5.0) 320 321 # all stop 322 robot.position2d[0].set_cmd_vel(0,0,0) 323 324 # shut down the robot's thread, unsubscribing all devices and 325 # disconnecting the client 326 robot.stop() 327 """ 328

329 - def __init__(self,host='localhost',port=6665,speed=20, 330 devices=[]):

331 332 self._thread = None 333 self._running = False 334 self._lock = RLock() 335 self.speed = speed 336 337 self._client = PlayerClient(playerc.playerc_client(None,host,port),self._lock) 338 339 self._queues_running = False 340 self._devices = [] 341 for devname,devnum in devices: 342 self.add_device(devname,devnum=devnum)

343

344 - def start(self):

345 assert self._thread is None 346 self._thread = Thread(target=self.run_loop,name="PlayerRobot Run Loop") 347 self._thread.setDaemon(True) 348 self._thread.

Source Code for Module topo.misc.playerrobot