naoqi_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 naoqi_driver.naoqi_node import NaoqiNode
00041 import camera_info_manager
00042 
00043 from dynamic_reconfigure.server import Server
00044 from naoqi_sensors.cfg import NaoqiCameraConfig
00045 
00046 # import resolutions
00047 from naoqi_sensors.vision_definitions import k960p, k4VGA, kVGA, kQVGA, kQQVGA
00048 # import color spaces
00049 from naoqi_sensors.vision_definitions import kYUV422ColorSpace, kYUVColorSpace, \
00050                     kRGBColorSpace, kBGRColorSpace, kDepthColorSpace, kRawDepthColorSpace
00051 # import extra parameters
00052 from naoqi_sensors.vision_definitions import kCameraSelectID, kCameraAutoExpositionID, kCameraAecAlgorithmID, \
00053                   kCameraContrastID, kCameraSaturationID, kCameraHueID, kCameraSharpnessID, kCameraAutoWhiteBalanceID, \
00054                   kCameraExposureID, kCameraAutoGainID, 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 NaoqiCam (NaoqiNode):
00062     def __init__(self, node_name='naoqi_camera'):
00063         NaoqiNode.__init__(self, node_name)
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(NaoqiCameraConfig, 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_optical_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_optical_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'] = "/CameraDepth_optical_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         else:
00115             rospy.loginfo('no camera calibration for top camera found')
00116 
00117         if rospy.has_param('~calibration_file_bottom'):
00118             self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
00119         else:
00120             rospy.loginfo('no camera calibration for bottom camera found')
00121 
00122         # set time reference
00123         if rospy.has_param('~use_ros_time'):
00124             self.config['use_ros_time'] = rospy.get_param('~use_ros_time')
00125         else:
00126             self.config['use_ros_time'] = False
00127 
00128 
00129     def load_camera_info( self ):
00130         if self.config['source'] == 0:
00131             self.config['camera_info_url'] = self.config['calibration_file_top']
00132         elif self.config['source'] == 1:
00133             self.config['camera_info_url'] = self.config['calibration_file_bottom']
00134         else:
00135             rospy.loginfo('no valid camera calibration file found')
00136 
00137     def reconfigure( self, new_config, level ):
00138         """
00139         Reconfigure the camera
00140         """
00141         rospy.loginfo('reconfigure changed')
00142         if self.pub_img_.get_num_connections() == 0:
00143             rospy.loginfo('Changes recorded but not applied as nobody is subscribed to the ROS topics.')
00144             self.config.update(new_config)
00145             return self.config
00146 
00147         # check if we are even subscribed to a camera
00148         is_camera_new = self.nameId is None
00149 
00150         if is_camera_new:
00151             rospy.loginfo('subscribed to camera proxy, since this is the first camera')
00152             self.nameId = self.camProxy.subscribeCamera("rospy_gvm", new_config['source'],
00153                                                         new_config['resolution'], new_config['color_space'],
00154                                                         new_config['frame_rate'])
00155 
00156         if self.config['source'] != new_config['source'] or is_camera_new:
00157             rospy.loginfo('updating camera source information')
00158 
00159             if new_config['source'] == kTopCamera:
00160                 self.frame_id = self.config['camera_top_frame']
00161             elif new_config['source'] == kBottomCamera:
00162                 self.frame_id = self.config['camera_bottom_frame']
00163             elif new_config['source'] == kDepthCamera:
00164                 self.frame_id = self.config['camera_depth_frame']
00165             else:
00166                 rospy.logerr('Invalid source. Must be 0, 1 or 2')
00167                 exit(1)
00168 
00169         # check if the camera changed
00170         if self.config['camera_info_url'] == "" or \
00171            ( self.config['camera_info_url'] != new_config['camera_info_url'] and \
00172            new_config['camera_info_url'] not in self.camera_infos ):
00173 
00174             self.load_camera_info()
00175 
00176             if 'cim' not in self.__dict__:
00177                 self.cim = camera_info_manager.CameraInfoManager(cname='nao_camera')
00178 
00179             if not self.cim.setURL( new_config['camera_info_url'] ):
00180                 rospy.logerr('malformed URL for calibration file')
00181             else:
00182                 try:
00183                     self.cim.loadCameraInfo()
00184                 except IOExcept:
00185                     rospy.logerr('Could not read from existing calibration file')
00186 
00187             if self.cim.isCalibrated():
00188                 rospy.loginfo('Successfully loaded camera info')
00189                 self.camera_infos[new_config['camera_info_url']] = self.cim.getCameraInfo()
00190             else:
00191                 rospy.logerr('There was a problem loading the calibration file. Check the URL!')
00192 
00193         # set params
00194         camParams = self.extractParams(new_config)
00195         self.setParams(camParams)
00196 
00197         key_methods =  [ ('resolution', 'setResolution'), ('color_space', 'setColorSpace'), ('frame_rate', 'setFrameRate')]
00198         if self.get_version() >= LooseVersion('2.0'):
00199             key_methods.append(('source', 'setActiveCamera'))
00200         for key, method in key_methods:
00201             if self.config[key] != new_config[key] or is_camera_new:
00202                 self.camProxy.__getattribute__(method)(self.nameId, new_config[key])
00203 
00204         self.config.update(new_config)
00205 
00206         return self.config
00207 
00208     def extractParams(self, new_config):
00209         camParams = []
00210 
00211         camParams.append( (kCameraAecAlgorithmID, new_config['auto_exposure_algo']) )
00212         camParams.append( (kCameraContrastID, new_config['contrast']) )
00213         camParams.append( (kCameraSaturationID, new_config['saturation']) )
00214         camParams.append( (kCameraHueID, new_config['hue']) ) # Migth be deprecated
00215         camParams.append( (kCameraSharpnessID, new_config['sharpness']) )
00216 
00217         camParams.append( (kCameraAutoWhiteBalanceID, new_config['auto_white_balance']) )
00218         if ( new_config['auto_white_balance']==0):
00219             camParams.append( (kCameraWhiteBalanceID, new_config['white_balance']) )
00220 
00221         camParams.append( (kCameraAutoExpositionID, new_config['auto_exposition']) )
00222         if ( new_config['auto_exposition']==0):
00223             camParams.append( (kCameraGainID, new_config['gain']) )
00224             camParams.append( (kCameraExposureID, new_config['exposure']) )
00225         else:
00226             camParams.append( (kCameraBrightnessID, new_config['brightness']) )
00227 
00228         return camParams
00229 
00230     def setParams(self, key_list):
00231         for key, value in key_list:
00232             if self.get_version() < LooseVersion('2.0'):
00233                 self.camProxy.setParam(key, value)
00234             else:
00235                 self.camProxy.setCameraParameter(self.nameId, key, value)
00236 
00237     def run(self):
00238         img = Image()
00239         r = rospy.Rate(self.config['frame_rate'])
00240         while self.is_looping():
00241             if self.pub_img_.get_num_connections() == 0:
00242                 if self.nameId:
00243                     rospy.loginfo('Unsubscribing from camera as nobody listens to the topics.')
00244                     self.camProxy.unsubscribe(self.nameId)
00245                     self.nameId = None
00246                 r.sleep()
00247                 continue
00248             if self.nameId is None:
00249                 self.reconfigure(self.config, 0)
00250                 r.sleep()
00251                 continue
00252             image = self.camProxy.getImageRemote(self.nameId)
00253             if image is None:
00254                 continue
00255             # Deal with the image
00256             if self.config['use_ros_time']:
00257                 img.header.stamp = rospy.Time.now()
00258             else:
00259                 img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
00260             img.header.frame_id = self.frame_id
00261             img.height = image[1]
00262             img.width = image[0]
00263             nbLayers = image[2]
00264             if image[3] == kYUVColorSpace:
00265                 encoding = "mono8"
00266             elif image[3] == kRGBColorSpace:
00267                 encoding = "rgb8"
00268             elif image[3] == kBGRColorSpace:
00269                 encoding = "bgr8"
00270             elif image[3] == kYUV422ColorSpace:
00271                 encoding = "yuv422" # this works only in ROS groovy and later
00272             elif image[3] == kDepthColorSpace or image[3] == kRawDepthColorSpace:
00273                 encoding = "16UC1"
00274             else:
00275                 rospy.logerr("Received unknown encoding: {0}".format(image[3]))
00276             img.encoding = encoding
00277             img.step = img.width * nbLayers
00278             img.data = image[6]
00279 
00280             self.pub_img_.publish(img)
00281 
00282             # deal with the camera info
00283             if self.config['source'] == kDepthCamera and (image[3] == kDepthColorSpace or image[3] == kRawDepthColorSpace):
00284                 infomsg = CameraInfo()
00285                 # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
00286                 ratio_x = float(640)/float(img.width)
00287                 ratio_y = float(480)/float(img.height)
00288                 infomsg.width = img.width
00289                 infomsg.height = img.height
00290                 # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
00291                 infomsg.K = [ 525, 0, 3.1950000000000000e+02,
00292                               0, 525, 2.3950000000000000e+02,
00293                               0, 0, 1 ]
00294                 infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
00295                               0, 525, 2.3950000000000000e+02, 0,
00296                               0, 0, 1, 0 ]
00297                 for i in range(3):
00298                     infomsg.K[i] = infomsg.K[i] / ratio_x
00299                     infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
00300                     infomsg.P[i] = infomsg.P[i] / ratio_x
00301                     infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
00302 
00303                 infomsg.D = []
00304                 infomsg.binning_x = 0
00305                 infomsg.binning_y = 0
00306                 infomsg.distortion_model = ""
00307 
00308                 infomsg.header = img.header
00309                 self.pub_info_.publish(infomsg)
00310             elif self.config['camera_info_url'] in self.camera_infos:
00311                 infomsg = self.camera_infos[self.config['camera_info_url']]
00312 
00313                 infomsg.header = img.header
00314                 self.pub_info_.publish(infomsg)
00315 
00316             r.sleep()
00317 
00318         if (self.nameId):
00319             rospy.loginfo("unsubscribing from camera ")
00320             self.camProxy.unsubscribe(self.nameId)


naoqi_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 Fri Jul 3 2015 12:51:48