sr_edc_controller_configuration: controllers_to

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 rospy
00030 
00031 from pr2_mechanism_msgs.msg import MechanismStatistics
00032 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00033 
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 ' + \
00047             str(int(c.max_time.to_sec() * 1e6)) + \
00048             ' micro seconds in update loop'
00049         if c.time_last_control_loop_overrun + rospy.Duration(30.0) > rospy.Time.now():
00050             d.level = 1
00051 
00052     d.values.append(
00053         KeyValue('Avg Time in Update Loop (usec)', str(int(c.mean_time.to_sec() * 1e6))))
00054     d.values.append(
00055         KeyValue('Max Time in update Loop (usec)', str(int(c.max_time.to_sec() * 1e6))))
00056     d.values.append(
00057         KeyValue('Variance on Time in Update Loop', str(int(c.variance_time.to_sec() * 1e6))))
00058     d.values.append(
00059         KeyValue('Percent of Cycle Time Used', str(int(c.mean_time.to_sec() / 0.00001))))
00060     d.values.append(
00061         KeyValue('Number of Control Loop Overruns', str(int(c.num_control_loop_overruns))))
00062     d.values.append(
00063         KeyValue('Timestamp of Last Control Loop Overrun (sec)', str(int(c.time_last_control_loop_overrun.to_sec()))))
00064     return d
00065 
00066 rospy.init_node('controller_to_diagnostics')
00067 use_sim_time = rospy.get_param('use_sim_time', False)
00068 pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray)
00069 
00070 last_publish_time = rospy.Time(0.0)
00071 
00072 
00073 def state_cb(msg):
00074     global last_publish_time
00075     now = rospy.get_rostime()
00076     if (now - last_publish_time).to_sec() > 1.0:
00077         if len(msg.controller_statistics) == 0:
00078             d = DiagnosticArray()
00079             d.header.stamp = msg.header.stamp
00080             ds = DiagnosticStatus()
00081             ds.name = "Controller: No controllers loaded in controller manager"
00082             d.status = [ds]
00083             pub_diag.publish(d)
00084         else:
00085             for c in msg.controller_statistics:
00086                 d = DiagnosticArray()
00087                 d.header.stamp = msg.header.stamp
00088                 d.status = [controller_to_diag(c)]
00089                 pub_diag.publish(d)
00090         last_publish_time = now
00091 
00092 rospy.Subscriber('mechanism_statistics', MechanismStatistics, state_cb)
00093 rospy.spin()