Пример #1
0
    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)
Пример #2
0
    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)
Пример #3
0
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()
Пример #4
0
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()