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()