synchronizer_classes.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 from __future__ import with_statement
00035 import roslib; roslib.load_manifest('pr2_camera_synchronizer')
00036 from dynamic_reconfigure.client import Client as DynamicReconfigureClient
00037 from dynamic_reconfigure.server import Server as DynamicReconfigureServer
00038 from ethercat_trigger_controllers.srv import SetMultiWaveform as SetMultiWaveform
00039 from ethercat_trigger_controllers.srv import SetMultiWaveformRequest as SetMultiWaveformRequest
00040 from ethercat_trigger_controllers.msg import MultiWaveform as MultiWaveform
00041 from ethercat_trigger_controllers.msg import MultiWaveformTransition as MultiWaveformTransition
00042 import pr2_camera_synchronizer.cfg.CameraSynchronizerConfig as Config
00043 import wge100_camera.cfg.WGE100CameraConfig as WGEConfig
00044 
00045 import rospy
00046 import time
00047 import math
00048 import threading
00049 import signal
00050 
00051 from pr2_camera_synchronizer.cfg import CameraSynchronizerConfig as ConfigType
00052 from pr2_camera_synchronizer.levels import *
00053 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
00054 
00055 ETHERCAT_INTERVAL = 0.001
00056 
00057 param_rate = "rate"
00058 param_trig_mode = "trig_mode"
00059 camera_parameters = [ param_rate, param_trig_mode ]
00060 
00061 param_proj_rate = "projector_rate"
00062 
00063 def roundToEthercat(val):
00064     return ETHERCAT_INTERVAL * round(val / ETHERCAT_INTERVAL)
00065 
00066 asynchronous_updaters = []
00067 def killAsynchronousUpdaters():
00068     for updater in asynchronous_updaters:
00069         updater.kill()
00070 class AsynchronousUpdater(threading.Thread):
00071     def __init__(self, f, name):
00072         threading.Thread.__init__(self)
00073         self.name = name
00074         self.f = f
00075         self.allargs = None
00076         self.cv = threading.Condition()
00077         asynchronous_updaters.append(self)
00078         self.exiting = False
00079         self.idle = True
00080         self.start()
00081 
00082     def update(self, *args, **nargs):
00083         with self.cv:
00084             self.allargs = (args, nargs)
00085             self.cv.notify()
00086             #print "update", self.allargs
00087 
00088     def run(self):
00089         #print "run"
00090         while True:
00091             #print "Starting loop on", self.name
00092             with self.cv:
00093                 if self.exiting:
00094                     break
00095                 if self.allargs == None:
00096                   #print "start wait"
00097                    self.idle = True
00098                    self.cv.wait()
00099                    self.idle_end_time = rospy.get_time()
00100                    self.idle = False
00101                   #print "end wait"
00102                 allargs = self.allargs
00103                 self.allargs = None
00104             if allargs != None:
00105                 try:
00106                     self.f(*allargs[0], **allargs[1])
00107                 except Exception, e:
00108                     rospy.logerr("AsynchronousUpdater failed with exception: %s"%str(e))
00109                     pass
00110 
00111     def kill(self):
00112         #print "kill"
00113         #print "Killing", self.name
00114         with self.cv:
00115             self.exiting = True
00116             self.allargs = None
00117             self.cv.notify()
00118 
00119     def getStatus(self): # For diagnostics
00120         if self.idle:
00121             return self.name, 0
00122         interval = rospy.get_time() - self.idle_end_time
00123         return self.name, interval
00124 
00125 class MultiTriggerController:
00126   def __init__(self, name):
00127     self.period = 0
00128     self.zero_offset = 0
00129     self.clear_waveform()
00130     self.name = name
00131     self.async = AsynchronousUpdater(self.async_update, "Controller "+name)
00132     self.service = None
00133     self.transitions = []
00134 
00135   def clear_waveform(self):
00136     self.transitions = []
00137                                                     
00138   def add_sample(self, time, value, topic):
00139     time = roundToEthercat(time) + 0.5 * ETHERCAT_INTERVAL
00140     self.transitions.append(MultiWaveformTransition(time, value, topic))
00141   
00142   def async_update(self, period, zero_offset, transitions):
00143       try:
00144           if self.service == None:
00145               service_name = self.name+"/set_waveform"
00146               rospy.wait_for_service(service_name)
00147               self.service = rospy.ServiceProxy(service_name, SetMultiWaveform)
00148               #print "Service", service_name, "exists."
00149 
00150           #print "Trigger async_update got proxy on", self.name
00151           waveform = MultiWaveform(period, zero_offset, transitions)
00152           #print "Updating waveform ", self.name, waveform
00153           rslt = self.service(waveform)
00154           if not rslt.success:
00155               rospy.logerr("Error setting waveform %s: %s"%(self.name, rslt.status_message))
00156           #print "Done updating waveform ", self.name
00157       except KeyboardInterrupt: # Handle CTRL+C
00158           print "Aborted trigger update on", self.name
00159 
00160   def update(self):
00161       # Run the update using an Asynchronous Updater so that if something
00162       # locks up, the rest of the node can keep working.
00163       #print "Trigger update on", self.name
00164       self.async.update(self.period, self.zero_offset, self.transitions)
00165 
00166 class ProsilicaInhibitTriggerController(MultiTriggerController):
00167     def __init__(self, name, param, true_val, false_val):
00168         MultiTriggerController.__init__(self, name)
00169         self.param = param
00170         self.true_val = true_val
00171         self.false_val = false_val
00172 
00173     def process_update(self, config, level):
00174         self.period = 1
00175         self.zero_offset = 0
00176         self.clear_waveform()
00177         self.add_sample(0, { True: self.true_val, False: self.false_val}[config[self.param]], '-')
00178         MultiTriggerController.update(self)
00179 
00180 class ProjectorTriggerController(MultiTriggerController):
00181     def __init__(self, name, proj):
00182         MultiTriggerController.__init__(self, name)
00183         self.proj = proj
00184 
00185     def update(self):
00186         self.period = self.proj.repeat_period
00187         self.zero_offset = self.proj.zero_offset
00188         self.clear_waveform()
00189         self.high_val = 0xf
00190         if (self.proj.needed == False and self.proj.mode == Config.CameraSynchronizer_ProjectorAuto) or \
00191                 self.proj.mode == Config.CameraSynchronizer_ProjectorOff:
00192             self.high_val = 0xe
00193         for i in range(0, len(self.proj.pulse_starts)):
00194           self.add_sample(self.proj.pulse_starts[i], self.high_val, 'on_time')
00195           self.add_sample(self.proj.pulse_ends[i], 0xe, 'off_time')
00196         MultiTriggerController.update(self)
00197 
00198 class SingleCameraTriggerController(MultiTriggerController):
00199   def __init__(self, name, camera):
00200     MultiTriggerController.__init__(self, name)
00201     self.camera = camera
00202 
00203   def update(self):
00204     if self.camera.reset_cameras:
00205       self.camera_reset()
00206       return
00207     self.clear_waveform()
00208     if not self.camera.ext_trig:
00209       self.period = 1
00210       self.add_sample(0, 0, "-")
00211     else:
00212       self.period = 2 * self.camera.period
00213       self.zero_offset = -self.camera.imager_period 
00214       first_pulse_start = self.camera.end_offset
00215       second_pulse_start = self.camera.period + first_pulse_start
00216       first_frame_end = first_pulse_start + self.camera.imager_period
00217       extra_pulse_start = (first_pulse_start + first_frame_end) / 2  
00218       trigger_name = "trigger"
00219       self.add_sample(first_pulse_start, 1, trigger_name)
00220       self.add_sample((first_pulse_start + extra_pulse_start) / 2, 0, "-")
00221       self.add_sample(extra_pulse_start, 1, "-")
00222       self.add_sample((extra_pulse_start + first_frame_end) / 2, 0, "-")
00223       self.add_sample(second_pulse_start, 1, trigger_name)
00224       self.add_sample((second_pulse_start + self.period) / 2, 0, "-")
00225       self.camera.trig_rising = True
00226       self.camera.trigger_name = self.name+"/"+trigger_name
00227     
00228     #print "About to update trigger", self.name
00229     MultiTriggerController.update(self)
00230 
00231   def camera_reset(self):
00232     self.clear_waveform()
00233     self.period = 1.5
00234     self.add_sample(0, 0, "-")
00235     self.add_sample(0.1, 1, "-")
00236     MultiTriggerController.update(self)
00237 
00238 class DualCameraTriggerController(SingleCameraTriggerController):
00239   def __init__(self, name, camera1, camera2):
00240     SingleCameraTriggerController.__init__(self, name, None)
00241     self.cameras = [camera1, camera2]
00242 
00243   def update(self):
00244     if self.cameras[0].reset_cameras or self.cameras[1].reset_cameras:
00245       self.camera_reset()
00246       return
00247     
00248     if self.cameras[0].period != self.cameras[1].period or \
00249        self.cameras[0].imager_period != self.cameras[1].imager_period:
00250        rospy.logerr("Cameras on same trigger have different period/imager_period settings.")
00251     
00252     self.clear_waveform()
00253     if self.cameras[0].end_offset == self.cameras[1].end_offset:
00254         # This works because all cameras have the same imager period.
00255         self.camera = self.cameras[0]
00256         SingleCameraTriggerController.update(self)
00257         self.cameras[1].trigger_name = self.cameras[0].trigger_name
00258         self.cameras[1].trig_rising = self.cameras[0].trig_rising
00259         return
00260     
00261     if self.cameras[0].end_offset > self.cameras[1].end_offset:
00262         # Reduction to the case where cameras[0] happens first.
00263         self.cameras.reverse()
00264 
00265     # Now we are sure that camera 1 happens strictly before camera 2.
00266     assert self.cameras[0].proj == self.cameras[1].proj
00267     assert self.cameras[0].period == self.cameras[1].period # Will fail if both stereo can be alternate or if they are updated in the wrong order.
00268 
00269     self.cameras[0].trig_rising = True
00270     self.cameras[1].trig_rising = False
00271 
00272     self.clear_waveform()
00273     self.period = 2 * self.cameras[1].period
00274     self.zero_offset = -self.cameras[0].imager_period 
00275 
00276     first_rise = self.cameras[0].end_offset
00277     first_fall = self.cameras[1].end_offset
00278     first_rise_end = first_rise + self.cameras[0].imager_period
00279     second_rise = self.cameras[0].end_offset + self.cameras[0].period
00280     second_fall = self.cameras[1].end_offset + self.cameras[1].period
00281     extra_rise = (2 * first_fall + first_rise_end) / 3                            
00282     extra_fall = (first_fall + 2 * first_rise_end) / 3
00283                                               
00284     trigger_name = [ "trigger_"+camera.name for camera in self.cameras ]
00285     self.add_sample(first_rise,  1, trigger_name[0])
00286     self.add_sample(first_fall,  0, trigger_name[1])
00287     self.add_sample(extra_rise,  1, "-")
00288     self.add_sample(extra_fall,  0, "-")
00289     self.add_sample(second_rise, 1, trigger_name[0])
00290     self.add_sample(second_fall, 0, trigger_name[1])
00291     for i in range(0,2):
00292       self.cameras[i].trigger_name = self.name+"/"+trigger_name[i]
00293       #print self.cameras[i].trigger_name
00294     
00295     #print "About to update trigger", self.name
00296     MultiTriggerController.update(self)
00297 
00298 class Projector:
00299   def process_update(self, config, level):
00300     # Produces:
00301     # MEMBERS
00302     # proj_end_offset, alt_end_offset, noproj_end_offset
00303     # pulse_starts, pulse_ends
00304     # repeat_period
00305     #
00306     # PARAMETERS 
00307     # projector_rate and projector_pulse_length
00308     #print "Setting needed to false."
00309     self.needed = False # If a camera needs us, the camera will say so.
00310 
00311     if level & lvl_projector == 0:
00312       return; # The projector's configuration is unchanged.
00313 
00314     base_period = 1.0 / config["projector_rate"]
00315     self.pulse_length = config["projector_pulse_length"]
00316     pulse_shift = config["projector_pulse_shift"]
00317     self.zero_offset = roundToEthercat(config["projector_tweak"])
00318     self.mode = config["projector_mode"]
00319 
00320     base_period = roundToEthercat(base_period)
00321     # Minimum is guard, pulse, guard, offset_pulse
00322     base_period = max(base_period, 4 * ETHERCAT_INTERVAL)
00323     
00324     self.repeat_period = 4.0 * base_period
00325 
00326     # Pulse can't be so long that it would coalesce with next one.
00327     self.pulse_length = min(self.pulse_length, math.floor(base_period / ETHERCAT_INTERVAL / 2.0 - 1) * ETHERCAT_INTERVAL)
00328     self.pulse_length = math.ceil(self.pulse_length / ETHERCAT_INTERVAL) * ETHERCAT_INTERVAL
00329 
00330     min_second_start = base_period
00331     max_second_start = 2 * base_period - 2 * ETHERCAT_INTERVAL - 2 * self.pulse_length
00332     second_start = min_second_start * (1 - pulse_shift) + max_second_start * pulse_shift
00333     second_start = roundToEthercat(second_start)
00334     config["projector_pulse_shift"] = (second_start - min_second_start) / (max_second_start - min_second_start)
00335 
00336     self.pulse_starts = [
00337         0,
00338         second_start,
00339         2 * base_period,
00340         2 * base_period + second_start + self.pulse_length + 2 * ETHERCAT_INTERVAL
00341         ]
00342     self.pulse_ends = [ start + self.pulse_length for start in self.pulse_starts ]
00343 
00344     self.proj_end_offset = self.pulse_length + ETHERCAT_INTERVAL
00345     self.alt_end_offset = second_start + self.pulse_length + ETHERCAT_INTERVAL
00346     self.noproj_end_offset = second_start - ETHERCAT_INTERVAL
00347 
00348     self.proj_exposure_duration = self.pulse_length + ETHERCAT_INTERVAL * 1.5
00349     self.noproj_max_exposure = second_start - self.pulse_length - ETHERCAT_INTERVAL * 1.5
00350 
00351     config["projector_rate"] = 1 / base_period
00352     config["projector_pulse_length"] = self.pulse_length
00353     config["projector_tweak"] = self.zero_offset
00354 
00355 class Camera:
00356   def __init__(self, node_name, proj, level, **paramnames):
00357     self.paramnames = paramnames
00358     self.name = node_name
00359     self.level = level
00360     self.proj = proj
00361     self.reconfigure_client = None
00362     self.async = AsynchronousUpdater(self.async_apply_update, "Camera "+node_name)
00363     self.trig_rising = True
00364 
00365   def param(self, config, name):
00366     return config[self.paramnames[name]]
00367 
00368   def setparam(self, config, name, value):
00369     config[self.paramnames[name]] = value
00370 
00371   # Uses the new configuration to compute new camera state.
00372   def process_update(self, config, level):
00373     # Produces:
00374     # MEMBERS
00375     # period, ext_trig, imager_period, end_offset
00376     #
00377     # PARAMETERS
00378     # "param_rate"
00379     self.trigger_name = "not_set" # Will be set by the CameraTriggerController
00380     
00381     # Took level checking out as it was causing problems with the projector
00382     # needed flag.
00383     #if self.level & level == 0: 
00384     #  return # This camera's configuration is unchanged.
00385     
00386     self.period = 1.0 / self.param(config, param_rate)
00387     projector_rate = config[param_proj_rate]
00388     self.reset_cameras = config["camera_reset"]
00389     self.ext_trig = True
00390     self.register_set = WGEConfig.WGE100Camera_PrimaryRegisterSet
00391 
00392     projector_limits_exposure = True
00393 
00394     trig_mode = self.param(config, param_trig_mode)
00395     # Internal triggering.
00396     if trig_mode == Config.CameraSynchronizer_InternalTrigger:
00397       self.ext_trig = False
00398       self.imager_period = self.period
00399       self.end_offset = -1
00400       projector_limits_exposure = False
00401     else:
00402       if self.proj.mode == Config.CameraSynchronizer_ProjectorOff:
00403           trig_mode = Config.CameraSynchronizer_IgnoreProjector
00404 
00405       if trig_mode == Config.CameraSynchronizer_AlternateProjector or trig_mode == Config.CameraSynchronizer_WithProjector:
00406           self.proj.needed = True
00407           #print self.name, "setting needed to true."
00408       #else:
00409           #print self.name, "not setting needed to true."
00410       
00411       # Set the period
00412       if trig_mode == Config.CameraSynchronizer_AlternateProjector:
00413         n = round(self.period / self.proj.repeat_period - 0.5)
00414         n = max(n, 0)
00415         self.period = (n + 0.5) * self.proj.repeat_period
00416         #print "Case 1", n
00417       elif trig_mode == Config.CameraSynchronizer_IgnoreProjector:
00418         self.period = roundToEthercat(self.period)
00419       else:
00420         n = round(2 * self.period / self.proj.repeat_period - 1)
00421         n = max(n, 0)
00422         self.period = (n + 1) * self.proj.repeat_period / 2.0
00423         #print "Case 2", n, self.period
00424 
00425       # Set the end_offset
00426       if trig_mode == Config.CameraSynchronizer_IgnoreProjector:
00427         self.end_offset = 0
00428         projector_limits_exposure = False
00429       if trig_mode == Config.CameraSynchronizer_AlternateProjector:
00430         self.end_offset = self.proj.alt_end_offset 
00431         self.register_set = WGEConfig.WGE100Camera_Auto
00432       elif trig_mode == Config.CameraSynchronizer_WithProjector:
00433         self.end_offset = self.proj.proj_end_offset
00434         self.register_set = WGEConfig.WGE100Camera_AlternateRegisterSet
00435       else:
00436         self.end_offset = self.proj.noproj_end_offset
00437 
00438       # Pick the imager period
00439       if trig_mode == Config.CameraSynchronizer_IgnoreProjector:
00440           self.imager_period = self.period - ETHERCAT_INTERVAL
00441       else:
00442           self.imager_period = self.proj.repeat_period / 2 - ETHERCAT_INTERVAL
00443 
00444     #print "Camera period", self.name, self.period, self.imager_period, self.proj.repeat_period
00445     if projector_limits_exposure:
00446         self.max_exposure = self.proj.noproj_max_exposure
00447         #print "Exposurei projector", self.max_exposure
00448     else:
00449         self.max_exposure = self.imager_period * 0.95
00450         #print "Exposurei imager", self.max_exposure
00451 
00452     self.setparam(config, param_rate, 1/self.period)
00453     self.setparam(config, param_trig_mode, trig_mode)
00454 
00455   def async_apply_update(self, reconfig_request):
00456       try:    
00457           #print "**** Start", self.name
00458           if self.reconfigure_client == None:
00459               #print "**** Making client", self.name
00460               self.reconfigure_client = DynamicReconfigureClient(self.name)
00461               #print "**** Made client", self.name
00462 
00463           self.reconfigure_client.update_configuration(reconfig_request)
00464           #print "**** Reconfigured client", self.name
00465           #print "Done updating camera ", self.name
00466       except KeyboardInterrupt: # Handle CTRL+C
00467           print "Aborted camera update on", self.name
00468       
00469   def apply_update(self):
00470       reconfig_request = {
00471               "ext_trig" : self.ext_trig,
00472               "trig_rate" : 1.0 / self.period,
00473               "imager_rate" : 1.0 / self.imager_period,
00474               "trig_timestamp_topic" : self.trigger_name,
00475               "rising_edge_trig" : self.trig_rising,
00476               "register_set" : self.register_set,
00477               "auto_exposure_alternate" : False,
00478               "exposure_alternate" : self.proj.proj_exposure_duration,
00479               "max_exposure" : self.max_exposure,
00480               }
00481       #print self.name, reconfig_request
00482 
00483       self.async.update(reconfig_request)
00484 
00485 # Need to set:
00486 # Global period if synchronized
00487 # Camera
00488 #   Frame rate
00489 #   Trigger mode
00490 #   Exposure alt
00491 #   Maximum exposure alt
00492 #   Alternate enabled
00493 #   Alternate only
00494 #   Trigger name
00495 # Trigger
00496 #   Full waveform
00497 # Projector
00498 #   Full waveform
00499 #   Prosilica setting
00500 
00501 class CameraSynchronizer:
00502   def __init__(self, forearm=True):
00503     stereo_camera_names = [ "narrow_stereo", "wide_stereo" ] # narrow must be first as it can be alternate, and hence has more period restrictions. 
00504     forearm_camera_names = [ "forearm_r", "forearm_l" ]
00505     self.camera_names = stereo_camera_names
00506     if forearm:
00507         self.camera_names = self.camera_names + forearm_camera_names
00508     # Parameter names are pretty symmetric. Build them up automatically.
00509     parameters = [ param_rate, param_trig_mode ]
00510     camera_parameters = dict((name, dict((suffix, name+"_"+suffix) for suffix in parameters)) for name in self.camera_names)
00511     # Some parameters are common to the wide and narrow cameras. Also store
00512     # the node names.
00513     for camera in stereo_camera_names:
00514       camera_parameters[camera][param_rate] = "stereo_rate"
00515       camera_parameters[camera]["node_name"] = camera+"_both"
00516     if forearm:
00517       for camera in forearm_camera_names:
00518         camera_parameters[camera]["node_name"] = camera[-1]+"_forearm_cam"
00519     for i in range(0, len(self.camera_names)):
00520       camera_parameters[self.camera_names[i]]["level"] = 1 << i; # This only works because of the specific levels in the .cfg file.
00521 
00522     self.projector = Projector()
00523     self.cameras = dict((name, Camera(proj = self.projector, **camera_parameters[name])) for name in self.camera_names)
00524     self.prosilica_inhibit = ProsilicaInhibitTriggerController('prosilica_inhibit_projector_controller', "prosilica_projector_inhibit", 0x0A, 0x00)
00525     
00526     self.controllers = [
00527       ProjectorTriggerController('projector_trigger', self.projector),
00528       DualCameraTriggerController('head_camera_trigger', *[self.cameras[name] for name in stereo_camera_names])]
00529     if forearm:
00530       self.controllers = self.controllers + [
00531           SingleCameraTriggerController('r_forearm_cam_trigger', self.cameras["forearm_r"]),
00532           SingleCameraTriggerController('l_forearm_cam_trigger', self.cameras["forearm_l"]),
00533         ]
00534 
00535     self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
00536 
00537   @staticmethod
00538   def print_threads():
00539     threads = threading.enumerate()
00540     n = 0
00541     for t in threads:
00542         if not t.isDaemon() and t != threading.currentThread():
00543             try: 
00544                print t.name
00545             except:
00546                 print "Unknown thread ", t
00547             n = n + 1
00548     return n
00549 
00550   def kill(self):
00551     print "\nWaiting for all threads to die..."
00552     killAsynchronousUpdaters()
00553     #time.sleep(1)
00554     while self.print_threads() > 0:         
00555         print "\nStill waiting for all threads to die..."
00556         time.sleep(1)
00557     print
00558 
00559   def reconfigure(self, config, level):
00560     # print "Reconfigure", config
00561     # Reconfigure the projector.
00562     self.projector.process_update(config, level)
00563     self.prosilica_inhibit.process_update(config, level)
00564     # Reconfigure the cameras.
00565     for camera in self.cameras.values():
00566       camera.process_update(config, level)
00567     #for trig_controller in self.trig_controllers:
00568     #  trig_controller.update()
00569     #for camera in self.cameras.keys():
00570     #  camera.update()
00571     for controller in self.controllers:
00572         controller.update();
00573     for camera in self.cameras.values():
00574         camera.apply_update()
00575     #print config
00576     self.config = config
00577     return config
00578   
00579   def update_diagnostics(self):
00580     da = DiagnosticArray()
00581     ds = DiagnosticStatus()
00582     ds.name = rospy.get_caller_id().lstrip('/') + ": Tasks"
00583     in_progress = 0;
00584     longest_interval = 0;
00585     for updater in list(asynchronous_updaters):
00586         (name, interval) = updater.getStatus()
00587         if interval == 0:
00588             msg = "Idle"
00589         else:
00590             in_progress = in_progress + 1
00591             msg = "Update in progress (%i s)"%interval
00592         longest_interval = max(interval, longest_interval)
00593         ds.values.append(KeyValue(name, msg))
00594     if in_progress == 0:
00595         ds.message = "Idle"
00596     else:
00597         ds.message = "Updates in progress: %i"%in_progress
00598     if longest_interval > 10:
00599         ds.level = 1
00600         ds.message = "Update is taking too long: %i"%in_progress
00601     ds.hardware_id = "none"
00602     da.status.append(ds)
00603     da.header.stamp = rospy.get_rostime()
00604     self.diagnostic_pub.publish(da)
00605 
00606   def spin(self):
00607     self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00608     try:
00609       reset_count = 0
00610       rospy.loginfo("Camera synchronizer is running...")
00611       controller_update_count = 0
00612       while not rospy.is_shutdown():
00613           if self.config['camera_reset'] == True:
00614               reset_count = reset_count + 1
00615               if reset_count > 3:
00616                   self.server.update_configuration({'camera_reset' : False})
00617           else:
00618               reset_count = 0
00619           self.update_diagnostics()
00620           # In case the controllers got restarted, refresh their state.
00621           controller_update_count += 1
00622           if controller_update_count >= 10:
00623               controller_update_count = 0
00624               for controller in self.controllers:
00625                   controller.update();
00626           rospy.sleep(1)
00627     finally:
00628       rospy.signal_shutdown("Main thread exiting")
00629       self.kill()
00630       print "Main thread exiting"


pr2_camera_synchronizer
Author(s): Blaise Gassend
autogenerated on Thu Sep 3 2015 11:31:20