Go to the documentation of this file.00001 __author__ = 'caja'
00002 import rospy
00003 import time
00004 import shlex
00005 import subprocess
00006 from threading import Thread
00007 from BAL.Interfaces.Runnable import Runnable
00008
00009 TIME_OUT = 5000
00010
00011
00012 class KeepAliveHandler(Runnable):
00013 is_init = False
00014
00015 def __init__(self, topic_name, msg_type):
00016 if not KeepAliveHandler.is_init:
00017 KeepAliveHandler.is_init = True
00018 self._watch_dog_time = int(round(time.time() * 1000))
00019 rospy.Subscriber(topic_name, msg_type, self.callback_to_watch)
00020 Thread(target=self.run, args=()).start()
00021 else: pass
00022
00023 def run(self):
00024 rate = rospy.Rate(50)
00025 send_err = False
00026 while not rospy.is_shutdown() and not send_err:
00027 if (int(round(time.time() * 1000)) - self._watch_dog_time) > TIME_OUT:
00028 rospy.logerr("RiC Board is not responding")
00029 subprocess.Popen(shlex.split("pkill -f ros"))
00030 send_err = True
00031 rate.sleep()
00032
00033 def callback_to_watch(self, msg):
00034 self._watch_dog_time = int(round(time.time() * 1000))