controllers_to_diagnostics.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 # 
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 # 
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 # 
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 import roslib; roslib.load_manifest('pr2_controller_manager')
00030 import rospy
00031 
00032 from pr2_mechanism_msgs.msg import MechanismStatistics
00033 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00034 
00035 def controller_to_diag(c):
00036     d = DiagnosticStatus()
00037     d.name = 'Controller ('+c.name+')'
00038 
00039     d.level = 0
00040     if (c.running):
00041         d.message = 'Running'
00042     else:
00043         d.message = 'Stopped'
00044 
00045     if (not use_sim_time and c.num_control_loop_overruns > 0):
00046         d.message += ' !!! Broke Realtime, used '+str(int(c.max_time.to_sec()*1e6))+' micro seconds in update loop'
00047         if c.time_last_control_loop_overrun + rospy.Duration(30.0) > rospy.Time.now():
00048             d.level = 1
00049 
00050     d.values.append(KeyValue('Avg Time in Update Loop (usec)',str(int(c.mean_time.to_sec()*1e6))))
00051     d.values.append(KeyValue('Max Time in update Loop (usec)',str(int(c.max_time.to_sec()*1e6))))
00052     d.values.append(KeyValue('Variance on Time in Update Loop',str(int(c.variance_time.to_sec()*1e6))))
00053     d.values.append(KeyValue('Percent of Cycle Time Used',str(int(c.mean_time.to_sec()/0.00001))))
00054     d.values.append(KeyValue('Number of Control Loop Overruns',str(int(c.num_control_loop_overruns))))
00055     d.values.append(KeyValue('Timestamp of Last Control Loop Overrun (sec)',str(int(c.time_last_control_loop_overrun.to_sec()))))
00056     return d
00057 
00058 rospy.init_node('controller_to_diagnostics')
00059 use_sim_time = rospy.get_param('use_sim_time', False)
00060 pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray)
00061 
00062 last_publish_time = rospy.Time(0.0)
00063 def state_cb(msg):
00064     global last_publish_time
00065     now = rospy.get_rostime()
00066     if (now - last_publish_time).to_sec() > 1.0:
00067         if len(msg.controller_statistics) == 0:
00068             d = DiagnosticArray()
00069             d.header.stamp = msg.header.stamp
00070             ds = DiagnosticStatus()
00071             ds.name = "Controller: No controllers loaded in controller manager"
00072             d.status = [ds]
00073             pub_diag.publish(d)
00074         else:
00075             for c in msg.controller_statistics:
00076                 d = DiagnosticArray()
00077                 d.header.stamp = msg.header.stamp
00078                 d.status = [controller_to_diag(c)]
00079                 pub_diag.publish(d)
00080         last_publish_time = now
00081 
00082 rospy.Subscriber('mechanism_statistics', MechanismStatistics, state_cb)
00083 rospy.spin()


sr_edc_controller_configuration
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Thu Aug 27 2015 15:16:32