watchdog.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <shared_serial/watchdog.h>
00003 
00004 bool WatchdogThread::set(double interval)
00005 {
00006   if (set_)
00007     return false;
00008 
00009   interval_ = interval;
00010   pthread_mutex_init(&mutex_, NULL);
00011   pthread_create(&thread_, NULL, WatchdogThread::watchDelegate, this);
00012   return set_ = true;
00013 }
00014 
00015 WatchdogThread::~WatchdogThread()
00016 {
00017   if (!set_)
00018     return;
00019 
00020   pthread_cancel(thread_);
00021   pthread_join(thread_, NULL);
00022   pthread_mutex_destroy(&mutex_);
00023 }
00024 
00025 void *WatchdogThread::watchDelegate(void *obj)
00026 {
00027   WatchdogThread *wd = static_cast<WatchdogThread*>(obj);
00028   wd->watch();
00029   return NULL;
00030 }
00031 
00032 void WatchdogThread::watch()
00033 {
00034   kicked_ = true;
00035 
00036   ros::Rate loop_rate(1./interval_);
00037   while (ros::ok())
00038   {
00039     pthread_mutex_lock(&mutex_);
00040     if (!kicked_)
00041       bark();
00042     kicked_ = false;
00043     pthread_mutex_unlock(&mutex_);
00044     loop_rate.sleep();
00045     pthread_testcancel();
00046   }
00047 }
00048 
00049 bool WatchdogThread::kick()
00050 {
00051   if (!set_)
00052     return false;
00053     
00054   pthread_mutex_lock(&mutex_);
00055   kicked_ = true;
00056   pthread_mutex_unlock(&mutex_);
00057   
00058   return true;
00059 }
00060  
00061 void WatchdogThread::bark()
00062 {
00063   ROS_FATAL("Watchdog timed out");
00064   ROS_ISSUE_BREAK();
00065 } 


shared_serial
Author(s): Wouter Caarls
autogenerated on Thu Jun 6 2019 19:47:36