def __init__(self): """ Constructor. """ camera_param_dict = RobotParams().get_camera_details() camera_list = camera_param_dict.keys() # check to make sure cameras is not an empty list if not camera_list: rospy.logerr(' '.join( ["camera list is empty: ", ' , '.join(camera_list)])) return camera_color_dict = {"mono": ['cognex'], "color": ['ienso_ethernet']} self.cameras_io = dict() for camera in camera_list: if camera_param_dict[camera]['cameraType'] in camera_color_dict[ '' 'color']: is_color = True else: is_color = False self.cameras_io[camera] = { 'interface': IODeviceInterface("internal" "_camera", camera), 'is_color': is_color }
def __init__(self): """ Constructor. """ self._node_config = None self._node_config_sub = rospy.Subscriber('io/internal_camera/config', IONodeConfiguration, self._node_config_cb) self.cameras_io = dict() camera_param_dict = RobotParams().get_camera_details() camera_list = camera_param_dict.keys() # check to make sure cameras is not an empty list if not camera_list: rospy.logerr(' '.join( ["camera list is empty: ", ' , '.join(camera_list)])) return intera_dataflow.wait_for( lambda: self._node_config is not None, timeout=5.0, timeout_msg=( "Failed to connect to Camera Node and retrieve configuration." )) cameras_to_load = self._get_camera_launch_config().keys() camera_capabilities = { "mono": ['cognex'], "color": ['ienso_ethernet'], "auto_exposure": ['ienso_ethernet'], "auto_gain": ['ienso_ethernet'] } for camera in camera_list: cameraType = camera_param_dict[camera]['cameraType'] try: interface = IODeviceInterface("internal_camera", camera) self.cameras_io[camera] = { 'interface': interface, 'is_color': (cameraType in camera_capabilities['color']), 'has_auto_exposure': (cameraType in camera_capabilities['auto_exposure']), 'has_auto_gain': (cameraType in camera_capabilities['auto_gain']), } except OSError as e: if camera not in cameras_to_load: rospy.logwarn( "Expected camera ({0}) is not configured to launch in this run" " configuration. Make sure you are running in SDK Mode." .format(camera)) else: rospy.logerr( "Could not find expected camera ({0}) for this robot.\n" "Please contact Rethink support: [email protected]" .format(camera))