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)
