Package baxter_interface :: Module camera
[hide private]
[frames] | no frames]

Source Code for Module baxter_interface.camera

  1  # Copyright (c) 2013-2014, Rethink Robotics 
  2  # All rights reserved. 
  3  # 
  4  # Redistribution and use in source and binary forms, with or without 
  5  # modification, are permitted provided that the following conditions are met: 
  6  # 
  7  # 1. Redistributions of source code must retain the above copyright notice, 
  8  #    this list of conditions and the following disclaimer. 
  9  # 2. Redistributions in binary form must reproduce the above copyright 
 10  #    notice, this list of conditions and the following disclaimer in the 
 11  #    documentation and/or other materials provided with the distribution. 
 12  # 3. Neither the name of the Rethink Robotics nor the names of its 
 13  #    contributors may be used to endorse or promote products derived from 
 14  #    this software without specific prior written permission. 
 15  # 
 16  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 17  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 18  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 19  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 20  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 21  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 22  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 23  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 24  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 25  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 26  # POSSIBILITY OF SUCH DAMAGE. 
 27   
 28  import errno 
 29   
 30  import rospy 
 31   
 32  from baxter_core_msgs.msg import ( 
 33      CameraControl, 
 34      CameraSettings, 
 35  ) 
 36  from baxter_core_msgs.srv import ( 
 37      CloseCamera, 
 38      ListCameras, 
 39      OpenCamera, 
 40  ) 
