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)
