Example #1
0
    def __init__(self, cam_ids):
        self.cam_ids = cam_ids
        self.cache = RobotMeasurementCache()
        self.lock = threading.Lock()

        # Specifies if we're currently waiting for a sample
        self.active = False

        # Construct a manager for each sensor stream (Don't enable any of them)
        cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
        #print "cam_ids"
        self.cam_managers = [(cam_id,
                              CamManager(cam_id, cam_info_topic,
                                         self.add_cam_measurement))
                             for cam_id in cam_ids]

        # Turn on all of the camera modes into verbose model, since we want the CalibrationPattern data
        for cam_manager in zip(*self.cam_managers)[1]:
            cam_manager.enable(verbose=True)

        # Subscribe to topic containing stable intervals
        self.measurement_pub = rospy.Publisher("robot_measurement",
                                               RobotMeasurement)
        self.request_interval_sub = rospy.Subscriber("request_interval",
                                                     Interval,
                                                     self.request_callback)

        # Set up the cache with only the sensors we care about
        chain_ids = []
        laser_ids = []
        self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
class CameraCaptureExecutive:
    def __init__(self, cam_ids):
        self.cam_ids = cam_ids
        self.cache = RobotMeasurementCache()
        self.lock = threading.Lock()


        # Specifies if we're currently waiting for a sample
        self.active = False

        # Construct a manager for each sensor stream (Don't enable any of them)
        cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
	#print "cam_ids"
        self.cam_managers   = [ (cam_id,   CamManager(  cam_id,  cam_info_topic, 
                                                        self.add_cam_measurement) )   for cam_id   in cam_ids ]

        # Turn on all of the camera modes into verbose model, since we want the CalibrationPattern data
        for cam_manager in zip(*self.cam_managers)[1]:
            cam_manager.enable(verbose=True)

        # Subscribe to topic containing stable intervals
        self.measurement_pub = rospy.Publisher("robot_measurement", RobotMeasurement)
        self.request_interval_sub = rospy.Subscriber("request_interval", Interval, self.request_callback)

        # Set up the cache with only the sensors we care about
        chain_ids = []
        laser_ids = []
        self.cache.reconfigure(cam_ids, chain_ids, laser_ids)

    def request_callback(self, msg):

        print "\n\n\n\n\033[34;1m------\n\033[0mGot a request callback"

        # See if the interval is big enough to care
        #if (msg.end - msg.start) < rospy.Duration(1,0):
        #    return

        with self.lock:
            # Do some query into the cache
            m_robot = self.cache.request_robot_measurement(msg.start, msg.end, min_duration=rospy.Duration(0.01))

            # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care)
            if m_robot:
                # Change camera ids to be the tf frame IDs
                for cam in m_robot.M_cam:
                    cam.camera_id = cam.cam_info.header.frame_id
                    m_robot.header.stamp = cam.cam_info.header.stamp
                self.measurement_pub.publish(m_robot)
            else:
                print "Couldn't get measurement in interval"


    def add_cam_measurement(self, cam_id, msg):
        #print "Adding [%s]" 
        if len(msg.features.image_points) > 0:
            with self.lock:
                self.cache.add_cam_measurement(cam_id, msg)
    def __init__(self, cam_ids):
        self.cam_ids = cam_ids
        self.cache = RobotMeasurementCache()
        self.lock = threading.Lock()


        # Specifies if we're currently waiting for a sample
        self.active = False

        # Construct a manager for each sensor stream (Don't enable any of them)
        cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
	#print "cam_ids"
        self.cam_managers   = [ (cam_id,   CamManager(  cam_id,  cam_info_topic, 
                                                        self.add_cam_measurement) )   for cam_id   in cam_ids ]

        # Turn on all of the camera modes into verbose model, since we want the CalibrationPattern data
        for cam_manager in zip(*self.cam_managers)[1]:
            cam_manager.enable(verbose=True)

        # Subscribe to topic containing stable intervals
        self.measurement_pub = rospy.Publisher("robot_measurement", RobotMeasurement)
        self.request_interval_sub = rospy.Subscriber("request_interval", Interval, self.request_callback)

        # Set up the cache with only the sensors we care about
        chain_ids = []
        laser_ids = []
        self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
Example #4
0
class CameraCaptureExecutive:
    def __init__(self, cam_ids):
        self.cam_ids = cam_ids
        self.cache = RobotMeasurementCache()
        self.lock = threading.Lock()

        # Specifies if we're currently waiting for a sample
        self.active = False

        # Construct a manager for each sensor stream (Don't enable any of them)
        cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
        #print "cam_ids"
        self.cam_managers = [(cam_id,
                              CamManager(cam_id, cam_info_topic,
                                         self.add_cam_measurement))
                             for cam_id in cam_ids]

        # Turn on all of the camera modes into verbose model, since we want the CalibrationPattern data
        for cam_manager in zip(*self.cam_managers)[1]:
            cam_manager.enable(verbose=True)

        # Subscribe to topic containing stable intervals
        self.measurement_pub = rospy.Publisher("robot_measurement",
                                               RobotMeasurement)
        self.request_interval_sub = rospy.Subscriber("request_interval",
                                                     Interval,
                                                     self.request_callback)

        # Set up the cache with only the sensors we care about
        chain_ids = []
        laser_ids = []
        self.cache.reconfigure(cam_ids, chain_ids, laser_ids)

    def request_callback(self, msg):

        print "\n\n\n\n\033[34;1m------\n\033[0mGot a request callback"

        # See if the interval is big enough to care
        #if (msg.end - msg.start) < rospy.Duration(1,0):
        #    return

        with self.lock:
            # Do some query into the cache
            m_robot = self.cache.request_robot_measurement(
                msg.start, msg.end, min_duration=rospy.Duration(0.01))

            # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care)
            if m_robot:
                # Change camera ids to be the tf frame IDs
                for cam in m_robot.M_cam:
                    cam.camera_id = cam.cam_info.header.frame_id
                    m_robot.header.stamp = cam.cam_info.header.stamp
                self.measurement_pub.publish(m_robot)
            else:
                print "Couldn't get measurement in interval"

    def add_cam_measurement(self, cam_id, msg):
        #print "Adding [%s]"
        if len(msg.features.image_points) > 0:
            with self.lock:
                self.cache.add_cam_measurement(cam_id, msg)