capture_exec.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 import rospy
00035 import yaml
00036 import sys
00037 import actionlib
00038 import joint_states_settler.msg
00039 import time
00040 import threading
00041 import os
00042 import string
00043 
00044 import capture_executive
00045 from capture_executive.config_manager import ConfigManager
00046 from capture_executive.sensor_managers import *
00047 from capture_executive.robot_measurement_cache import RobotMeasurementCache
00048 
00049 from calibration_msgs.msg import RobotMeasurement
00050 
00051 from urdf_parser_py.urdf import URDF
00052 
00053 class CaptureExecutive:
00054     def __init__(self, config_dir, system, robot_desc, output_debug=False):
00055         # Load configs
00056         self.cam_config        = yaml.load(open(config_dir + "/cam_config.yaml"))
00057         self.chain_config      = yaml.load(open(config_dir + "/chain_config.yaml"))
00058         self.laser_config      = yaml.load(open(config_dir + "/laser_config.yaml"))
00059         self.controller_config = yaml.load(open(config_dir + "/controller_config.yaml"))
00060         # Not all robots have lasers.... :(
00061         if self.laser_config == None:
00062             self.laser_config = dict()
00063         # Debug mode makes bag files huge...
00064         self.output_debug = output_debug
00065 
00066         # Error message from sample capture
00067         self.message = None
00068 
00069         # Status message from interval computation
00070         self.interval_status = None
00071 
00072         # parse urdf and get list of links
00073         links = URDF().parse(robot_desc).link_map.keys()
00074 
00075         # load system config
00076         system = yaml.load(open(system))
00077         
00078         # remove cams that are not on urdf
00079         for cam in self.cam_config.keys():
00080             try:
00081                 link = system['sensors']['rectified_cams'][cam]['frame_id']
00082                 if not link in links:
00083                     print 'URDF does not contain link [%s]. Removing camera [%s]' % (link, cam)
00084                     del self.cam_config[cam]
00085             except:
00086                 print 'System description does not contain camera [%s]' % cam
00087                 del self.cam_config[cam]
00088 
00089         # remove arms that are not on urdf
00090         for chain in self.chain_config.keys():
00091             try:
00092                 link = system['sensors']['chains'][chain]['tip']
00093                 if not link in links:
00094                     print 'URDF does not contain link [%s]. Removing chain [%s]' % (link, chain)
00095                     del self.chain_config[chain]
00096             except:
00097                 print 'System description does not contain chain [%s]' % chain
00098                 del self.chain_config[chain]
00099 
00100         self.cache = RobotMeasurementCache()
00101         self.lock = threading.Lock()
00102 
00103         # Specifies if we're currently waiting for a sample
00104         self.active = False
00105 
00106         # Construct Configuration Manager (manages configuration of nodes in the pipeline)
00107         self.config_manager = ConfigManager(self.cam_config,
00108                                             self.chain_config,
00109                                             self.laser_config,
00110                                             self.controller_config)
00111 
00112         # Construct a manager for each sensor stream (Don't enable any of them)
00113         self.cam_managers  = [ (cam_id,   CamManager(  cam_id,   self.add_cam_measurement) )   for cam_id   in self.cam_config.keys() ]
00114         self.chain_managers = [ (chain_id, ChainManager(chain_id, self.add_chain_measurement) ) for chain_id in self.chain_config.keys() ]
00115         self.laser_managers = [ (laser_id, LaserManager(laser_id, self.add_laser_measurement) ) for laser_id in self.laser_config.keys() ]
00116 
00117         # Subscribe to topic containing stable intervals
00118         self.request_interval_sub = rospy.Subscriber("intersected_interval", calibration_msgs.msg.Interval, self.request_callback)
00119 
00120         # Subscribe to topic containing stable intervals
00121         self.interval_status_sub = rospy.Subscriber(
00122               "intersected_interval_status",
00123               calibration_msgs.msg.IntervalStatus, self.status_callback)
00124 
00125         # Hardcoded cache sizes. Not sure where these params should live
00126         # ...
00127 
00128     def capture(self, next_configuration, timeout):
00129         done = False
00130         self.m_robot = None
00131         self.message = None
00132         self.interval_status = None
00133 
00134         timeout_time = rospy.Time().now() + timeout
00135 
00136         self.lock.acquire()
00137         if self.active:
00138             rospy.logerr("Can't capture since we're already active")
00139             done = True
00140         self.lock.release()
00141 
00142         camera_measurements = next_configuration["camera_measurements"]
00143         next_configuration["camera_measurements"] = []
00144 
00145         for (i, cam) in enumerate(camera_measurements):
00146             if self.cam_config.has_key(cam["cam_id"]):
00147                 next_configuration["camera_measurements"].append(cam)
00148             else:
00149                 rospy.logdebug("Not capturing measurement for camera: %s"%(cam["cam_id"]))
00150 
00151         # Set up the pipeline
00152         self.config_manager.reconfigure(next_configuration)
00153 
00154         time.sleep(2.0)
00155 
00156         # Set up the cache with only the sensors we care about
00157         cam_ids   = [x["cam_id"]   for x in next_configuration["camera_measurements"]]
00158         chain_ids = [x["chain_id"] for x in next_configuration["joint_measurements"]]
00159         try:
00160             laser_ids = [x["laser_id"] for x in next_configuration["laser_measurements"]]
00161         except:
00162             laser_ids = list()
00163         self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
00164 
00165         rospy.logdebug("Setting up sensor managers")
00166         enable_list = []
00167         disable_list = []
00168         # Set up the sensor managers
00169         for cam_id, cam_manager in self.cam_managers:
00170             if cam_id in [x["cam_id"] for x in next_configuration["camera_measurements"]]:
00171                 enable_list.append(cam_id)
00172                 cam_manager.enable(self.output_debug)
00173             else:
00174                 disable_list.append(cam_id)
00175                 cam_manager.disable()
00176 
00177         for chain_id, chain_manager in self.chain_managers:
00178             if chain_id in [x["chain_id"] for x in next_configuration["joint_measurements"]]:
00179                 enable_list.append(chain_id)
00180                 chain_manager.enable()
00181             else:
00182                 disable_list.append(chain_id)
00183                 chain_manager.disable()
00184 
00185         for laser_id, laser_manager in self.laser_managers:
00186             enabled_lasers = [x["laser_id"] for x in next_configuration["laser_measurements"]]
00187             if laser_id in enabled_lasers:
00188                 enable_list.append(laser_id)
00189                 laser_manager.enable(self.output_debug)
00190             else:
00191                 disable_list.append(laser_id)
00192                 laser_manager.disable()
00193 
00194         print "Enabling:"
00195         for cur_enabled in enable_list:
00196             print " + %s" % cur_enabled
00197         print "Disabling:"
00198         for cur_disabled in disable_list:
00199             print " - %s" % cur_disabled
00200 
00201         self.lock.acquire()
00202         self.active = True
00203         self.lock.release()
00204 
00205         print "\nCollecting sensor data.."
00206         # Keep waiting until the request_callback function populates the m_robot msg
00207         while (not rospy.is_shutdown()) and (not done) and (rospy.Time().now() < timeout_time):
00208             time.sleep(.1)
00209             self.lock.acquire()
00210             if self.m_robot is not None:
00211                 done = True
00212             self.lock.release()
00213 
00214         print "Stopping sensor streams..\n"
00215         # Stop listening to all the sensor streams
00216         for cam_id, cam_manager in self.cam_managers:
00217             cam_manager.disable()
00218         for chain_id, chain_manager in self.chain_managers:
00219             chain_manager.disable()
00220         for laser_id, laser_manager in self.laser_managers:
00221             laser_manager.disable()
00222 
00223         # Turn off the entire pipeline
00224         self.config_manager.stop()
00225 
00226         # Fill in meta information about the sample
00227         if self.m_robot is not None:
00228             self.m_robot.sample_id = next_configuration["sample_id"]
00229             self.m_robot.target_id = next_configuration["target"]["target_id"]
00230             self.m_robot.chain_id  = next_configuration["target"]["chain_id"]
00231         elif self.interval_status is not None:
00232             total = 0
00233             bad_sensors = []
00234             l = len(self.interval_status.names)
00235             if l != len(self.interval_status.yeild_rates):
00236                rospy.logerr("Invalid interval status message; names and yeild rates have different lengths")
00237                l = min(l, len(self.interval_status.yeild_rates))
00238             # analyze status message
00239             for i in range(l):
00240                if self.interval_status.yeild_rates[i] == 0.0:
00241                   bad_sensors.append(self.interval_status.names[i])
00242                total += self.interval_status.yeild_rates[i]
00243 
00244             # if we didn't get any samples from some sensors, complain
00245             if len(bad_sensors) > 0:
00246                print "Didn't get good data from %s"%(', '.join(bad_sensors))
00247             else:
00248                # if we got data from all of our sensors, complain about the
00249                # ones that were below the mean (heuristic)
00250                avg = total / l
00251                for i in range(l):
00252                   if self.interval_status.yeild_rates[i] <= avg:
00253                      bad_sensors.append(self.interval_status.names[i])
00254                print "%s may be performing poorly"%(", ".join(bad_sensors))
00255         elif self.message is not None:
00256             print self.message
00257 
00258         self.lock.acquire()
00259         self.active = False
00260         self.lock.release()
00261 
00262         return self.m_robot
00263 
00264     def request_callback(self, msg):
00265         # See if the interval is big enough to care
00266         if (msg.end - msg.start) < rospy.Duration(1,0):
00267             return
00268 
00269         self.lock.acquire()
00270         if self.active:
00271             # Do some query into the cache, and possibly stop stuff from running
00272             m = self.cache.request_robot_measurement(msg.start, msg.end)
00273             if isinstance(m, basestring):
00274                self.message = m
00275             else:
00276                self.m_robot = m
00277             # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care)
00278             if self.m_robot is not None:
00279                 self.active = False
00280         self.lock.release()
00281 
00282     def status_callback(self, msg):
00283         self.lock.acquire()
00284         if self.active:
00285             self.interval_status = msg
00286         self.lock.release()
00287 
00288     def add_cam_measurement(self, cam_id, msg):
00289         if len(msg.image_points) > 0:
00290             self.lock.acquire()
00291             self.cache.add_cam_measurement(cam_id, msg)
00292             self.lock.release()
00293 
00294     def add_chain_measurement(self, chain_id, msg):
00295         self.lock.acquire()
00296         self.cache.add_chain_measurement(chain_id, msg)
00297         self.lock.release()
00298 
00299     def add_laser_measurement(self, laser_id, msg, interval_start, interval_end):
00300         self.lock.acquire()
00301         self.cache.add_laser_measurement(laser_id, msg, interval_start, interval_end)
00302         self.lock.release()
00303 
00304 if __name__=='__main__':
00305     rospy.init_node("capture_executive_node")
00306 
00307     rospy.logdebug("Starting executive...")
00308     rospy.sleep(5.0)
00309 
00310     # TODO: convert to parameters?
00311     samples_dir = rospy.myargv()[1]
00312     config_dir = rospy.myargv()[2]
00313     system = rospy.myargv()[3]
00314 
00315     try:
00316         robot_description = rospy.get_param('robot_description')
00317     except:
00318         rospy.logfatal('robot_description not set, exiting')
00319         sys.exit(-1)
00320 
00321     executive = CaptureExecutive(config_dir, system, robot_description)
00322     time.sleep(1.0)
00323 
00324     # setup our samples
00325     sample_steps = list()
00326     sample_names = dict()
00327     sample_options = dict()
00328     sample_success = dict()
00329     sample_failure = dict()
00330     for directory in os.listdir(samples_dir):
00331         try:
00332             sample_options[directory] = yaml.load(open(samples_dir + '/' + directory + '/config.yaml'))
00333             sample_steps.append(directory)
00334         except IOError:
00335             continue
00336         sample_names[directory] = [x for x in os.listdir(samples_dir + '/' + directory + '/') if '.yaml' in x and x != 'config.yaml']
00337         sample_names[directory].sort()
00338         sample_success[directory] = 0
00339         sample_failure[directory] = 0
00340     sample_steps.sort()
00341 
00342     for step in sample_steps:
00343         rospy.logdebug("%s Samples: \n - %s" % (sample_options[step]['group'], "\n - ".join(sample_names[step])))
00344 
00345     pub = rospy.Publisher("robot_measurement", RobotMeasurement)
00346 
00347     try:
00348         for step in sample_steps:
00349             keep_collecting = True
00350             full_paths = [samples_dir + '/' + step + '/' + x for x in sample_names[step] ]
00351             if len(full_paths) == 0:
00352                 rospy.logfatal(samples_dir + '/' + step + '/' + ' is not found' )
00353                 rospy.logfatal('please run make_samples.py' )
00354                 sys.exit(-1)
00355             cur_config = yaml.load(open(full_paths[0]))
00356             m_robot = executive.capture(cur_config, rospy.Duration(0.01))
00357             while not rospy.is_shutdown() and keep_collecting:
00358                 print
00359                 print sample_options[step]["prompt"]
00360                 print "Press <enter> to continue, type N to exit this step"
00361                 resp = raw_input(">>>")
00362                 if string.upper(resp) == "N":
00363                     print sample_options[step]["finish"]
00364                     keep_collecting = False
00365                 else:
00366                     for cur_sample_path in full_paths:
00367                         print "On %s sample [%s]" % (sample_options[step]["group"], cur_sample_path)
00368                         cur_config = yaml.load(open(cur_sample_path))
00369                         m_robot = executive.capture(cur_config, rospy.Duration(40))
00370                         if m_robot is None:
00371                             print "--------------- Failed To Capture a %s Sample -----------------" % sample_options[step]["group"]
00372                             if not sample_options[step]["repeat"]:
00373                                 sample_failure[step] += 1
00374                         else:
00375                             print "++++++++++++++ Successfully Captured a %s Sample ++++++++++++++" % sample_options[step]["group"]
00376                             sample_success[step] += 1
00377                             pub.publish(m_robot)
00378                         print "Succeeded on %u %s samples" % (sample_success[step], sample_options[step]["group"])
00379                         if rospy.is_shutdown():
00380                             break
00381                     keep_collecting = sample_options[step]["repeat"]
00382     except EOFError:
00383         print "Exiting"    
00384 
00385     time.sleep(1)
00386 
00387     print "Calibration data collection has completed!"
00388     for step in sample_steps:
00389         if sample_options[step]["repeat"]:
00390             print "%s Samples: %u" % (sample_options[step]["group"], sample_success[step])
00391         else:
00392             print "%s Samples: %u/%u" % (sample_options[step]["group"], sample_success[step], sample_success[step] + sample_failure[step])
00393     print ""
00394     print "You can now kill this node, along with any other calibration nodes that are running."
00395 
00396 


calibration_launch
Author(s): Michael Ferguson
autogenerated on Tue Sep 27 2016 04:06:51