diag_sim.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2011, Robot Control and Pattern Recognition Group, Warsaw University of Technology
00006 #
00007 # All rights reserved.
00008 # 
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions are met:
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 copyright
00014 #       notice, this list of conditions and the following disclaimer in the
00015 #       documentation and/or other materials provided with the distribution.
00016 #     * Neither the name of the <organization> nor the
00017 #       names of its contributors may be used to endorse or promote products
00018 #       derived from this software without specific prior written permission.
00019 # 
00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00021 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00022 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00023 # DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
00024 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00025 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00026 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00027 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00029 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030 
00031 
00032 import roslib
00033 roslib.load_manifest('diag_sim')
00034 
00035 import rospy
00036 import diagnostic_msgs.msg
00037 
00038 
00039 def diag_sim():    
00040     diag_pub = rospy.Publisher('/diagnostics', diagnostic_msgs.msg.DiagnosticArray)
00041     rospy.init_node('diag_sim')
00042     
00043     cnt = 0
00044     
00045     while not rospy.is_shutdown():
00046         
00047         cnt = cnt + 1
00048         
00049         #Main header
00050         diag = diagnostic_msgs.msg.DiagnosticArray()
00051         diag.header.stamp = rospy.Time.now()
00052 
00053         state = ""
00054         qual = ""
00055         msg = ""
00056         warning = ""
00057         error = ""
00058         power = ""
00059         strategy = ""
00060         lvl = diagnostic_msgs.msg.DiagnosticStatus.OK
00061         
00062         #FRI info                                                                                                                              
00063         stat = diagnostic_msgs.msg.DiagnosticStatus()
00064         stat.name = "FRI state"
00065         
00066     
00067         [div, mod] = divmod(cnt, 4)
00068         if mod == 0:
00069             lvl = diagnostic_msgs.msg.DiagnosticStatus.OK
00070             msg = "Communication quality PERFECT"
00071             qual = "PERFECT"
00072             state = "monitor"
00073             error = "0000000"
00074             warning = "0000000"
00075             power = "1111111"
00076             strategy = "Position"
00077         if mod == 1:
00078             lvl = diagnostic_msgs.msg.DiagnosticStatus.OK
00079             msg = "Communication quality OK"
00080             qual = "OK"
00081             state = "monitor"
00082             error = "0000000"
00083             warning = "0000000"
00084             power = "1110011"
00085             strategy = "Cartesian impedance"
00086         if mod == 2:
00087             lvl = diagnostic_msgs.msg.DiagnosticStatus.WARN
00088             msg = "Communication quality BAD"
00089             qual = "BAD"
00090             state = "command"
00091             error = "0000001"
00092             warning = "0000100"
00093             power = "1111111"
00094             strategy = "Joint impedance"
00095         if mod == 3:
00096             lvl = diagnostic_msgs.msg.DiagnosticStatus.ERROR
00097             msg = "Communication quality UNACCEPTABLE"
00098             qual = "UNACCEPTABLE"
00099             state = "command"
00100             error = "1000000"
00101             warning = "0100000"
00102             power = "1110011"
00103             strategy = "Invalid"
00104             
00105         stat.level = lvl 
00106         stat.message = msg
00107 
00108         stat.values.append(diagnostic_msgs.msg.KeyValue("Kuka System Time", "0"))
00109         stat.values.append(diagnostic_msgs.msg.KeyValue("State",            state))
00110         stat.values.append(diagnostic_msgs.msg.KeyValue("Quality",          qual))
00111         stat.values.append(diagnostic_msgs.msg.KeyValue("Desired Send Sample Time", "3"))
00112         stat.values.append(diagnostic_msgs.msg.KeyValue("Desired Command Sample Time", str(mod*0.025+0.025)))
00113         stat.values.append(diagnostic_msgs.msg.KeyValue("Safety Limits",    "5"))
00114         stat.values.append(diagnostic_msgs.msg.KeyValue("Answer Rate",      "6"))
00115         stat.values.append(diagnostic_msgs.msg.KeyValue("Latency",          "7"))
00116         stat.values.append(diagnostic_msgs.msg.KeyValue("Jitter",           "8"))
00117         stat.values.append(diagnostic_msgs.msg.KeyValue("Average Missed Answer Packages", "9"))
00118         stat.values.append(diagnostic_msgs.msg.KeyValue("Total Missed Packages", "10"))
00119 
00120         #append
00121         diag.status.append(stat)
00122         
00123         
00124         #FRI info                                                                                                                              
00125         stat = diagnostic_msgs.msg.DiagnosticStatus()
00126         stat.name = "robot state"
00127         stat.level = diagnostic_msgs.msg.DiagnosticStatus.OK
00128         stat.message = "OK"
00129     
00130 
00131         stat.values.append(diagnostic_msgs.msg.KeyValue("Power",            power))
00132         stat.values.append(diagnostic_msgs.msg.KeyValue("Control Strategy", strategy))
00133         stat.values.append(diagnostic_msgs.msg.KeyValue("Error",            error))
00134         stat.values.append(diagnostic_msgs.msg.KeyValue("Warning",          warning))
00135 
00136         #append
00137         diag.status.append(stat)
00138         
00139         #publish
00140         diag_pub.publish(diag)
00141         rospy.sleep(1.0)
00142 
00143 
00144 if __name__ == '__main__':
00145     try:
00146         diag_sim()
00147     except rospy.ROSInterruptException: pass
00148     except IOError: pass
00149     except KeyError: pass
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


diag_sim
Author(s): Maciej StefaƄczyk
autogenerated on Thu Nov 14 2013 11:55:00