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('pr2_calibration_executive')
00035 import sys
00036
00037 import rospy
00038 from camera_pose_calibration.msg import RobotMeasurement
00039 from bisect import bisect
00040
00041 def stamped_cmp(x,y):
00042 return cmp(x.header.stamp, y.header.stamp)
00043
00044 def laser_cmp(x,y):
00045 return cmp(x[0].header.stamp, y[0].header.stamp)
00046
00047
00048
00049 class RobotMeasurementCache:
00050 def __init__(self):
00051 self._cam_sizes = dict()
00052 self._laser_sizes = dict()
00053 self._chain_sizes = dict()
00054 pass
00055
00056
00057 def reconfigure(self, cam_ids, chain_ids, laser_ids):
00058 self._cam_caches = dict( [ (x,[]) for x in cam_ids ] )
00059 self._laser_caches = dict( [ (x,[]) for x in laser_ids] )
00060 self._chain_caches = dict( [ (x,[]) for x in chain_ids] )
00061
00062
00063
00064
00065
00066
00067
00068 for cam_id in [x for x in cam_ids if x not in self._cam_sizes]:
00069
00070 self._cam_sizes[cam_id] = 100
00071 for chain_id in [x for x in chain_ids if x not in self._chain_sizes]:
00072
00073 self._chain_sizes[chain_id] = 3000
00074 for laser_id in [x for x in laser_ids if x not in self._laser_sizes]:
00075
00076 self._laser_sizes[laser_id] = 5
00077
00078
00079 def set_max_sizes(self, cam_sizes, chain_sizes, laser_sizes):
00080 self._cam_sizes = cam_sizes
00081 self._laser_sizes = laser_sizes
00082 self._chain_sizes = chain_sizes
00083
00084 def add_cam_measurement(self, cam_id, m):
00085 cur_cache = self._cam_caches[cam_id]
00086 cur_cache.append( m )
00087 cur_cache.sort(stamped_cmp)
00088 while len(cur_cache) > self._cam_sizes[cam_id]:
00089 cur_cache.pop(0)
00090
00091 def add_chain_measurement(self, chain_id, m):
00092 cur_cache = self._chain_caches[chain_id]
00093 cur_cache.append( m )
00094 cur_cache.sort(stamped_cmp)
00095
00096
00097
00098
00099
00100
00101 while len(cur_cache) > self._chain_sizes[chain_id]:
00102 cur_cache.pop(0)
00103
00104 def add_laser_measurement(self, laser_id, m, interval_start, interval_end):
00105 print self._laser_caches.keys()
00106
00107 cur_cache = self._laser_caches[laser_id]
00108 cur_cache.append( [m, interval_start, interval_end] )
00109 cur_cache.sort(laser_cmp)
00110
00111 while len(cur_cache) > self._laser_sizes[laser_id]:
00112 cur_cache.pop(0)
00113
00114 def request_robot_measurement(self, interval_start, interval_end, min_duration = rospy.Duration(2,0)):
00115
00116
00117 req_duration = interval_end - interval_start
00118 req_center = interval_start + rospy.Duration(req_duration.to_sec()*.5)
00119
00120 if req_duration < min_duration:
00121
00122 return None
00123
00124
00125 cam_measurements = dict( [ (x, None) for x in self._cam_caches.keys() ] )
00126 for cam_id in self._cam_caches.keys():
00127
00128
00129 cur_cache = self._cam_caches[cam_id]
00130 right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00131
00132
00133
00134
00135
00136
00137
00138
00139 if right_center_index >= len(cur_cache):
00140 cam_measurements[cam_id] = None
00141 elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00142 cam_measurements[cam_id] = None
00143 else:
00144 cam_measurements[cam_id] = cur_cache[right_center_index]
00145
00146
00147 chain_measurements = dict( [ (x, None) for x in self._chain_caches.keys() ] )
00148 for chain_id in self._chain_caches.keys():
00149
00150
00151 cur_cache = self._chain_caches[chain_id]
00152 right_center_index = bisect( [x.header.stamp for x in cur_cache], req_center )
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 if right_center_index >= len(cur_cache):
00163 chain_measurements[chain_id] = None
00164 elif cur_cache[right_center_index].header.stamp > interval_end or cur_cache[right_center_index].header.stamp < interval_start:
00165 chain_measurements[chain_id] = None
00166 else:
00167 chain_measurements[chain_id] = cur_cache[right_center_index]
00168
00169
00170 laser_measurements = dict( [ (x, None) for x in self._laser_caches.keys() ] )
00171 for laser_id in self._laser_caches.keys():
00172 cur_cache = self._laser_caches[laser_id]
00173 sub_list = [x for x in cur_cache if x[2] <= interval_end and x[1] >= interval_start]
00174 if len(sub_list) > 0:
00175 laser_measurements[laser_id] = sub_list[0][0]
00176 else:
00177 laser_measurements[laser_id] = None
00178
00179
00180 for cam_id, m in cam_measurements.items():
00181 if m is None:
00182
00183 return None
00184
00185 for chain_id, m in chain_measurements.items():
00186 if m is None:
00187
00188 return None
00189
00190 for laser_id, m in laser_measurements.items():
00191 if m is None:
00192
00193 return None
00194
00195 print "Received everything!"
00196
00197
00198 m_robot = RobotMeasurement()
00199 m_robot.M_cam = cam_measurements.values()
00200
00201 return m_robot
00202