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)
Beispiel #3
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)
Beispiel #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).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()
Beispiel #5
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)
Beispiel #6
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()
    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)
Beispiel #10
0
    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()
Beispiel #11
0
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)