controllers_to_diagnostics.py
Go to the documentation of this file.
1 #! /usr/bin/python
2 # Copyright (c) 2009, Willow Garage, Inc.
3 # All rights reserved.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above copyright
11 # notice, this list of conditions and the following disclaimer in the
12 # documentation and/or other materials provided with the distribution.
13 # * Neither the name of the Willow Garage, Inc. nor the names of its
14 # contributors may be used to endorse or promote products derived from
15 # this software without specific prior written permission.
16 #
17 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 
29 import rospy
30 
31 from sr_robot_msgs.msg import MechanismStatistics
32 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
33 
34 
36  d = DiagnosticStatus()
37  d.name = 'Controller (' + c.name + ')'
38 
39  d.level = 0
40  if (c.running):
41  d.message = 'Running'
42  else:
43  d.message = 'Stopped'
44 
45  if (not use_sim_time and c.num_control_loop_overruns > 0):
46  d.message += ' !!! Broke Realtime, used ' + \
47  str(int(c.max_time.to_sec() * 1e6)) + \
48  ' micro seconds in update loop'
49  if c.time_last_control_loop_overrun + rospy.Duration(30.0) > rospy.Time.now():
50  d.level = 1
51 
52  d.values.append(
53  KeyValue('Avg Time in Update Loop (usec)', str(int(c.mean_time.to_sec() * 1e6))))
54  d.values.append(
55  KeyValue('Max Time in update Loop (usec)', str(int(c.max_time.to_sec() * 1e6))))
56  d.values.append(
57  KeyValue('Variance on Time in Update Loop', str(int(c.variance_time.to_sec() * 1e6))))
58  d.values.append(
59  KeyValue('Percent of Cycle Time Used', str(int(c.mean_time.to_sec() / 0.00001))))
60  d.values.append(
61  KeyValue('Number of Control Loop Overruns', str(int(c.num_control_loop_overruns))))
62  d.values.append(
63  KeyValue('Timestamp of Last Control Loop Overrun (sec)', str(int(c.time_last_control_loop_overrun.to_sec()))))
64  return d
65 
66 rospy.init_node('controller_to_diagnostics')
67 use_sim_time = rospy.get_param('use_sim_time', False)
68 pub_diag = rospy.Publisher('/diagnostics', DiagnosticArray)
69 
70 last_publish_time = rospy.Time(0.0)
71 
72 
73 def state_cb(msg):
74  global last_publish_time
75  now = rospy.get_rostime()
76  if (now - last_publish_time).to_sec() > 1.0:
77  if len(msg.controller_statistics) == 0:
78  d = DiagnosticArray()
79  d.header.stamp = msg.header.stamp
80  ds = DiagnosticStatus()
81  ds.name = "Controller: No controllers loaded in controller manager"
82  d.status = [ds]
83  pub_diag.publish(d)
84  else:
85  for c in msg.controller_statistics:
86  d = DiagnosticArray()
87  d.header.stamp = msg.header.stamp
88  d.status = [controller_to_diag(c)]
89  pub_diag.publish(d)
90  last_publish_time = now
91 
92 rospy.Subscriber('mechanism_statistics', MechanismStatistics, state_cb)
93 rospy.spin()


sr_edc_controller_configuration
Author(s): Ugo Cupcic, Toni Oliver
autogenerated on Tue Oct 13 2020 04:01:53