plot_multi_trigger.py
Go to the documentation of this file.
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()


pr2_camera_synchronizer
Author(s): Blaise Gassend
autogenerated on Thu Jun 6 2019 20:28:49