Source Code for Module baxter_interface.camera

  1  # Copyright (c) 2013-2015, 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( 
 80                  ("Cannot locate a service for camera name '{0}'. " 
 81                  "Close a different camera first and try again.".format(self._id))) 
 82   
 83          self._open_svc = rospy.ServiceProxy('/cameras/open', OpenCamera) 
 84          self._close_svc = rospy.ServiceProxy('/cameras/close', CloseCamera) 
 85   
 86          self._settings = CameraSettings() 
 87          self._settings.width = 320 
 88          self._settings.height = 200 
 89          self._settings.fps = 20 
 90          self._open = False 
 91   
 92 -    def _reload(self): 
 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