camera.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to provide access to the camera by wrapping NaoQI access (may not
00005 # be the most efficient way...)
00006 #
00007 # Copyright 2012 Daniel Maier, University of Freiburg
00008 # Copyright 2014 Aldebaran Robotics
00009 # http://www.ros.org/wiki/nao
00010 #
00011 # Redistribution and use in source and binary forms, with or without
00012 # modification, are permitted provided that the following conditions are met:
00013 #
00014 #    # Redistributions of source code must retain the above copyright
00015 #       notice, this list of conditions and the following disclaimer.
00016 #    # Redistributions in binary form must reproduce the above copyright
00017 #       notice, this list of conditions and the following disclaimer in the
00018 #       documentation and/or other materials provided with the distribution.
00019 #    # Neither the name of the University of Freiburg nor the names of its
00020 #       contributors may be used to endorse or promote products derived from
00021 #       this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00027 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 from collections import defaultdict
00037 from distutils.version import LooseVersion
00038 import rospy
00039 from sensor_msgs.msg import Image, CameraInfo
00040 from nao_driver.nao_driver_naoqi import NaoNode
00041 import camera_info_manager
00042 
00043 from dynamic_reconfigure.server import Server
00044 from nao_sensors.cfg import NaoCameraConfig
00045 
00046 # import resolutions
00047 from nao_sensors.vision_definitions import k960p, k4VGA, kVGA, kQVGA, kQQVGA
00048 # import color spaces
00049 from nao_sensors.vision_definitions import kYUV422ColorSpace, kYUVColorSpace, \
00050                     kRGBColorSpace, kBGRColorSpace, kDepthColorSpace
00051 # import extra parameters
00052 from nao_sensors.vision_definitions import kCameraSelectID, kCameraAutoExpositionID, kCameraAecAlgorithmID, \
00053                   kCameraContrastID, kCameraSaturationID, kCameraHueID, kCameraSharpnessID, kCameraAutoWhiteBalanceID, \
00054                   kCameraExposureID, kCameraGainID, kCameraBrightnessID, kCameraWhiteBalanceID
00055 
00056 # those should appear in vision_definitions.py at some point
00057 kTopCamera = 0
00058 kBottomCamera = 1
00059 kDepthCamera = 2
00060 
00061 class NaoCam (NaoNode):
00062     def __init__(self):
00063         NaoNode.__init__(self, 'nao_camera')
00064 
00065         self.camProxy = self.get_proxy("ALVideoDevice")
00066         if self.camProxy is None:
00067             exit(1)
00068         self.nameId = None
00069         self.camera_infos = {}
00070         def returnNone():
00071             return None
00072         self.config = defaultdict(returnNone)
00073 
00074         # ROS publishers
00075         self.pub_img_ = rospy.Publisher('~image_raw', Image, queue_size=5)
00076         self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo, queue_size=5)
00077 
00078         # initialize the parameter server
00079         self.srv = Server(NaoCameraConfig, self.reconfigure)
00080 
00081         # initial load from param server
00082         self.init_config()
00083 
00084         # initially load configurations
00085         self.reconfigure(self.config, 0)
00086 
00087     def init_config( self ):
00088         # mandatory configurations to be set
00089         self.config['frame_rate'] = rospy.get_param('~frame_rate')
00090         self.config['source'] = rospy.get_param('~source')
00091         self.config['resolution'] = rospy.get_param('~resolution')
00092         self.config['color_space'] = rospy.get_param('~color_space')
00093 
00094         # optional for camera frames
00095         # top camera with default
00096         if rospy.has_param('~camera_top_frame'):
00097             self.config['camera_top_frame'] = rospy.get_param('~camera_top_frame')
00098         else:
00099             self.config['camera_top_frame'] = "/CameraTop_frame"
00100         # bottom camera with default
00101         if rospy.has_param('~camera_bottom_frame'):
00102             self.config['camera_bottom_frame'] = rospy.get_param('~camera_bottom_frame')
00103         else:
00104             self.config['camera_bottom_frame'] = "/CameraBottom_frame"
00105         # depth camera with default
00106         if rospy.has_param('~camera_depth_frame'):
00107             self.config['camera_depth_frame'] = rospy.get_param('~camera_depth_frame')
00108         else:
00109             self.config['camera_depth_frame'] = "/DepthCamera_frame"
00110 
00111         #load calibration files
00112         if rospy.has_param('~calibration_file_top'):
00113             self.config['calibration_file_top'] = rospy.get_param('~calibration_file_top')
00114         if rospy.has_param('~calibration_file_bottom'):
00115             self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
00116 
00117         if rospy.has_param('~use_ros_time'):
00118             self.config['use_ros_time'] = rospy.get_param('~use_ros_time')
00119         else:
00120             self.config['use_ros_time'] = False
00121 
00122 
00123     def reconfigure( self, new_config, level ):
00124         """
00125         Reconfigure the camera
00126         """
00127         rospy.loginfo('reconfigure changed')
00128         if self.pub_img_.get_num_connections() == 0:
00129             rospy.loginfo('Changes recorded but not applied as nobody is subscribed to the ROS topics.')
00130             self.config.update(new_config)
00131             return self.config
00132 
00133         # check if we are even subscribed to a camera
00134         is_camera_new = self.nameId is None
00135 
00136         if is_camera_new:
00137             rospy.loginfo('subscribed to camera proxy, since this is the first camera')
00138             if self.get_version() < LooseVersion('2.0'):
00139                 self.nameId = self.camProxy.subscribe("rospy_gvm", new_config['source'],
00140                                                       new_config['resolution'], new_config['color_space'],
00141                                                       new_config['frame_rate'])
00142             else:
00143                 self.nameId = self.camProxy.subscribeCamera("rospy_gvm", new_config['source'],
00144                                                             new_config['resolution'], new_config['color_space'],
00145                                                             new_config['frame_rate'])
00146 
00147         if self.config['source'] != new_config['source'] or is_camera_new:
00148             rospy.loginfo('updating camera source information')
00149 
00150             if new_config['source'] == kTopCamera:
00151                 self.frame_id = self.config['camera_top_frame']
00152             elif new_config['source'] == kBottomCamera:
00153                 self.frame_id = self.config['camera_bottom_frame']
00154             elif new_config['source'] == kDepthCamera:
00155                 self.frame_id = new_config['camera_depth_frame']
00156             else:
00157                 rospy.logerr('Invalid source. Must be 0, 1 or 2')
00158                 exit(1)
00159 
00160         # check if the camera changed
00161         if self.config['camera_info_url'] != new_config['camera_info_url'] and \
00162                         new_config['camera_info_url'] and new_config['camera_info_url'] not in self.camera_infos:
00163             if 'cim' not in self.__dict__:
00164                 self.cim = camera_info_manager.CameraInfoManager(cname='nao_camera')
00165 
00166             if not self.cim.setURL( new_config['camera_info_url'] ):
00167                 rospy.logerr('malformed URL for calibration file')
00168             else:
00169                 try:
00170                     self.cim.loadCameraInfo()
00171                 except IOExcept:
00172                     rospy.logerr('Could not read from existing calibration file')
00173 
00174             if self.cim.isCalibrated():
00175                 rospy.loginfo('Successfully loaded camera info')
00176                 self.camera_infos[new_config['camera_info_url']] = self.cim.getCameraInfo()
00177             else:
00178                 rospy.logerr('There was a problem loading the calibration file. Check the URL!')
00179 
00180         # set params
00181         key_naoqi_keys = [('auto_exposition', kCameraAutoExpositionID),
00182                           ('auto_exposure_algo', kCameraAecAlgorithmID),
00183                           ('contrast', kCameraContrastID), ('saturation', kCameraSaturationID),
00184                           ('hue', kCameraHueID), ('sharpness', kCameraSharpnessID),
00185                           ('auto_white_balance', kCameraAutoWhiteBalanceID)]
00186         if self.get_version() < LooseVersion('2.0'):
00187             key_method.append(('source', 'setActiveCamera'))
00188 
00189         for key, naoqi_key in key_naoqi_keys:
00190             if self.config[key] != new_config[key] or is_camera_new:
00191                 if self.get_version() < LooseVersion('2.0'):
00192                     self.camProxy.setParam(naoqi_key, new_config[key])
00193                 else:
00194                     self.camProxy.setCameraParameter(self.nameId, naoqi_key, new_config[key])
00195 
00196         for key, naoqi_key, auto_exp_val in [('exposure', kCameraExposureID, 0),
00197                                              ('gain', kCameraGainID, 0), ('brightness', kCameraBrightnessID, 1)]:
00198             if self.config[key] != new_config[key] or is_camera_new:
00199                 if self.get_version() < LooseVersion('2.0'):
00200                     self.camProxy.setParam(kCameraAutoExpositionID, auto_exp_val)
00201                     self.camProxy.setParam(naoqi_key, new_config[key])
00202                 else:
00203                     self.camProxy.setCameraParameter(self.nameId, kCameraAutoExpositionID, auto_exp_val)
00204                     self.camProxy.setCameraParameter(self.nameId, naoqi_key, new_config[key])
00205 
00206         if self.config['white_balance'] != new_config['white_balance'] or is_camera_new:
00207             if self.get_version() < LooseVersion('2.0'):
00208                 self.camProxy.setParam(kCameraAutoWhiteBalanceID, 0)
00209                 self.camProxy.setParam(kCameraWhiteBalanceID, new_config['white_balance'])
00210             else:
00211                 self.camProxy.setCameraParameter(self.nameId, kCameraAutoWhiteBalanceID, 0)
00212                 self.camProxy.setCameraParameter(self.nameId, kCameraWhiteBalanceID, new_config['white_balance'])
00213 
00214         key_methods =  [ ('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')]
00215         if self.get_version() >= LooseVersion('2.0'):
00216             key_methods.append(('source', 'setActiveCamera'))
00217         for key, method in key_methods:
00218             if self.config[key] != new_config[key] or is_camera_new:
00219                 self.camProxy.__getattribute__(method)(self.nameId, new_config[key])
00220 
00221         self.config.update(new_config)
00222 
00223         return self.config
00224 
00225     def run(self):
00226         img = Image()
00227         r = rospy.Rate(self.config['frame_rate'])
00228         while self.is_looping():
00229             if self.pub_img_.get_num_connections() == 0:
00230                 if self.nameId:
00231                     rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
00232                     self.camProxy.unsubscribe(self.nameId)
00233                     self.nameId = None
00234                 r.sleep()
00235                 continue
00236             if self.nameId is None:
00237                 self.reconfigure(self.config, 0)
00238                 r.sleep()
00239                 continue
00240             image = self.camProxy.getImageRemote(self.nameId)
00241             if image is None:
00242                 continue
00243             # Deal with the image
00244             if self.config['use_ros_time']:
00245                 img.header.stamp = rospy.Time.now()
00246             else:
00247                 img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
00248             img.header.frame_id = self.frame_id
00249             img.height = image[1]
00250             img.width = image[0]
00251             nbLayers = image[2]
00252             if image[3] == kYUVColorSpace:
00253                 encoding = "mono8"
00254             elif image[3] == kRGBColorSpace:
00255                 encoding = "rgb8"
00256             elif image[3] == kBGRColorSpace:
00257                 encoding = "bgr8"
00258             elif image[3] == kYUV422ColorSpace:
00259                 encoding = "yuv422" # this works only in ROS groovy and later
00260             elif image[3] == kDepthColorSpace:
00261                 encoding = "mono16"
00262             else:
00263                 rospy.logerr("Received unknown encoding: {0}".format(image[3]))
00264             img.encoding = encoding
00265             img.step = img.width * nbLayers
00266             img.data = image[6]
00267 
00268             self.pub_img_.publish(img)
00269 
00270             # deal with the camera info
00271             if self.config['source'] == kDepthCamera and image[3] == kDepthColorSpace:
00272                 infomsg = CameraInfo()
00273                 # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
00274                 ratio_x = float(640)/float(img.width)
00275                 ratio_y = float(480)/float(img.height)
00276                 infomsg.width = img.width
00277                 infomsg.height = img.height
00278                 # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
00279                 infomsg.K = [ 525, 0, 3.1950000000000000e+02,
00280                               0, 525, 2.3950000000000000e+02,
00281                               0, 0, 1 ]
00282                 infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
00283                               0, 525, 2.3950000000000000e+02, 0,
00284                               0, 0, 1, 0 ]
00285                 for i in range(3):
00286                     infomsg.K[i] = infomsg.K[i] / ratio_x
00287                     infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
00288                     infomsg.P[i] = infomsg.P[i] / ratio_x
00289                     infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
00290 
00291                 infomsg.D = []
00292                 infomsg.binning_x = 0
00293                 infomsg.binning_y = 0
00294                 infomsg.distortion_model = ""
00295 
00296                 infomsg.header = img.header
00297                 self.pub_info_.publish(infomsg)
00298             elif self.config['camera_info_url'] in self.camera_infos:
00299                 infomsg = self.camera_infos[self.config['camera_info_url']]
00300 
00301                 infomsg.header = img.header
00302                 self.pub_info_.publish(infomsg)
00303 
00304             r.sleep()
00305 
00306         if (self.nameId):
00307             rospy.loginfo("unsubscribing from camera ")
00308             self.camProxy.unsubscribe(self.nameId)
00309 
00310 if __name__ == "__main__":
00311     naocam = NaoCam()
00312     naocam.start()
00313     rospy.spin()


nao_sensors
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 Sun Nov 2 2014 11:27:42