naoqi_camera.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 #
4 # ROS node to provide access to the camera by wrapping NaoQI access (may not
5 # be the most efficient way...)
6 #
7 # Copyright 2012 Daniel Maier, University of Freiburg
8 # Copyright 2014 Aldebaran Robotics
9 # http://www.ros.org/wiki/nao
10 #
11 # Redistribution and use in source and binary forms, with or without
12 # modification, are permitted provided that the following conditions are met:
13 #
14 # # Redistributions of source code must retain the above copyright
15 # notice, this list of conditions and the following disclaimer.
16 # # Redistributions in binary form must reproduce the above copyright
17 # notice, this list of conditions and the following disclaimer in the
18 # documentation and/or other materials provided with the distribution.
19 # # Neither the name of the University of Freiburg nor the names of its
20 # contributors may be used to endorse or promote products derived from
21 # this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
26 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
27 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
28 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
29 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
30 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
31 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
32 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 # POSSIBILITY OF SUCH DAMAGE.
34 #
35 
36 from collections import defaultdict
37 from distutils.version import LooseVersion
38 import rospy
39 from sensor_msgs.msg import Image, CameraInfo
40 from naoqi_driver.naoqi_node import NaoqiNode
41 import camera_info_manager
42 
43 from dynamic_reconfigure.server import Server
44 from naoqi_sensors_py.cfg import NaoqiCameraConfig
45 
46 # import resolutions
47 from naoqi_sensors.vision_definitions import k960p, k4VGA, kVGA, kQVGA, kQQVGA
48 # import color spaces
49 from naoqi_sensors.vision_definitions import kYUV422ColorSpace, kYUVColorSpace, \
50  kRGBColorSpace, kBGRColorSpace, kDepthColorSpace, kRawDepthColorSpace
51 # import extra parameters
52 from naoqi_sensors.vision_definitions import kCameraSelectID, kCameraAutoExpositionID, kCameraAecAlgorithmID, \
53  kCameraContrastID, kCameraSaturationID, kCameraHueID, kCameraSharpnessID, kCameraAutoWhiteBalanceID, \
54  kCameraExposureID, kCameraAutoGainID, kCameraGainID, kCameraBrightnessID, kCameraWhiteBalanceID
55 
56 # those should appear in vision_definitions.py at some point
57 kTopCamera = 0
58 kBottomCamera = 1
59 kDepthCamera = 2
60 
62  def __init__(self, node_name='naoqi_camera'):
63  NaoqiNode.__init__(self, node_name)
64 
65  self.camProxy = self.get_proxy("ALVideoDevice")
66  if self.camProxy is None:
67  exit(1)
68  self.nameId = None
69  self.camera_infos = {}
70  def returnNone():
71  return None
72  self.config = defaultdict(returnNone)
73 
74  # ROS publishers
75  self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=5)
76  self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=5)
77 
78  # initialize the parameter server
79  self.srv = Server(NaoqiCameraConfig, self.reconfigure)
80 
81  # initial load from param server
82  self.init_config()
83 
84  # initially load configurations
85  self.reconfigure(self.config, 0)
86 
87  def init_config( self ):
88  # mandatory configurations to be set
89  self.config['frame_rate'] = rospy.get_param('~frame_rate')
90  self.config['source'] = rospy.get_param('~source')
91  self.config['resolution'] = rospy.get_param('~resolution')
92  self.config['color_space'] = rospy.get_param('~color_space')
93 
94  # optional for camera frames
95  # top camera with default
96  if rospy.has_param('~camera_top_frame'):
97  self.config['camera_top_frame'] = rospy.get_param('~camera_top_frame')
98  else:
99  self.config['camera_top_frame'] = "/CameraTop_optical_frame"
100  # bottom camera with default
101  if rospy.has_param('~camera_bottom_frame'):
102  self.config['camera_bottom_frame'] = rospy.get_param('~camera_bottom_frame')
103  else:
104  self.config['camera_bottom_frame'] = "/CameraBottom_optical_frame"
105  # depth camera with default
106  if rospy.has_param('~camera_depth_frame'):
107  self.config['camera_depth_frame'] = rospy.get_param('~camera_depth_frame')
108  else:
109  self.config['camera_depth_frame'] = "/CameraDepth_optical_frame"
110 
111  #load calibration files
112  if rospy.has_param('~calibration_file_top'):
113  self.config['calibration_file_top'] = rospy.get_param('~calibration_file_top')
114  else:
115  rospy.loginfo('no camera calibration for top camera found')
116 
117  if rospy.has_param('~calibration_file_bottom'):
118  self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
119  else:
120  rospy.loginfo('no camera calibration for bottom camera found')
121 
122  # set time reference
123  if rospy.has_param('~use_ros_time'):
124  self.config['use_ros_time'] = rospy.get_param('~use_ros_time')
125  else:
126  self.config['use_ros_time'] = False
127 
128 
129  def load_camera_info( self ):
130  if self.config['source'] == 0:
131  self.config['camera_info_url'] = self.config['calibration_file_top']
132  elif self.config['source'] == 1:
133  self.config['camera_info_url'] = self.config['calibration_file_bottom']
134  else:
135  rospy.loginfo('no valid camera calibration file found')
136 
137  def reconfigure( self, new_config, level ):
138  """
139  Reconfigure the camera
140  """
141  rospy.loginfo('reconfigure changed')
142  if self.pub_img_.get_num_connections() == 0:
143  rospy.loginfo('Changes recorded but not applied as nobody is subscribed to the ROS topics.')
144  self.config.update(new_config)
145  return self.config
146 
147  # check if we are even subscribed to a camera
148  is_camera_new = self.nameId is None
149 
150  if is_camera_new:
151  rospy.loginfo('subscribed to camera proxy, since this is the first camera')
152  self.nameId = self.camProxy.subscribeCamera("rospy_gvm", new_config['source'],
153  new_config['resolution'], new_config['color_space'],
154  new_config['frame_rate'])
155 
156  if self.config['source'] != new_config['source'] or is_camera_new:
157  rospy.loginfo('updating camera source information')
158 
159  if new_config['source'] == kTopCamera:
160  self.frame_id = self.config['camera_top_frame']
161  elif new_config['source'] == kBottomCamera:
162  self.frame_id = self.config['camera_bottom_frame']
163  elif new_config['source'] == kDepthCamera:
164  self.frame_id = self.config['camera_depth_frame']
165  else:
166  rospy.logerr('Invalid source. Must be 0, 1 or 2')
167  exit(1)
168 
169  # check if the camera changed
170  if self.config['camera_info_url'] == "" or \
171  ( self.config['camera_info_url'] != new_config['camera_info_url'] and \
172  new_config['camera_info_url'] not in self.camera_infos ):
173 
174  self.load_camera_info()
175 
176  if 'cim' not in self.__dict__:
177  self.cim = camera_info_manager.CameraInfoManager(cname='nao_camera')
178 
179  if not self.cim.setURL( new_config['camera_info_url'] ):
180  rospy.logerr('malformed URL for calibration file')
181  else:
182  try:
183  self.cim.loadCameraInfo()
184  except IOExcept:
185  rospy.logerr('Could not read from existing calibration file')
186 
187  if self.cim.isCalibrated():
188  rospy.loginfo('Successfully loaded camera info')
189  self.camera_infos[new_config['camera_info_url']] = self.cim.getCameraInfo()
190  else:
191  rospy.logerr('There was a problem loading the calibration file. Check the URL!')
192 
193  # set params
194  camParams = self.extractParams(new_config)
195  self.setParams(camParams)
196 
197  key_methods = [ ('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')]
198  if self.get_version() >= LooseVersion('2.0'):
199  key_methods.append(('source', 'setActiveCamera'))
200  for key, method in key_methods:
201  if self.config[key] != new_config[key] or is_camera_new:
202  self.camProxy.__getattribute__(method)(self.nameId, new_config[key])
203 
204  self.config.update(new_config)
205 
206  return self.config
207 
208  def extractParams(self, new_config):
209  camParams = []
210 
211  camParams.append( (kCameraAecAlgorithmID, new_config['auto_exposure_algo']) )
212  camParams.append( (kCameraContrastID, new_config['contrast']) )
213  camParams.append( (kCameraSaturationID, new_config['saturation']) )
214  camParams.append( (kCameraHueID, new_config['hue']) ) # Migth be deprecated
215  camParams.append( (kCameraSharpnessID, new_config['sharpness']) )
216 
217  camParams.append( (kCameraAutoWhiteBalanceID, new_config['auto_white_balance']) )
218  if ( new_config['auto_white_balance']==0):
219  camParams.append( (kCameraWhiteBalanceID, new_config['white_balance']) )
220 
221  camParams.append( (kCameraAutoExpositionID, new_config['auto_exposition']) )
222  if ( new_config['auto_exposition']==0):
223  camParams.append( (kCameraGainID, new_config['gain']) )
224  camParams.append( (kCameraExposureID, new_config['exposure']) )
225  else:
226  camParams.append( (kCameraBrightnessID, new_config['brightness']) )
227 
228  return camParams
229 
230  def setParams(self, key_list):
231  for key, value in key_list:
232  if self.get_version() < LooseVersion('2.0'):
233  self.camProxy.setParam(key, value)
234  else:
235  self.camProxy.setCameraParameter(self.nameId, key, value)
236 
237  def run(self):
238  img = Image()
239  r = rospy.Rate(self.config['frame_rate'])
240  while self.is_looping():
241  if self.pub_img_.get_num_connections() == 0:
242  if self.nameId:
243  rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
244  self.camProxy.unsubscribe(self.nameId)
245  self.nameId = None
246  r.sleep()
247  continue
248  if self.nameId is None:
249  self.reconfigure(self.config, 0)
250  r.sleep()
251  continue
252  image = self.camProxy.getImageRemote(self.nameId)
253  if image is None:
254  continue
255  # Deal with the image
256  if self.config['use_ros_time']:
257  img.header.stamp = rospy.Time.now()
258  else:
259  img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
260  img.header.frame_id = self.frame_id
261  img.height = image[1]
262  img.width = image[0]
263  nbLayers = image[2]
264  if image[3] == kYUVColorSpace:
265  encoding = "mono8"
266  elif image[3] == kRGBColorSpace:
267  encoding = "rgb8"
268  elif image[3] == kBGRColorSpace:
269  encoding = "bgr8"
270  elif image[3] == kYUV422ColorSpace:
271  encoding = "yuv422" # this works only in ROS groovy and later
272  elif image[3] == kDepthColorSpace or image[3] == kRawDepthColorSpace:
273  encoding = "16UC1"
274  else:
275  rospy.logerr("Received unknown encoding: {0}".format(image[3]))
276  img.encoding = encoding
277  img.step = img.width * nbLayers
278  img.data = image[6]
279 
280  self.pub_img_.publish(img)
281 
282  # deal with the camera info
283  if self.config['source'] == kDepthCamera and (image[3] == kDepthColorSpace or image[3] == kRawDepthColorSpace):
284  infomsg = CameraInfo()
285  # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
286  ratio_x = float(640)/float(img.width)
287  ratio_y = float(480)/float(img.height)
288  infomsg.width = img.width
289  infomsg.height = img.height
290  # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
291  infomsg.K = [ 525, 0, 3.1950000000000000e+02,
292  0, 525, 2.3950000000000000e+02,
293  0, 0, 1 ]
294  infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
295  0, 525, 2.3950000000000000e+02, 0,
296  0, 0, 1, 0 ]
297  for i in range(3):
298  infomsg.K[i] = infomsg.K[i] / ratio_x
299  infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
300  infomsg.P[i] = infomsg.P[i] / ratio_x
301  infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
302 
303  infomsg.D = []
304  infomsg.binning_x = 0
305  infomsg.binning_y = 0
306  infomsg.distortion_model = ""
307 
308  infomsg.header = img.header
309  self.pub_info_.publish(infomsg)
310  elif self.config['camera_info_url'] in self.camera_infos:
311  infomsg = self.camera_infos[self.config['camera_info_url']]
312 
313  infomsg.header = img.header
314  self.pub_info_.publish(infomsg)
315 
316  r.sleep()
317 
318  if (self.nameId):
319  rospy.loginfo("unsubscribing from camera ")
320  self.camProxy.unsubscribe(self.nameId)
def reconfigure(self, new_config, level)
def __init__(self, node_name='naoqi_camera')
Definition: naoqi_camera.py:62
def extractParams(self, new_config)
def get_proxy(self, name, warn=True)


naoqi_sensors_py
Author(s): Séverin Lemaignan, Vincent Rabaud, Karsten Knese, Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy, Daniel Maier
autogenerated on Thu Jul 16 2020 03:18:33