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 __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)
class CaptureExecutive: 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) # Hardcoded cache sizes. Not sure where these params should live # ... def capture(self, next_configuration, timeout): done = False self.m_robot = None self.message = None self.interval_status = None timeout_time = rospy.Time().now() + timeout self.lock.acquire() if self.active: rospy.logerr("Can't capture since we're already active") done = True self.lock.release() camera_measurements = next_configuration["camera_measurements"] next_configuration["camera_measurements"] = [] for (i, cam) in enumerate(camera_measurements): if self.cam_config.has_key(cam["cam_id"]): next_configuration["camera_measurements"].append(cam) else: rospy.logdebug("Not capturing measurement for camera: %s" % (cam["cam_id"])) # Set up the pipeline self.config_manager.reconfigure(next_configuration) time.sleep(2.0) # Set up the cache with only the sensors we care about cam_ids = [ x["cam_id"] for x in next_configuration["camera_measurements"] ] chain_ids = [ x["chain_id"] for x in next_configuration["joint_measurements"] ] try: laser_ids = [ x["laser_id"] for x in next_configuration["laser_measurements"] ] except: laser_ids = list() self.cache.reconfigure(cam_ids, chain_ids, laser_ids) rospy.logdebug("Setting up sensor managers") enable_list = [] disable_list = [] # Set up the sensor managers for cam_id, cam_manager in self.cam_managers: if cam_id in [ x["cam_id"] for x in next_configuration["camera_measurements"] ]: enable_list.append(cam_id) cam_manager.enable(self.output_debug) else: disable_list.append(cam_id) cam_manager.disable() for chain_id, chain_manager in self.chain_managers: if chain_id in [ x["chain_id"] for x in next_configuration["joint_measurements"] ]: enable_list.append(chain_id) chain_manager.enable() else: disable_list.append(chain_id) chain_manager.disable() for laser_id, laser_manager in self.laser_managers: enabled_lasers = [ x["laser_id"] for x in next_configuration["laser_measurements"] ] if laser_id in enabled_lasers: enable_list.append(laser_id) laser_manager.enable(self.output_debug) else: disable_list.append(laser_id) laser_manager.disable() print "Enabling:" for cur_enabled in enable_list: print " + %s" % cur_enabled print "Disabling:" for cur_disabled in disable_list: print " - %s" % cur_disabled self.lock.acquire() self.active = True self.lock.release() print "\nCollecting sensor data.." # Keep waiting until the request_callback function populates the m_robot msg while (not rospy.is_shutdown()) and (not done) and (rospy.Time().now() < timeout_time): time.sleep(.1) self.lock.acquire() if self.m_robot is not None: done = True self.lock.release() print "Stopping sensor streams..\n" # Stop listening to all the sensor streams for cam_id, cam_manager in self.cam_managers: cam_manager.disable() for chain_id, chain_manager in self.chain_managers: chain_manager.disable() for laser_id, laser_manager in self.laser_managers: laser_manager.disable() # Turn off the entire pipeline self.config_manager.stop() # Fill in meta information about the sample if self.m_robot is not None: self.m_robot.sample_id = next_configuration["sample_id"] self.m_robot.target_id = next_configuration["target"]["target_id"] self.m_robot.chain_id = next_configuration["target"]["chain_id"] elif self.interval_status is not None: total = 0 bad_sensors = [] l = len(self.interval_status.names) if l != len(self.interval_status.yeild_rates): rospy.logerr( "Invalid interval status message; names and yeild rates have different lengths" ) l = min(l, len(self.interval_status.yeild_rates)) # analyze status message for i in range(l): if self.interval_status.yeild_rates[i] == 0.0: bad_sensors.append(self.interval_status.names[i]) total += self.interval_status.yeild_rates[i] # if we didn't get any samples from some sensors, complain if len(bad_sensors) > 0: print "Didn't get good data from %s" % (', '.join(bad_sensors)) else: # if we got data from all of our sensors, complain about the # ones that were below the mean (heuristic) avg = total / l for i in range(l): if self.interval_status.yeild_rates[i] <= avg: bad_sensors.append(self.interval_status.names[i]) print "%s may be performing poorly" % (", ".join(bad_sensors)) elif self.message is not None: print self.message self.lock.acquire() self.active = False self.lock.release() return self.m_robot def request_callback(self, msg): # See if the interval is big enough to care if (msg.end - msg.start) < rospy.Duration(1, 0): return self.lock.acquire() if self.active: # Do some query into the cache, and possibly stop stuff from running m = self.cache.request_robot_measurement(msg.start, msg.end) if isinstance(m, basestring): self.message = m else: self.m_robot = m # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care) if self.m_robot is not None: self.active = False self.lock.release() def status_callback(self, msg): self.lock.acquire() if self.active: self.interval_status = msg self.lock.release() def add_cam_measurement(self, cam_id, msg): if len(msg.image_points) > 0: self.lock.acquire() self.cache.add_cam_measurement(cam_id, msg) self.lock.release() def add_chain_measurement(self, chain_id, msg): self.lock.acquire() self.cache.add_chain_measurement(chain_id, msg) self.lock.release() def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): self.lock.acquire() self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end) self.lock.release()
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).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: 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)
class CaptureExecutive: 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).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: 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) # Hardcoded cache sizes. Not sure where these params should live # ... def capture(self, next_configuration, timeout): done = False self.m_robot = None self.message = None self.interval_status = None timeout_time = rospy.Time().now() + timeout self.lock.acquire() if self.active: rospy.logerr("Can't capture since we're already active") done = True self.lock.release() camera_measurements = next_configuration["camera_measurements"] next_configuration["camera_measurements"] = [] for (i, cam) in enumerate(camera_measurements): if self.cam_config.has_key(cam["cam_id"]): next_configuration["camera_measurements"].append(cam) else: rospy.logdebug("Not capturing measurement for camera: %s"%(cam["cam_id"])) # Set up the pipeline self.config_manager.reconfigure(next_configuration) time.sleep(2.0) # Set up the cache with only the sensors we care about cam_ids = [x["cam_id"] for x in next_configuration["camera_measurements"]] chain_ids = [x["chain_id"] for x in next_configuration["joint_measurements"]] try: laser_ids = [x["laser_id"] for x in next_configuration["laser_measurements"]] except: laser_ids = list() self.cache.reconfigure(cam_ids, chain_ids, laser_ids) rospy.logdebug("Setting up sensor managers") enable_list = [] disable_list = [] # Set up the sensor managers for cam_id, cam_manager in self.cam_managers: if cam_id in [x["cam_id"] for x in next_configuration["camera_measurements"]]: enable_list.append(cam_id) cam_manager.enable(self.output_debug) else: disable_list.append(cam_id) cam_manager.disable() for chain_id, chain_manager in self.chain_managers: if chain_id in [x["chain_id"] for x in next_configuration["joint_measurements"]]: enable_list.append(chain_id) chain_manager.enable() else: disable_list.append(chain_id) chain_manager.disable() for laser_id, laser_manager in self.laser_managers: enabled_lasers = [x["laser_id"] for x in next_configuration["laser_measurements"]] if laser_id in enabled_lasers: enable_list.append(laser_id) laser_manager.enable(self.output_debug) else: disable_list.append(laser_id) laser_manager.disable() print "Enabling:" for cur_enabled in enable_list: print " + %s" % cur_enabled print "Disabling:" for cur_disabled in disable_list: print " - %s" % cur_disabled self.lock.acquire() self.active = True self.lock.release() print "\nCollecting sensor data.." # Keep waiting until the request_callback function populates the m_robot msg while (not rospy.is_shutdown()) and (not done) and (rospy.Time().now() < timeout_time): time.sleep(.1) self.lock.acquire() if self.m_robot is not None: done = True self.lock.release() print "Stopping sensor streams..\n" # Stop listening to all the sensor streams for cam_id, cam_manager in self.cam_managers: cam_manager.disable() for chain_id, chain_manager in self.chain_managers: chain_manager.disable() for laser_id, laser_manager in self.laser_managers: laser_manager.disable() # Turn off the entire pipeline self.config_manager.stop() # Fill in meta information about the sample if self.m_robot is not None: self.m_robot.sample_id = next_configuration["sample_id"] self.m_robot.target_id = next_configuration["target"]["target_id"] self.m_robot.chain_id = next_configuration["target"]["chain_id"] elif self.interval_status is not None: total = 0 bad_sensors = [] l = len(self.interval_status.names) if l != len(self.interval_status.yeild_rates): rospy.logerr("Invalid interval status message; names and yeild rates have different lengths") l = min(l, len(self.interval_status.yeild_rates)) # analyze status message for i in range(l): if self.interval_status.yeild_rates[i] == 0.0: bad_sensors.append(self.interval_status.names[i]) total += self.interval_status.yeild_rates[i] # if we didn't get any samples from some sensors, complain if len(bad_sensors) > 0: print "Didn't get good data from %s"%(', '.join(bad_sensors)) else: # if we got data from all of our sensors, complain about the # ones that were below the mean (heuristic) avg = total / l for i in range(l): if self.interval_status.yeild_rates[i] <= avg: bad_sensors.append(self.interval_status.names[i]) print "%s may be performing poorly"%(", ".join(bad_sensors)) elif self.message is not None: print self.message self.lock.acquire() self.active = False self.lock.release() return self.m_robot def request_callback(self, msg): # See if the interval is big enough to care if (msg.end - msg.start) < rospy.Duration(1,0): return self.lock.acquire() if self.active: # Do some query into the cache, and possibly stop stuff from running m = self.cache.request_robot_measurement(msg.start, msg.end) if isinstance(m, basestring): self.message = m else: self.m_robot = m # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care) if self.m_robot is not None: self.active = False self.lock.release() def status_callback(self, msg): self.lock.acquire() if self.active: self.interval_status = msg self.lock.release() def add_cam_measurement(self, cam_id, msg): if len(msg.image_points) > 0: self.lock.acquire() self.cache.add_cam_measurement(cam_id, msg) self.lock.release() def add_chain_measurement(self, chain_id, msg): self.lock.acquire() self.cache.add_chain_measurement(chain_id, msg) self.lock.release() def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): self.lock.acquire() self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end) self.lock.release()
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): # 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()
class CaptureManagerServer: 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() def goal_callback(self): rospy.loginfo("[%s] Setting calibration configuration" % rospy.get_name()) goal = self._server.accept_new_goal() # get ids cam_ids = [x.id for x in goal.camera_measurements] laser_ids = [x.id for x in goal.laser_measurements] chain_ids = [x.id for x in goal.joint_measurements] self.goal_sample_id = goal.sample_id self.goal_target_id = goal.target_id self.goal_chain_id = goal.chain_id # Set up the pipeline self.config_manager.reconfigure(goal) rospy.logdebug("[%s] Set up pipelines" % rospy.get_name()) # Set up cache self.cache.clear() self.cache.reconfigure(cam_ids, chain_ids, laser_ids) rospy.logdebug("[%s] Configurating cache" % rospy.get_name()) # Set up the sensor managers enable_list = [] disable_list = [] for cam_id, cam_manager in self.cam_managers: if cam_id in cam_ids: enable_list.append(cam_id) cam_manager.enable() else: disable_list.append(cam_id) cam_manager.disable() rospy.logdebug("[%s] Set up camera managers" % rospy.get_name()) for chain_id, chain_manager in self.chain_managers: if chain_id in chain_ids: enable_list.append(chain_id) chain_manager.enable() else: disable_list.append(chain_id) chain_manager.disable() rospy.logdebug("[%s] Set up chain managers" % rospy.get_name()) for laser_id, laser_manager in self.laser_managers: if laser_id in laser_ids: enable_list.append(laser_id) laser_manager.enable() else: disable_list.append(laser_id) laser_manager.disable() rospy.logdebug("[%s] Set up laser managers" % rospy.get_name()) if self._server.is_preempt_requested(): rospy.loginfo("[%s] preempt detected during accept" % rospy.get_name()) self._server.set_preempted() def preempt_callback(self): rospy.loginfo("[%s] preemting..." % rospy.get_name()) self._server.set_preempted() self.goal_sample_id = None self.goal_target_id = None self.goal_chain_id = None #disable all the things for cam_id, cam_manager in self.cam_managers: cam_manager.disable() rospy.loginfo("[%s] Disabled camera managers" % rospy.get_name()) for chain_id, chain_manager in self.chain_managers: chain_manager.disable() rospy.loginfo("[%s] Disabled chain managers" % rospy.get_name()) for laser_id, laser_manager in self.laser_managers: laser_manager.disable() rospy.loginfo("[%s] Disabled laser managers" % rospy.get_name()) def request_callback(self, msg): rospy.logdebug("[%s] in request_callback" % rospy.get_name()) # See if the interval is big enough to care # TODO: make this a property if(self._server.is_active() and self.goal_sample_id): rospy.logdebug("[%s] server active" % rospy.get_name()) if (msg.end - msg.start) < rospy.Duration(1,0): self.interval_status.progress = "interval not big enough" self._server.publish_feedback(self.interval_status) return m = self.cache.request_robot_measurement(msg.start, msg.end) if isinstance(m, basestring): self.interval_status.progress = m else: m.sample_id = self.goal_sample_id m.target_id = self.goal_target_id m.chain_id = self.goal_chain_id msg = CaptureManagerResult() msg.result = m # Set succeeded self._server.set_succeeded(msg) # Publish RobotMeasurement self.result.publish(m) # Reset ids self.goal_sample_id = None self.goal_target_id = None self.goal_chain_id = None self.interval_status.progress = "succeeded" self._server.publish_feedback(self.interval_status) def status_callback(self, msg): if(self._server.is_active() and self.goal_sample_id): self.interval_status.status = msg self._server.publish_feedback(self.interval_status) def add_cam_measurement(self, cam_id, msg): if (self._server.is_active() and self.goal_sample_id): self.cache.add_cam_measurement(cam_id, msg) def add_chain_measurement(self, chain_id, msg): if(self._server.is_active() and self.goal_sample_id): self.cache.add_chain_measurement(chain_id, msg) def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): if(self._server.is_active() and self.goal_sample_id): self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end)
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()
class CaptureManagerServer: 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() def goal_callback(self): rospy.loginfo("[%s] Setting calibration configuration" % rospy.get_name()) goal = self._server.accept_new_goal() # get ids cam_ids = [x.id for x in goal.camera_measurements] laser_ids = [x.id for x in goal.laser_measurements] chain_ids = [x.id for x in goal.joint_measurements] self.goal_sample_id = goal.sample_id self.goal_target_id = goal.target_id self.goal_chain_id = goal.chain_id # Set up the pipeline self.config_manager.reconfigure(goal) rospy.logdebug("[%s] Set up pipelines" % rospy.get_name()) # Set up cache self.cache.clear() self.cache.reconfigure(cam_ids, chain_ids, laser_ids) rospy.logdebug("[%s] Configurating cache" % rospy.get_name()) # Set up the sensor managers enable_list = [] disable_list = [] for cam_id, cam_manager in self.cam_managers: if cam_id in cam_ids: enable_list.append(cam_id) cam_manager.enable() else: disable_list.append(cam_id) cam_manager.disable() rospy.logdebug("[%s] Set up camera managers" % rospy.get_name()) for chain_id, chain_manager in self.chain_managers: if chain_id in chain_ids: enable_list.append(chain_id) chain_manager.enable() else: disable_list.append(chain_id) chain_manager.disable() rospy.logdebug("[%s] Set up chain managers" % rospy.get_name()) for laser_id, laser_manager in self.laser_managers: if laser_id in laser_ids: enable_list.append(laser_id) laser_manager.enable() else: disable_list.append(laser_id) laser_manager.disable() rospy.logdebug("[%s] Set up laser managers" % rospy.get_name()) if self._server.is_preempt_requested(): rospy.loginfo("[%s] preempt detected during accept" % rospy.get_name()) self._server.set_preempted() def preempt_callback(self): rospy.loginfo("[%s] preemting..." % rospy.get_name()) self._server.set_preempted() self.goal_sample_id = None self.goal_target_id = None self.goal_chain_id = None #disable all the things for cam_id, cam_manager in self.cam_managers: cam_manager.disable() rospy.loginfo("[%s] Disabled camera managers" % rospy.get_name()) for chain_id, chain_manager in self.chain_managers: chain_manager.disable() rospy.loginfo("[%s] Disabled chain managers" % rospy.get_name()) for laser_id, laser_manager in self.laser_managers: laser_manager.disable() rospy.loginfo("[%s] Disabled laser managers" % rospy.get_name()) def request_callback(self, msg): rospy.logdebug("[%s] in request_callback" % rospy.get_name()) # See if the interval is big enough to care # TODO: make this a property if (self._server.is_active() and self.goal_sample_id): rospy.logdebug("[%s] server active" % rospy.get_name()) if (msg.end - msg.start) < rospy.Duration(1, 0): self.interval_status.progress = "interval not big enough" self._server.publish_feedback(self.interval_status) return m = self.cache.request_robot_measurement(msg.start, msg.end) if isinstance(m, basestring): self.interval_status.progress = m else: m.sample_id = self.goal_sample_id m.target_id = self.goal_target_id m.chain_id = self.goal_chain_id msg = CaptureManagerResult() msg.result = m # Set succeeded self._server.set_succeeded(msg) # Publish RobotMeasurement self.result.publish(m) # Reset ids self.goal_sample_id = None self.goal_target_id = None self.goal_chain_id = None self.interval_status.progress = "succeeded" self._server.publish_feedback(self.interval_status) def status_callback(self, msg): if (self._server.is_active() and self.goal_sample_id): self.interval_status.status = msg self._server.publish_feedback(self.interval_status) def add_cam_measurement(self, cam_id, msg): if (self._server.is_active() and self.goal_sample_id): self.cache.add_cam_measurement(cam_id, msg) def add_chain_measurement(self, chain_id, msg): if (self._server.is_active() and self.goal_sample_id): self.cache.add_chain_measurement(chain_id, msg) def add_laser_measurement(self, laser_id, msg, interval_start, interval_end): if (self._server.is_active() and self.goal_sample_id): self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end)