def test_laser_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure([], [], ["laser1"])
cache.set_max_sizes( {}, {}, {"laser1":100})
for n in range(1,11):
msg = LaserMeasurement()
cache.add_laser_measurement("laser1", msg, rospy.Time(n*10,0), rospy.Time(n*10+2,0))
m_robot = cache.request_robot_measurement(rospy.Time(19,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 0 )
self.assertEquals( len(m_robot.M_chain), 0 )
self.assertEquals( len(m_robot.M_laser), 1 )
m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(21,0))
self.assertTrue(m_robot is None)
def test_chain_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure([], ["chain1"], [])
cache.set_max_sizes( {}, {"chain1":100}, {})
for n in range(1,11):
msg = ChainMeasurement()
msg.header.stamp = rospy.Time(n*10,0)
cache.add_chain_measurement("chain1", msg)
m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 0 )
self.assertEquals( len(m_robot.M_chain), 1 )
self.assertEquals( len(m_robot.M_laser), 0 )
m_robot = cache.request_robot_measurement(rospy.Time(21,0), rospy.Time(29,0))
self.assertTrue(m_robot is None)
def test_multi_cam_easy(self):
cache = RobotMeasurementCache()
cache.reconfigure(["cam1", "cam2"], [], [])
cache.set_max_sizes( {"cam1":100, "cam2":100}, {}, {})
for n in range(1,11):
msg = CameraMeasurement()
msg.header.stamp = rospy.Time(n*10,0)
cache.add_cam_measurement("cam1", msg)
for n in range(1,11):
msg = CameraMeasurement()
msg.header.stamp = rospy.Time(n*10+1,0)
cache.add_cam_measurement("cam2", msg)
m_robot = cache.request_robot_measurement(rospy.Time(18,0), rospy.Time(32,0))
self.assertTrue(m_robot is not None)
self.assertEquals( len(m_robot.M_cam), 2 )
self.assertEquals( len(m_robot.M_chain), 0 )
self.assertEquals( len(m_robot.M_laser), 0 )
m_robot = cache.request_robot_measurement(rospy.Time(20,0), rospy.Time(30,0))
self.assertTrue(m_robot is None)
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)
def __init__(self, config_dir, system, robot_desc):
# 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 all the things
if self.cam_config == None:
self.cam_config = dict()
if self.chain_config == None:
self.chain_config = dict()
if self.laser_config == None:
self.laser_config = dict()
if self.controller_config == None:
self.controller_config = dict()
# parse urdf and get list of links
links = URDF().parse(robot_desc).links.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:
rospy.logwarn(
'URDF does not contain link [%s]. Removing camera [%s]'
% (link, cam))
del self.cam_config[cam]
except:
rospy.logwarn(
'System description does not contain camera [%s]' % cam)
del self.cam_config[cam]
# remove lasers that are not on urdf
for laser in self.laser_config.keys():
try:
link = system['sensors']['tilting_lasers'][laser]['frame_id']
if not link in links:
rospy.logwarn(
'URDF does not contain link [%s]. Removing laser [%s]'
% (link, laser))
del self.laser_config[laser]
except:
rospy.logwarn(
'System description does not contain laser [%s]' % laser)
del self.laser_config[laser]
# remove chains that are not on urdf
for chain in self.chain_config.keys():
try:
link = system['sensors']['chains'][chain]['tip']
if not link in links:
rospy.logwarn(
'URDF does not contain link [%s]. Removing chain [%s]'
% (link, chain))
del self.chain_config[chain]
continue
link = system['sensors']['chains'][chain]['root']
if not link in links:
rospy.logwarn(
'URDF does not contain link [%s]. Removing chain [%s]'
% (link, chain))
del self.chain_config[chain]
continue
except:
rospy.logwarn(
'System description does not contain chain [%s]' % chain)
del self.chain_config[chain]
# 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()]
# Create measurement cache
self.cache = RobotMeasurementCache()
# Set up goals
self.goal_sample_id = None
self.goal_target_id = None
self.goal_chain_id = None
# Set up publisher
self.result = rospy.Publisher("robot_measurement", RobotMeasurement)
# Subscribe to topic containing stable intervals
self.request_interval_sub = rospy.Subscriber(
"intersected_interval", calibration_msgs.msg.Interval,
self.request_callback)
self.interval_status = CaptureManagerFeedback()
self.interval_status_sub = rospy.Subscriber(
"intersected_interval_status", calibration_msgs.msg.IntervalStatus,
self.status_callback)
# Set up action
self._server = actionlib.SimpleActionServer('capture_manager',
CaptureManagerAction, None,
False)
self._server.register_goal_callback(self.goal_callback)
self._server.register_preempt_callback(self.preempt_callback)
self._server.start()
