cb_qual_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2009, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 ##\author Kevin Watts
00036 ##\brief Analyzes results from counterbalance test controller
00037 
00038 PKG = 'pr2_counterbalance_check'
00039 import roslib
00040 roslib.load_manifest(PKG)
00041 
00042 import os
00043 import rospy
00044 
00045 from pr2_self_test_msgs.srv import TestResult, TestResultRequest
00046 from std_msgs.msg import Bool
00047 from joint_qualification_controllers.msg import CounterbalanceTestData
00048 
00049 from pr2_counterbalance_check.counterbalance_analysis import *
00050 
00051 
00052 class CounterbalanceAnalyzer:
00053     def __init__(self):
00054         self._sent_results = False
00055         self._motors_halted = True
00056         self._data = None
00057 
00058         self.motors_topic = rospy.Subscriber('pr2_etherCAT/motors_halted', Bool, self._motors_cb)
00059         self.data_topic = rospy.Subscriber('cb_test_data', CounterbalanceTestData, self._data_callback)
00060         self._result_service = rospy.ServiceProxy('test_result', TestResult)
00061 
00062         self._model_file = rospy.get_param('~model_file', None)
00063 
00064 
00065     def has_data(self):
00066         return self._data is not None
00067 
00068     def send_results(self, r):
00069         if not self._sent_results:
00070             try:
00071                 rospy.wait_for_service('test_result', 10)
00072             except:
00073                 rospy.logerr('Wait for service \'test_result\' timed out! Unable to send results.')
00074                 return False
00075                 
00076             self._result_service.call(r)
00077             self._sent_results = True
00078 
00079             return True
00080             
00081     def test_failed_service_call(self, except_str = ''):
00082         rospy.logerr(except_str)
00083         r = TestResultRequest()
00084         r.html_result = except_str
00085         r.text_summary = 'Caught exception, automated test failure.'
00086         r.result = TestResultRequest.RESULT_FAIL
00087         self.send_results(r)
00088 
00089     def _motors_cb(self, msg):
00090         self._motors_halted = msg.data
00091 
00092     def _data_callback(self, msg):
00093         self._data = msg
00094         
00095     def process_results(self):
00096         msg = self._data
00097         try:
00098             data = CounterbalanceAnalysisData(msg)
00099             params = CounterbalanceAnalysisParams(msg)
00100 
00101             lift_effort_result = analyze_lift_efforts(params, data)
00102             lift_effort_plot = plot_efforts_by_lift_position(params, data)
00103 
00104             if params.flex_test:
00105                 flex_effort_result = analyze_flex_efforts(params, data)
00106                 lift_effort_contour = plot_effort_contour(params, data)
00107                 flex_effort_contour = plot_effort_contour(params, data, False)
00108 
00109                 if self._model_file and os.path.exists(self._model_file):
00110                     adjust_result = check_cb_adjustment(params, data, self._model_file)
00111                 elif self._model_file:
00112                     adjust_result = CounterbalanceAnalysisResult()
00113                     adjust_result.result = False
00114                     adjust_result.html = '<p>CB model file is missing. File %s does not exist. This file is used for testing the CB adjustment.</p>\n' % self._model_file
00115                     adjust_result.summary = 'CB model file missing, unable to analyze'
00116                 else: # Don't check CB adjustment
00117                     adjust_result = CounterbalanceAnalysisResult()
00118                     adjust_result.result = True
00119                     adjust_result.html = '<p>Did not check counterbalance adjustment.</p>'
00120                                         
00121 
00122             html = []
00123             if params.flex_test:
00124                 if self._model_file:
00125                     html.append('<H4>CB Adjustment Recommendations and Analysis</H4>')
00126                     html.append(adjust_result.html)
00127                     html.append('<p>Further information is for debugging and analysis information only.</p><br><hr size="2" />')
00128 
00129                 html.append('<H4>Lift Effort Contour Plot</H4>')
00130                 html.append('<img src=\"IMG_PATH/%s.png\" width=\"640\" height=\"480\" />' % (lift_effort_contour.title))
00131                 html.append('<H4>Flex Effort Contour Plot</H4>')
00132                 html.append('<img src=\"IMG_PATH/%s.png\" width=\"640\" height=\"480\" />' % (flex_effort_contour.title))
00133                 
00134                 html.append('<H4>Flex Effort Analysis</H4>')
00135                 html.append(flex_effort_result.html)
00136 
00137             html.append('<H4>Lift Effort Analysis</H4>')
00138             html.append(lift_effort_result.html)
00139             html.append('<img src=\"IMG_PATH/%s.png\" width=\"640\" height=\"480\" />' % (lift_effort_plot.title))
00140 
00141             html.append('<p>Test Parameters</p>')
00142             html.append('<table border="1" cellpadding="2" cellspacing="0">')
00143             html.append('<tr><td><b>Joint</b></td><td><b>Dither Amplitude</b></td></tr>')
00144             html.append('<tr><td>%s</td><td>%s</td></tr>' % (params.lift_joint, params.lift_dither))
00145             if params.flex_test:
00146                 html.append('<tr><td>%s</td><td>%s</td></tr>' % (params.flex_joint, params.flex_dither))
00147             html.append('</table>')
00148             html.append('<hr size=2>')
00149            
00150             r = TestResultRequest()
00151             r.html_result = '\n'.join(html)
00152             r.text_summary = ' '.join([lift_effort_result.summary])
00153             if params.flex_test:
00154                 r.text_summary = ' '.join([r.text_summary, flex_effort_result.summary])
00155 
00156             r.result = TestResultRequest.RESULT_HUMAN_REQUIRED
00157 
00158 
00159             if params.flex_test and (lift_effort_result.result and 
00160                                      flex_effort_result.result and 
00161                                      adjust_result.result):
00162                 r.result = TestResultRequest.RESULT_PASS
00163             elif not params.flex_test and lift_effort_result.result:
00164                 r.result = TestResultRequest.RESULT_PASS
00165 
00166 
00167             r.plots = [ lift_effort_plot ]
00168             if params.flex_test:
00169                 r.plots.append(lift_effort_contour)
00170                 r.plots.append(flex_effort_contour)
00171             r.params = params.get_test_params()
00172             r.values = lift_effort_result.values
00173             if params.flex_test:
00174                 r.values.extend(flex_effort_result.values)
00175                 r.values.extend(adjust_result.values)
00176 
00177             # Adjustment required
00178             if params.flex_test and not adjust_result.result:
00179                 r.result = TestResultRequest.RESULT_HUMAN_REQUIRED
00180                 r.text_summary = adjust_result.summary
00181 
00182             # Check motors halted
00183             if self._motors_halted:
00184                 r.text_summary = 'Fail, motors halted. Check estop and power board.'
00185                 r.html_result = '<H4>Motors Halted</H4>\n<p>Unable to analyze CB. Check estop and power board.</p>\n' + r.html_result
00186                 r.result = TestResultRequest.RESULT_FAIL
00187             
00188             # Check timeout of controller
00189             if params.timeout_hit:
00190                 r.text_summary = 'Fail, controller timeout hit. Timeout = %.ds. Test terminated early.' % (int(params.named_params["Timeout"]))
00191                 r.html_result = '<H4>Timeout Hit</H4>\n<p>Unable to analyzer CB. Controller timeout hit.</p>\n' + r.html_result
00192                 r.result = TestResultRequest.RESULT_FAIL
00193 
00194 
00195             self.send_results(r)
00196         except Exception, e:
00197             import traceback
00198             self.test_failed_service_call(traceback.format_exc());
00199 
00200             
00201 if __name__ == '__main__':
00202     rospy.init_node('cb_analyzer')
00203     app = CounterbalanceAnalyzer()
00204     try:
00205         my_rate = rospy.Rate(5)
00206         while not app.has_data() and not rospy.is_shutdown():
00207             my_rate.sleep()
00208 
00209         if not rospy.is_shutdown():
00210             app.process_results()
00211 
00212         rospy.spin()
00213     except KeyboardInterrupt, e:
00214         pass
00215     except Exception, e:
00216         print 'Caught Exception in CB application'
00217         import traceback
00218         traceback.print_exc()
00219         result_service = rospy.ServiceProxy('test_result', TestResult)        
00220         
00221         r = TestResultRequest()
00222         r.html_result = traceback.format_exc()
00223         r.text_summary = 'Caught exception, automated test failure.'
00224         r.plots = []
00225         r.result = TestResultRequest.RESULT_HUMAN_REQUIRED
00226         result_service.call(r)


pr2_counterbalance_check
Author(s): Kevin Watts
autogenerated on Sat Apr 27 2019 03:11:00