36 from collections
import defaultdict
37 from distutils.version
import LooseVersion
39 from sensor_msgs.msg
import Image, CameraInfo
41 import camera_info_manager
43 from dynamic_reconfigure.server
import Server
44 from naoqi_sensors_py.cfg
import NaoqiCameraConfig
50 kRGBColorSpace, kBGRColorSpace, kDepthColorSpace, kRawDepthColorSpace
53 kCameraContrastID, kCameraSaturationID, kCameraHueID, kCameraSharpnessID, kCameraAutoWhiteBalanceID, \
54 kCameraExposureID, kCameraAutoGainID, kCameraGainID, kCameraBrightnessID, kCameraWhiteBalanceID
63 NaoqiNode.__init__(self, node_name)
72 self.
config = defaultdict(returnNone)
75 self.
pub_img_ = rospy.Publisher(
'~image_raw', Image, queue_size=5)
76 self.
pub_info_ = rospy.Publisher(
'~camera_info', CameraInfo, queue_size=5)
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')
96 if rospy.has_param(
'~camera_top_frame'):
97 self.
config[
'camera_top_frame'] = rospy.get_param(
'~camera_top_frame')
99 self.
config[
'camera_top_frame'] =
"/CameraTop_optical_frame" 101 if rospy.has_param(
'~camera_bottom_frame'):
102 self.
config[
'camera_bottom_frame'] = rospy.get_param(
'~camera_bottom_frame')
104 self.
config[
'camera_bottom_frame'] =
"/CameraBottom_optical_frame" 106 if rospy.has_param(
'~camera_depth_frame'):
107 self.
config[
'camera_depth_frame'] = rospy.get_param(
'~camera_depth_frame')
109 self.
config[
'camera_depth_frame'] =
"/CameraDepth_optical_frame" 112 if rospy.has_param(
'~calibration_file_top'):
113 self.
config[
'calibration_file_top'] = rospy.get_param(
'~calibration_file_top')
115 rospy.loginfo(
'no camera calibration for top camera found')
117 if rospy.has_param(
'~calibration_file_bottom'):
118 self.
config[
'calibration_file_bottom'] = rospy.get_param(
'~calibration_file_bottom')
120 rospy.loginfo(
'no camera calibration for bottom camera found')
123 if rospy.has_param(
'~use_ros_time'):
124 self.
config[
'use_ros_time'] = rospy.get_param(
'~use_ros_time')
126 self.
config[
'use_ros_time'] =
False 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']
135 rospy.loginfo(
'no valid camera calibration file found')
139 Reconfigure the camera 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)
148 is_camera_new = self.
nameId is None 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'])
156 if self.
config[
'source'] != new_config[
'source']
or is_camera_new:
157 rospy.loginfo(
'updating camera source information')
159 if new_config[
'source'] == kTopCamera:
161 elif new_config[
'source'] == kBottomCamera:
163 elif new_config[
'source'] == kDepthCamera:
166 rospy.logerr(
'Invalid source. Must be 0, 1 or 2')
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 ):
176 if 'cim' not in self.__dict__:
179 if not self.cim.setURL( new_config[
'camera_info_url'] ):
180 rospy.logerr(
'malformed URL for calibration file')
183 self.cim.loadCameraInfo()
185 rospy.logerr(
'Could not read from existing calibration file')
187 if self.cim.isCalibrated():
188 rospy.loginfo(
'Successfully loaded camera info')
189 self.
camera_infos[new_config[
'camera_info_url']] = self.cim.getCameraInfo()
191 rospy.logerr(
'There was a problem loading the calibration file. Check the URL!')
197 key_methods = [ (
'resolution',
'setResolution'), (
'color_space',
'setColorSpace'), (
'frame_rate',
'setFrameRate')]
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])
204 self.config.update(new_config)
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']) )
215 camParams.append( (kCameraSharpnessID, new_config[
'sharpness']) )
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']) )
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']) )
226 camParams.append( (kCameraBrightnessID, new_config[
'brightness']) )
231 for key, value
in key_list:
233 self.camProxy.setParam(key, value)
235 self.camProxy.setCameraParameter(self.
nameId, key, value)
239 r = rospy.Rate(self.
config[
'frame_rate'])
241 if self.pub_img_.get_num_connections() == 0:
243 rospy.loginfo(
'Unsubscribing from camera as nobody listens to the topics.')
244 self.camProxy.unsubscribe(self.
nameId)
252 image = self.camProxy.getImageRemote(self.
nameId)
256 if self.
config[
'use_ros_time']:
257 img.header.stamp = rospy.Time.now()
259 img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
261 img.height = image[1]
264 if image[3] == kYUVColorSpace:
266 elif image[3] == kRGBColorSpace:
268 elif image[3] == kBGRColorSpace:
270 elif image[3] == kYUV422ColorSpace:
272 elif image[3] == kDepthColorSpace
or image[3] == kRawDepthColorSpace:
275 rospy.logerr(
"Received unknown encoding: {0}".format(image[3]))
276 img.encoding = encoding
277 img.step = img.width * nbLayers
280 self.pub_img_.publish(img)
283 if self.
config[
'source'] == kDepthCamera
and (image[3] == kDepthColorSpace
or image[3] == kRawDepthColorSpace):
284 infomsg = CameraInfo()
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
291 infomsg.K = [ 525, 0, 3.1950000000000000e+02,
292 0, 525, 2.3950000000000000e+02,
294 infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
295 0, 525, 2.3950000000000000e+02, 0,
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
304 infomsg.binning_x = 0
305 infomsg.binning_y = 0
306 infomsg.distortion_model =
"" 308 infomsg.header = img.header
309 self.pub_info_.publish(infomsg)
313 infomsg.header = img.header
314 self.pub_info_.publish(infomsg)
319 rospy.loginfo(
"unsubscribing from camera ")
320 self.camProxy.unsubscribe(self.
nameId)
def setParams(self, key_list)
def reconfigure(self, new_config, level)
def __init__(self, node_name='naoqi_camera')
def load_camera_info(self)
def extractParams(self, new_config)
def get_proxy(self, name, warn=True)