$search
00001 #! /usr/bin/env python 00002 00003 from __future__ import with_statement 00004 00005 PKG='pr2_camera_synchronizer' 00006 import roslib; roslib.load_manifest(PKG) 00007 import rospy 00008 from ethercat_trigger_controllers.msg import MultiWaveform 00009 import dynamic_reconfigure.client 00010 import threading 00011 import sys 00012 import copy 00013 import time 00014 import subprocess 00015 import atexit 00016 00017 # TODO: 00018 # - Check that exposure duration is correct for projectorless modes. 00019 # - Check modes that aren't synchronized with the projector. 00020 00021 slight_shift = 1e-6 #ms 00022 00023 class TriggerBase: 00024 def sorted_pts(self): 00025 #print >> sys.stderr, self.name 00026 #print >> sys.stderr, self.pts 00027 pts = [ (x % self.period, y) for (x, y) in self.pts ] 00028 firstpt = pts.index(min(pts, key = lambda (x,y):x)) 00029 pts = pts[firstpt:] + pts[:firstpt] 00030 #print >> sys.stderr, pts 00031 #print >> sys.stderr 00032 return pts 00033 00034 def intervals(self): 00035 if not self.pts: 00036 return [] 00037 if self.pts[-1][1]: 00038 raise Exception("Last point in sequence has non-zero y coord in %s."%self.name) 00039 intervals = [] 00040 idx = 0 00041 while idx < len(self.pts): 00042 if self.pts[idx][1]: 00043 startidx = idx 00044 max_value = 0 00045 while self.pts[idx][1]: 00046 max_value = max(max_value, self.pts[idx][1]) 00047 idx += 1 00048 intervals.append((self.pts[startidx][0], self.pts[idx][0], max_value)) 00049 idx += 1 00050 return intervals 00051 00052 def replicate_interval(intervals, period, reps): 00053 out = list(intervals) 00054 for i in range(1, reps): 00055 offset = i * period 00056 out += [(s+offset,e+offset,v) for (s,e,v) in intervals] 00057 return out 00058 00059 def interval_lengths(intervals): 00060 return [e-s for s,e,v in intervals] 00061 00062 def interval_intersection(a, b, period): 00063 a0, a1, _ = a 00064 b0, b1, _ = b 00065 00066 a0 += period 00067 a1 += period 00068 b0 += period 00069 b1 += period 00070 00071 # Assume that all these values are now positive. 00072 assert(a0 > 0) 00073 assert(a1 > 0) 00074 assert(b0 > 0) 00075 assert(b1 > 0) 00076 00077 # Get all the intervals to start in the first period. 00078 a0w = a0 % period 00079 a1w = a1 + a0w - a0 00080 b0w = b0 % period 00081 b1w = b1 + b0w - b0 00082 00083 # Get the second interval to start before the first, but not more than 00084 # one period before. 00085 if b0w > a0w: 00086 b0w -= period 00087 b1w -= period 00088 00089 # How much intersection? 00090 total = max(0, min(b1w, a1w) - max(b0w, a0w)) 00091 00092 # How much intersection when the second interval gets shifted forward 00093 # one period? 00094 00095 b0w += period 00096 b1w += period 00097 total += max(0, min(b1w, a1w) - max(b0w, a0w) ) 00098 00099 #print >> sys.stderr, "Interval intersection", a, b, period, total 00100 return total 00101 00102 00103 00104 00105 00106 class Trigger(TriggerBase): 00107 def __init__(self, parent, trigger_name): 00108 self.parent = parent 00109 parent.add(self) 00110 self.name = trigger_name 00111 self.ready = False 00112 rospy.Subscriber(trigger_name + "/waveform", MultiWaveform, self.callback) 00113 self.last_msg = None 00114 00115 def compute(self): 00116 if not self.ready: 00117 return False 00118 return True 00119 00120 def callback(self, msg): 00121 #print >> sys.stderr, "Trigger callback", self.name 00122 if self.last_msg == msg: 00123 return 00124 self.last_msg = msg 00125 self.period = msg.period 00126 self.ready = True 00127 offset = msg.zero_offset 00128 self.pts = [ ((trans.time + offset) % self.period, trans.value % 2) for trans in msg.transitions ] 00129 self.parent.update() 00130 00131 00132 00133 00134 00135 00136 class Camera(TriggerBase): 00137 def __init__(self, parent, node_name, trigger): 00138 self.name = node_name 00139 self.ready = False 00140 self.parent = parent 00141 parent.add_camera(self) 00142 self.trigger = trigger 00143 self.trigger_delay = 0.00008 00144 # Approximate, but good enough given that everything else is on the 1kHz ethercat clock. 00145 self.client = dynamic_reconfigure.client.Client(node_name, 00146 config_callback = self.callback) 00147 00148 def check(self, projector): 00149 try: 00150 projector_intervals = projector.intervals() 00151 camera_intervals = self.intervals() 00152 00153 #print >> sys.stderr 00154 #print >> sys.stderr, self.name 00155 #print >> sys.stderr, projector_intervals 00156 #print >> sys.stderr, camera_intervals 00157 00158 reps = self.period / projector.period 00159 if reps % 1: 00160 raise Exception("Camera %s period is %f, projector period is %f, ratio %f"% 00161 (self.name,self.period,projector.period,reps)) 00162 reps = int(reps) 00163 00164 projector_intervals = replicate_interval(projector_intervals, projector.period, reps) 00165 projector_pulse_len = max(interval_lengths(projector_intervals)+[0]) 00166 00167 for s, e, v in camera_intervals: 00168 info = "camera %s exposing from %f to %f"%(self.name,s,e) 00169 if v == 0.75: 00170 alt = True 00171 interval = s+0.0004, e-0.0004, v # Conservative check that we don't lose texture light. 00172 elif v == 1: 00173 alt = False 00174 interval = s-0.0005, e+0.0005, v # Conservative check that we don't hit texture light. 00175 else: 00176 raise Exception("Can't determine register set, %s.", info) 00177 00178 red_light_times = [ interval_intersection(interval, pi, self.period) for pi in projector_intervals ] 00179 red_light_times.sort() 00180 red_light_time = sum(red_light_times) 00181 if red_light_time and red_light_time != red_light_times[-1]: 00182 #print >> sys.stderr, red_light_times 00183 #print >> sys.stderr, projector_intervals 00184 raise Exception("Intersection with multiple pulses, %s."%info) 00185 00186 if red_light_time and abs(red_light_time - projector_pulse_len) > slight_shift: 00187 #print >> sys.stderr, projector_intervals 00188 #print >> sys.stderr, projector.pts 00189 raise Exception("Partial intersection with pulse (alt=%s), %s, %f s of %f, delta %e."%(alt, info, red_light_time, projector_pulse_len, red_light_time-projector_pulse_len)) 00190 if alt and not red_light_time: 00191 raise Exception("Alternate imager but no projector, %s."%info) 00192 if not alt and red_light_time: 00193 raise Exception("Primary imager and has projector, %s."%info) 00194 with_proj_exp = projector_pulse_len + 0.0015 + 2 * slight_shift 00195 if alt and e - s > with_proj_exp: 00196 raise Exception("Too long exposure for textured image %f instead of %f, %s."%(e-s,with_proj_exp,info)) 00197 except Exception, e: 00198 #import traceback 00199 #traceback.print_exc() 00200 self.parent.set_error(repr(e)) 00201 00202 def compute(self): 00203 if not self.ready or not self.trigger.ready: 00204 #print >> sys.stderr, "Not ready", self.name 00205 return False 00206 if not self.config['ext_trig']: 00207 #print >> sys.stderr, "Not ext trig", self.name 00208 return False 00209 self.period = self.trigger.period 00210 self.pts = [] 00211 rising = self.config['rising_edge_trig'] 00212 before = 0 if rising else 1 00213 after = 1 - before 00214 pretrigpts = [] 00215 imager_period = 1.0 / self.config['imager_rate'] 00216 00217 #print >> sys.stderr, "\n", self.name 00218 00219 # Figure out the trigger times and the corresponding register set. 00220 tpts = self.trigger.sorted_pts() 00221 for i in range(len(tpts)): 00222 if tpts[i-1][1] == before and tpts[i][1] == after: 00223 pretrigpts.append(tpts[i][0] + self.trigger_delay) 00224 #print >> sys.stderr, "preselected", tpts[i] 00225 trigpts = [] 00226 00227 # Run through the triggers once to find the last trigger time in 00228 # the sequence. 00229 last_trig = -imager_period 00230 trig_count = 0 00231 last_count = 0 00232 for i in range(len(pretrigpts)): 00233 trig_count += 1 00234 if pretrigpts[i] - last_trig > imager_period: 00235 last_trig = pretrigpts[i] 00236 last_count = trig_count 00237 00238 # Run through the triggers again keeping 00239 first_trig = last_trig 00240 last_trig = first_trig - self.period 00241 for i in range(len(pretrigpts)): 00242 trig_count += 1 00243 if pretrigpts[i] - last_trig > imager_period: 00244 last_trig = pretrigpts[i] 00245 reg_set = self.config['register_set'] 00246 if reg_set == 2: 00247 reg_set = 1 if (last_count - trig_count) % 2 else 0 00248 trigpts.append((pretrigpts[i], reg_set)) 00249 last_count = trig_count 00250 else: 00251 #print >> sys.stderr, "dropped", pretrigpts[i] 00252 pass 00253 if last_trig != first_trig: 00254 self.parent.set_error("Non-consistent triggering times for %s."%self.name) 00255 00256 # Figure out exposure times for each register set. 00257 cfg_suffix = [ "", "_alternate" ] 00258 exposure_min = [] 00259 exposure_max = [] 00260 max_exp = self.config['max_exposure'] 00261 for i in range(2): 00262 auto_exp = self.config['auto_exposure'+cfg_suffix[i]] 00263 set_exp = self.config['exposure'+cfg_suffix[i]] 00264 cur_min = 0 00265 cur_max = max_exp 00266 if not auto_exp: 00267 cur_min = min(max_exp, set_exp) 00268 cur_max = cur_min 00269 exposure_min.append(cur_min) 00270 exposure_max.append(cur_max) 00271 00272 # Generate the exposure points. 00273 for (t, rs) in trigpts: 00274 alternate_offset = - rs * 0.25 00275 exp_end = t + imager_period 00276 exp_start_early = exp_end - exposure_max[rs] 00277 exp_start_late = exp_end - exposure_min[rs] 00278 if exp_start_early != exp_start_late: 00279 self.pts.append((exp_start_early - slight_shift, 0.5 + alternate_offset)) 00280 self.pts.append((exp_start_late, 1 + alternate_offset)) 00281 self.pts.append((exp_end + slight_shift, 0)) 00282 return True 00283 00284 def callback(self, config): 00285 #print >> sys.stderr, "Camera callback", self.name 00286 if self.ready and self.config == config: 00287 return 00288 self.config = copy.deepcopy(config) 00289 self.ready = True 00290 self.parent.update() 00291 00292 00293 00294 00295 00296 00297 class TriggerChecker: 00298 def __init__(self, plot, silent = False): 00299 self.do_plot = plot 00300 self.triggers = [] 00301 self.mutex = threading.Lock() 00302 self.error = None 00303 self.updated = False 00304 self.cameras = [] 00305 self.projector = None 00306 self.silent = silent 00307 if self.do_plot: 00308 try: 00309 self.gnuplot = subprocess.Popen('gnuplot', stdin = subprocess.PIPE) 00310 except: 00311 print "Gnuplot must be installed to allow plotting of waveforms." 00312 sys.exit(1) 00313 atexit.register(self.gnuplot.kill) 00314 00315 def add(self, trigger): 00316 self.triggers.append(trigger) 00317 00318 def add_camera(self, cam): 00319 self.cameras.append(cam) 00320 self.add(cam) 00321 00322 def set_projector(self, projector): 00323 self.projector = projector 00324 00325 def clear(self): 00326 with self.mutex: 00327 for trig in self.triggers: 00328 trig.ready = False 00329 self.updated = False 00330 00331 def update(self): 00332 with self.mutex: 00333 self.error = None 00334 n = len(self.triggers) 00335 for i in range(n): 00336 if not self.triggers[i].compute(): 00337 if not self.silent: 00338 self.set_error('No data for %s. (This is normal at startup.)'%self.triggers[i].name) 00339 return 00340 00341 if self.do_plot: 00342 if self.error: 00343 self.empty_plot() 00344 else: 00345 self.plot() 00346 if self.projector: 00347 for c in self.cameras: 00348 c.check(self.projector) 00349 if not self.silent: 00350 if not self.error: 00351 rospy.loginfo("All checks passed.") 00352 else: 00353 rospy.logerr("Some checks failed.") 00354 self.updated = True 00355 00356 def set_error(self, msg): 00357 if not self.silent: 00358 rospy.logerr(msg) 00359 self.error = msg 00360 00361 def plot(self): 00362 n = len(self.triggers) 00363 period = max(t.period for t in self.triggers) 00364 for i in range(n): 00365 if (period / self.triggers[i].period) % 1: 00366 self.set_error('Period for %s is %f, expected divisor of %f'%(self.triggers[i].name, 00367 self.triggers[i].period, period)) 00368 return 00369 style = 'with linespoints title "%s"' 00370 print >> self.gnuplot.stdin, 'plot "-"', style%self.triggers[0].name, 00371 for i in range(1, n): 00372 print >> self.gnuplot.stdin, ', "-"', style%self.triggers[i].name, 00373 print >> self.gnuplot.stdin 00374 for i in range(n): 00375 t = self.triggers[i] 00376 reps = int(period / t.period) 00377 pts = t.sorted_pts() 00378 if len(pts) == 0: 00379 pts = [(0, 0)] 00380 def plot_pt(x, y): 00381 print >> self.gnuplot.stdin, x, (n - i - 1) * 1.1 + y%2 00382 plot_pt(0, pts[-1][1]) 00383 for k in range(reps): 00384 xoffs = t.period * k 00385 for j in range(len(pts)): 00386 plot_pt(pts[j][0] + xoffs, pts[j-1][1]) 00387 plot_pt(pts[j][0] + xoffs, pts[j][1]) 00388 plot_pt(period, pts[-1][1]) 00389 print >> self.gnuplot.stdin, "e" 00390 print >> self.gnuplot.stdin 00391 sys.stdout.flush() 00392 00393 def empty_plot(self): 00394 print 'plot x, -x' 00395 00396 import unittest 00397 class Test(unittest.TestCase): 00398 def setUp(self): 00399 global tp, reconfig_client 00400 tp.silent = True 00401 self.reconfig_client = reconfig_client 00402 tp.clear() 00403 self.reconfig_client.update_configuration({'projector_mode': 1}) 00404 00405 def wait_for_ready(self): 00406 global tp 00407 time.sleep(2) 00408 starttime = time.time() 00409 while not rospy.is_shutdown(): 00410 if time.time() > starttime + 5: 00411 self.fail("Took too long to get responses.") 00412 if tp.updated: 00413 break 00414 time.sleep(0.1) 00415 00416 def evaluate(self): 00417 global tp 00418 self.wait_for_ready() 00419 tp.silent = False 00420 tp.update() 00421 self.assertFalse(tp.error, tp.error) 00422 00423 def test_with(self): 00424 self.reconfig_client.update_configuration({ 00425 'projector_pulse_shift': 0.0, 00426 'wide_stereo_trig_mode': 3, 00427 'prosilica_projector_inhibit': False, 00428 'camera_reset': False, 00429 'forearm_r_rate': 30.0, 00430 'projector_rate': 58.823529411764703, 00431 'stereo_rate': 29.411764705882351, 00432 'projector_pulse_length': 0.002, 00433 'projector_mode': 2, 00434 'forearm_r_trig_mode': 3, 00435 'projector_tweak': 0.0, 00436 'forearm_l_trig_mode': 3, 00437 'forearm_l_rate': 30.0, 00438 'narrow_stereo_trig_mode': 3, 00439 }) 00440 self.evaluate() 00441 00442 def test_without(self): 00443 self.reconfig_client.update_configuration({ 00444 'projector_pulse_shift': 0.0, 00445 'wide_stereo_trig_mode': 4, 00446 'prosilica_projector_inhibit': False, 00447 'camera_reset': False, 00448 'forearm_r_rate': 30.0, 00449 'projector_rate': 58.823529411764703, 00450 'stereo_rate': 29.411764705882351, 00451 'projector_pulse_length': 0.002, 00452 'projector_mode': 3, 00453 'forearm_r_trig_mode': 4, 00454 'projector_tweak': 0.0, 00455 'forearm_l_trig_mode': 4, 00456 'forearm_l_rate': 30.0, 00457 'narrow_stereo_trig_mode': 4, 00458 }) 00459 self.evaluate() 00460 00461 def test_alt(self): 00462 self.reconfig_client.update_configuration({ 00463 'projector_pulse_shift': 0.0, 00464 'wide_stereo_trig_mode': 4, 00465 'prosilica_projector_inhibit': False, 00466 'camera_reset': False, 00467 'forearm_r_rate': 30.0, 00468 'projector_rate': 58.823529411764703, 00469 'stereo_rate': 29.411764705882351, 00470 'projector_pulse_length': 0.002, 00471 'projector_mode': 3, 00472 'forearm_r_trig_mode': 4, 00473 'projector_tweak': 0.0, 00474 'forearm_l_trig_mode': 4, 00475 'forearm_l_rate': 30.0, 00476 'narrow_stereo_trig_mode': 5, 00477 }) 00478 self.evaluate() 00479 00480 def test_slow(self): 00481 self.reconfig_client.update_configuration({ 00482 'projector_pulse_shift': 0.0, 00483 'wide_stereo_trig_mode': 4, 00484 'prosilica_projector_inhibit': False, 00485 'camera_reset': False, 00486 'forearm_r_rate': 5.0, 00487 'projector_rate': 58.823529411764703, 00488 'stereo_rate': 5.0, 00489 'projector_pulse_length': 0.002, 00490 'projector_mode': 3, 00491 'forearm_r_trig_mode': 4, 00492 'projector_tweak': 0.0, 00493 'forearm_l_trig_mode': 3, 00494 'forearm_l_rate': 5.0, 00495 'narrow_stereo_trig_mode': 5, 00496 }) 00497 self.evaluate() 00498 00499 def main(): 00500 global tp, reconfig_client 00501 regression_test = rospy.get_param('~regression_test', False) 00502 tp = TriggerChecker(plot = not regression_test, silent = regression_test) 00503 head_trig = Trigger(tp, '/head_camera_trigger') 00504 l_forearm_trig = Trigger(tp, 'l_forearm_cam_trigger') 00505 r_forearm_trig = Trigger(tp, 'r_forearm_cam_trigger') 00506 Camera(tp, '/narrow_stereo_both', head_trig) 00507 tp.set_projector(Trigger(tp, '/projector_trigger')) 00508 Camera(tp, '/wide_stereo_both', head_trig) 00509 Camera(tp, '/l_forearm_cam', l_forearm_trig) 00510 Camera(tp, '/r_forearm_cam', r_forearm_trig) 00511 if regression_test: 00512 import rostest 00513 rospy.loginfo("Running in unit test mode") 00514 reconfig_client = dynamic_reconfigure.client.Client('synchronizer') 00515 rostest.rosrun(PKG, 'test_bare_bones', Test) 00516 else: 00517 rospy.loginfo("Running in plotting mode") 00518 while not rospy.is_shutdown(): 00519 time.sleep(0.1) 00520 00521 if __name__ == "__main__": 00522 rospy.init_node('trigger_plotter', anonymous = True) 00523 main()