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