41 42 43 -class CameraController(object):
44 """ 45 Interface class for controlling camera settings on the Baxter robot. 46 """ 47 48 # Valid resolutions 49 MODES = [ 50 (1280, 800), 51 (960, 600), 52 (640, 400), 53 (480, 300), 54 (384, 240), 55 (320, 200), 56 ] 57 58 # Used to represent when the camera is using automatic controls. 59 # Valid for exposure, gain and white balance. 60 CONTROL_AUTO = -1 61
62 - def __init__(self, name):
63 """ 64 Constructor. 65 66 @param name: camera identifier. You can get a list of valid 67 identifiers by calling the ROS service /cameras/list. 68 69 Expected names are right_hand_camera, left_hand_camera 70 and head_camera. However if the cameras are not 71 identified via the parameter server, they are simply 72 indexed starting at 0. 73 """ 74 self._id = name 75 76 list_svc = rospy.ServiceProxy('/cameras/list', ListCameras) 77 rospy.wait_for_service('/cameras/list', timeout=10) 78 if not self._id in list_svc().cameras: 79 raise AttributeError("Invalid camera name '%s'" % (self._id,)) 80 81 self._open_svc = rospy.ServiceProxy('/cameras/open', OpenCamera) 82 self._close_svc = rospy.ServiceProxy('/cameras/close', CloseCamera) 83 84 self._settings = CameraSettings() 85 self._settings.width = 320 86 self._settings.height = 200 87 self._settings.fps = 20 88 self._open = False
89
90 - def _reload(self):
91 if self._open: 92 self.close() 93 self.open()
94
95 - def _get_value(self, control, default):
96 lookup = [c.value for c in self._settings.controls if c.id == control] 97 try: 98 return lookup[0] 99 except IndexError: 100 return default
101
102 - def _set_control_value(self, control, value):
103 lookup = [c for c in self._settings.controls if c.id == control] 104 try: 105 lookup[0].value = value 106 except IndexError: 107 self._settings.controls.append(CameraControl(control, value))
108 109 @property
110 - def resolution(self):
111 """ 112 Camera resolution as a tuple. (width, height). Valid resolutions are 113 listed as tuples in CameraController.MODES 114 """ 115 return (self._settings.width, self._settings.height)
116 117 @resolution.setter
118 - def resolution(self, res):
119 res = tuple(res) 120 if len(res) != 2: 121 raise AttributeError("Invalid resolution specified") 122 123 if not res in self.MODES: 124 raise ValueError("Invalid camera mode %dx%d" % (res[0], res[1])) 125 126 self._settings.width = res[0] 127 self._settings.height = res[1] 128 self._reload()
129 130 @property
131 - def fps(self):
132 """ 133 Camera frames per second 134 """ 135 return self._settings.fps
136 137 @fps.setter
138 - def fps(self, fps):
139 self._settings.fps = fps 140 self._reload()
141 142 @property
143 - def exposure(self):
144 """ 145 Camera exposure. If autoexposure is on, returns 146 CameraController.CONTROL_AUTO 147 """ 148 return self._get_value(CameraControl.CAMERA_CONTROL_EXPOSURE, 149 self.CONTROL_AUTO)
150 151 @exposure.setter
152 - def exposure(self, exposure):
153 """ 154 Camera Exposure. Valid range is 0-100 or CameraController.CONTROL_AUTO 155 """ 156 exposure = int(exposure) 157 if (exposure < 0 or exposure > 100) and exposure != self.CONTROL_AUTO: 158 raise ValueError("Invalid exposure value") 159 160 self._set_control_value(CameraControl.CAMERA_CONTROL_EXPOSURE, 161 exposure) 162 self._reload()
163 164 @property
165 - def gain(self):
166 """ 167 Camera gain. If autogain is on, returns CameraController.CONTROL_AUTO 168 """ 169 return self._get_value(CameraControl.CAMERA_CONTROL_GAIN, 170 self.CONTROL_AUTO)
171 172 @gain.setter
173 - def gain(self, gain):
174 """ 175 Camera gain. Range is 0-79 or CameraController.CONTROL_AUTO 176 """ 177 gain = int(gain) 178 if (gain < 0 or gain > 79) and gain != self.CONTROL_AUTO: 179 raise ValueError("Invalid gain value") 180 181 self._set_control_value(CameraControl.CAMERA_CONTROL_GAIN, gain) 182 self._reload()
183 184 @property
185 - def white_balance_red(self):
186 """ 187 White balance red. If autocontrol is on, returns 188 CameraController.CONTROL_AUTO 189 """ 190 return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_R, 191 self.CONTROL_AUTO)
192 193 @white_balance_red.setter
194 - def white_balance_red(self, value):
195 """ 196 White balance red. Range is 0-4095 or CameraController.CONTROL_AUTO 197 """ 198 value = int(value) 199 if (value < 0 or value > 4095) and value != self.CONTROL_AUTO: 200 raise ValueError("Invalid white balance value") 201 202 self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_R, 203 value) 204 self._reload()
205 206 @property
207 - def white_balance_green(self):
208 """ 209 White balance green. If autocontrol is on, returns 210 CameraController.CONTROL_AUTO 211 """ 212 return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_G, 213 self.CONTROL_AUTO)
214 215 @white_balance_green.setter
216 - def white_balance_green(self, value):
217 """ 218 White balance green. Range is 0-4095 or CameraController.CONTROL_AUTO 219 """ 220 value = int(value) 221 if (value < 0 or value > 4095) and value != self.CONTROL_AUTO: 222 raise ValueError("Invalid white balance value") 223 224 self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_G, 225 value) 226 self._reload()
227 228 @property
229 - def white_balance_blue(self):
230 """ 231 White balance blue. If autocontrol is on, returns 232 CameraController.CONTROL_AUTO 233 """ 234 return self._get_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_B, 235 self.CONTROL_AUTO)
236 237 @white_balance_blue.setter
238 - def white_balance_blue(self, value):
239 """ 240 White balance blue. Range is 0-4095 or CameraController.CONTROL_AUTO 241 """ 242 value = int(value) 243 if (value < 0 or value > 4095) and value != self.CONTROL_AUTO: 244 raise ValueError("Invalid white balance value") 245 246 self._set_control_value(CameraControl.CAMERA_CONTROL_WHITE_BALANCE_B, 247 value) 248 self._reload()
249 250 @property
251 - def window(self):
252 """ 253 Camera windowing, returns a tuple, (x, y) 254 """ 255 x = self._get_value(CameraControl.CAMERA_CONTROL_WINDOW_X, 256 self.CONTROL_AUTO) 257 if (x == self.CONTROL_AUTO): 258 return (tuple(map(lambda x: x / 2, self.resolution)) if 259 self.half_resolution else 260 self.resolution) 261 else: 262 return (x, self._get_value(CameraControl.CAMERA_CONTROL_WINDOW_Y, 263 self.CONTROL_AUTO))
264 265 @window.setter
266 - def window(self, win):
267 """ 268 Set camera window. The max size is a function of the current camera 269 resolution and if half_resolution is enabled or not. 270 """ 271 x, y = tuple(win) 272 cur_x, cur_y = self.resolution 273 limit_x = 1280 - cur_x 274 limit_y = 800 - cur_y 275 276 if self.half_resolution: 277 limit_x /= 2 278 limit_y /= 2 279 280 if x < 0 or x > limit_x: 281 raise ValueError("Max X window is %d" % (limit_x,)) 282 283 if y < 0 or y > limit_y: 284 raise ValueError("Max Y window is %d" % (limit_y,)) 285 286 self._set_control_value(CameraControl.CAMERA_CONTROL_WINDOW_X, x) 287 self._set_control_value(CameraControl.CAMERA_CONTROL_WINDOW_Y, y) 288 self._reload()
289 290 @property
291 - def flip(self):
292 """ 293 Camera flip. Returns True if flip is enabled on the camera. 294 """ 295 return self._get_value(CameraControl.CAMERA_CONTROL_FLIP, False)
296 297 @flip.setter
298 - def flip(self, value):
299 self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP, 300 int(value != 0)) 301 self._reload()
302 303 @property
304 - def mirror(self):
305 """ 306 Camera mirror. Returns True if mirror is enabled on the camera. 307 """ 308 return self._get_value(CameraControl.CAMERA_CONTROL_MIRROR, False)
309 310 @mirror.setter
311 - def mirror(self, value):
312 self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR, 313 int(value != 0)) 314 self._reload()
315 316 @property
317 - def half_resolution(self):
318 """ 319 Return True if binning/half resolution is enabled on the camera. 320 """ 321 return self._get_value(CameraControl.CAMERA_CONTROL_RESOLUTION_HALF, 322 False)
323 324 @half_resolution.setter
325 - def half_resolution(self, value):
326 self._set_control_value(CameraControl.CAMERA_CONTROL_RESOLUTION_HALF, 327 int(value != 0)) 328 self._reload()
329
330 - def open(self):
331 """ 332 Open the camera currently under control. 333 """ 334 if self._id == 'head_camera': 335 self._set_control_value(CameraControl.CAMERA_CONTROL_FLIP, True) 336 self._set_control_value(CameraControl.CAMERA_CONTROL_MIRROR, True) 337 ret = self._open_svc(self._id, self._settings) 338 if ret.err != 0: 339 raise OSError(ret.err, "Failed to open camera") 340 self._open = True
341
342 - def close(self):
343 """ 344 Close, if necessary the camera. 345 """ 346 ret = self._close_svc(self._id) 347 if ret.err != 0 and ret.err != errno.EINVAL: 348 raise OSError(ret.err, "Failed to close camera") 349 self._open = False
350