Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib; roslib.load_manifest('camera_pose_calibration')
00035 import rospy
00036 import yaml
00037 import sys
00038 import actionlib
00039 import time
00040 import threading
00041 import message_filters
00042
00043 from camera_pose_calibration.robot_measurement_cache import RobotMeasurementCache
00044
00045 from calibration_msgs.msg import Interval, CalibrationPattern
00046 from camera_pose_calibration.msg import RobotMeasurement, CameraMeasurement
00047 from sensor_msgs.msg import CameraInfo, Image
00048
00049
00050 class CameraCaptureExecutive:
00051 def __init__(self, cam_ids):
00052 self.cam_ids = cam_ids
00053 self.cache = RobotMeasurementCache()
00054 self.lock = threading.Lock()
00055
00056
00057
00058 self.active = False
00059
00060
00061 cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
00062
00063 self.cam_managers = [ (cam_id, CamManager( cam_id, cam_info_topic,
00064 self.add_cam_measurement) ) for cam_id in cam_ids ]
00065
00066
00067 for cam_manager in zip(*self.cam_managers)[1]:
00068 cam_manager.enable(verbose=True)
00069
00070
00071 self.measurement_pub = rospy.Publisher("robot_measurement", RobotMeasurement)
00072 self.request_interval_sub = rospy.Subscriber("request_interval", Interval, self.request_callback)
00073
00074
00075 chain_ids = []
00076 laser_ids = []
00077 self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
00078
00079 def request_callback(self, msg):
00080
00081 print "\n\n\n\n\033[34;1m------\n\033[0mGot a request callback"
00082
00083
00084
00085
00086
00087 with self.lock:
00088
00089 m_robot = self.cache.request_robot_measurement(msg.start, msg.end, min_duration=rospy.Duration(0.01))
00090
00091
00092 if m_robot:
00093
00094 for cam in m_robot.M_cam:
00095 cam.camera_id = cam.cam_info.header.frame_id
00096 m_robot.header.stamp = cam.cam_info.header.stamp
00097 self.measurement_pub.publish(m_robot)
00098 else:
00099 print "Couldn't get measurement in interval"
00100
00101
00102 def add_cam_measurement(self, cam_id, msg):
00103
00104 if len(msg.features.image_points) > 0:
00105 with self.lock:
00106 self.cache.add_cam_measurement(cam_id, msg)
00107
00108
00109 class CamManager:
00110 def __init__(self, cam_id, cam_info_topic, callback):
00111 self._cam_id = cam_id
00112 self._callback = callback
00113 self._mode = "off"
00114
00115 self._lock = threading.Lock()
00116
00117
00118 self._features_sub = message_filters.Subscriber(cam_id + "/features", CalibrationPattern)
00119 self._cam_info_sub = message_filters.Subscriber(cam_id + "/" + cam_info_topic, CameraInfo)
00120
00121 self._verbose_sync = message_filters.TimeSynchronizer([self._cam_info_sub, self._features_sub], 10)
00122
00123 self._verbose_sync.registerCallback(self.verbose_callback)
00124
00125 def verbose_callback(self, cam_info, features):
00126 with self._lock:
00127 if self._mode is "verbose":
00128
00129 msg = CameraMeasurement()
00130 msg.header.stamp = cam_info.header.stamp
00131 msg.camera_id = self._cam_id
00132 msg.cam_info = cam_info
00133 msg.features = features
00134 self._callback(self._cam_id, msg)
00135
00136 def enable(self, verbose=False):
00137 with self._lock:
00138 self._mode = "verbose"
00139
00140 def disable(self):
00141 with self._lock:
00142 self._mode = "off"
00143
00144
00145 if __name__ == '__main__':
00146 rospy.init_node('capture_exec')
00147 args = rospy.myargv(sys.argv)
00148
00149
00150 if len(args) < 2:
00151 rospy.logfatal("multicam_capture_exec.py needs camera's as arguments")
00152 raise
00153 camera_list = args[1:]
00154 executive = CameraCaptureExecutive(camera_list)
00155 rospy.spin()