def __init__(self, config_dir, system, robot_desc, output_debug=False): # Load configs self.cam_config = yaml.load(open(config_dir + "/cam_config.yaml")) self.chain_config = yaml.load(open(config_dir + "/chain_config.yaml")) self.laser_config = yaml.load(open(config_dir + "/laser_config.yaml")) self.controller_config = yaml.load( open(config_dir + "/controller_config.yaml")) # Not all robots have lasers.... :( if self.laser_config == None: self.laser_config = dict() # Debug mode makes bag files huge... self.output_debug = output_debug # Error message from sample capture self.message = None # Status message from interval computation self.interval_status = None # parse urdf and get list of links links = URDF().parse(robot_desc).link_map.keys() # load system config system = yaml.load(open(system)) # remove cams that are not on urdf for cam in self.cam_config.keys(): try: link = system['sensors']['rectified_cams'][cam]['frame_id'] if not link in links: print 'URDF does not contain link [%s]. Removing camera [%s]' % ( link, cam) del self.cam_config[cam] except: print 'System description does not contain camera [%s]' % cam del self.cam_config[cam] # remove arms that are not on urdf for chain in self.chain_config.keys(): try: link = system['sensors']['chains'][chain]['tip'] if not link in links: print 'URDF does not contain link [%s]. Removing chain [%s]' % ( link, chain) del self.chain_config[chain] except: print 'System description does not contain chain [%s]' % chain del self.chain_config[chain] self.cache = RobotMeasurementCache() self.lock = threading.Lock() # Specifies if we're currently waiting for a sample self.active = False # Construct Configuration Manager (manages configuration of nodes in the pipeline) self.config_manager = ConfigManager(self.cam_config, self.chain_config, self.laser_config, self.controller_config) # Construct a manager for each sensor stream (Don't enable any of them) self.cam_managers = [(cam_id, CamManager(cam_id, self.add_cam_measurement)) for cam_id in self.cam_config.keys()] self.chain_managers = [(chain_id, ChainManager(chain_id, self.add_chain_measurement)) for chain_id in self.chain_config.keys()] self.laser_managers = [(laser_id, LaserManager(laser_id, self.add_laser_measurement)) for laser_id in self.laser_config.keys()] # Subscribe to topic containing stable intervals self.request_interval_sub = rospy.Subscriber( "intersected_interval", calibration_msgs.msg.Interval, self.request_callback) # Subscribe to topic containing stable intervals self.interval_status_sub = rospy.Subscriber( "intersected_interval_status", calibration_msgs.msg.IntervalStatus, self.status_callback)