source: trunk/shared_serial/src/watchdog.cpp @ 8

Last change on this file since 8 was 8, checked in by wcaarls, 10 years ago

Imported shared_serial at revision 990

File size: 1.1 KB
Line 
1#include <ros/ros.h>
2#include <shared_serial/watchdog.h>
3
4bool WatchdogThread::set(double interval)
5{
6  if (set_)
7    return false;
8
9  interval_ = interval;
10  pthread_mutex_init(&mutex_, NULL);
11  pthread_create(&thread_, NULL, WatchdogThread::watchDelegate, this);
12  return set_ = true;
13}
14
15WatchdogThread::~WatchdogThread()
16{
17  if (!set_)
18    return;
19
20  pthread_cancel(thread_);
21  pthread_join(thread_, NULL);
22  pthread_mutex_destroy(&mutex_);
23}
24
25void *WatchdogThread::watchDelegate(void *obj)
26{
27  WatchdogThread *wd = static_cast<WatchdogThread*>(obj);
28  wd->watch();
29  return NULL;
30}
31
32void WatchdogThread::watch()
33{
34  kicked_ = true;
35
36  ros::Rate loop_rate(1./interval_);
37  while (ros::ok())
38  {
39    pthread_mutex_lock(&mutex_);
40    if (!kicked_)
41      bark();
42    kicked_ = false;
43    pthread_mutex_unlock(&mutex_);
44    loop_rate.sleep();
45    pthread_testcancel();
46  }
47}
48
49bool WatchdogThread::kick()
50{
51  if (!set_)
52    return false;
53   
54  pthread_mutex_lock(&mutex_);
55  kicked_ = true;
56  pthread_mutex_unlock(&mutex_);
57 
58  return true;
59}
60 
61void WatchdogThread::bark()
62{
63  ROS_FATAL("Watchdog timed out");
64  ROS_ISSUE_BREAK();
65}
Note: See TracBrowser for help on using the repository browser.