fake_diagnostics.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('cob_controller_configuration_gazebo')
00003 
00004 import sys
00005 import rospy
00006 from diagnostic_msgs.msg import DiagnosticArray,DiagnosticStatus
00007 
00008 global last_received_
00009 
00010 def callback(msg):
00011         global last_received_
00012         last_received_ = rospy.Time.now()
00013 
00014 if __name__ == "__main__":
00015         rospy.init_node('fake_diagnostics')
00016 
00017         if not rospy.has_param("~diagnostics_name"):
00018                 rospy.logerr("parameter diagnostics_name not found, shutting down " + rospy.get_name())
00019                 sys.exit()
00020         diagnostics_name = rospy.get_param("~diagnostics_name")
00021 
00022         if not rospy.has_param("~topic_name"):
00023                 rospy.logwarn("parameter topic_name not found. Not listening to any topic for " + rospy.get_name())
00024         topic_name = rospy.get_param("~topic_name",None)
00025 
00026         global last_received_
00027         last_received_ = rospy.Time.now()
00028 
00029         # subscribe to topics
00030         if topic_name != None:
00031                 rospy.Subscriber(topic_name, rospy.AnyMsg, callback)
00032 
00033         pub_diagnostics = rospy.Publisher('/diagnostics', DiagnosticArray)
00034 
00035         rospy.loginfo("fake diagnostics for %s running listening to %s",diagnostics_name, topic_name)
00036         rate = rospy.Rate(1)
00037         while not rospy.is_shutdown():
00038                 # if no topic_name is set, we assume that we received a 
00039                 if topic_name == None:
00040                         last_received_ = rospy.Time.now()
00041 
00042                 # only publish ok if message received recently
00043                 if rospy.Time.now() - last_received_ <= rospy.Duration(10.0):
00044                         status = DiagnosticStatus()
00045                         status.level = 0
00046                         status.name = diagnostics_name
00047                         status.message = diagnostics_name + " running"
00048                         diagnostics = DiagnosticArray()
00049                         diagnostics.status.append(status)
00050                 else:
00051                         status = DiagnosticStatus()
00052                         status.level = 2
00053                         status.name = diagnostics_name
00054                         status.message = "no message received on " + topic_name
00055                         diagnostics = DiagnosticArray()
00056                         diagnostics.status.append(status)
00057                 pub_diagnostics.publish(diagnostics)
00058                 rate.sleep()


cob_controller_configuration_gazebo
Author(s): Florian Weisshardt
autogenerated on Sun Oct 5 2014 23:12:37