Last change
on this file was
49,
checked in by wcaarls, 11 years ago
|
Reinstated rosbuild shared_serial (catkin version is now on github)
|
File size:
1.1 KB
|
Line | |
---|
1 | #include <ros/ros.h> |
---|
2 | #include <shared_serial/watchdog.h> |
---|
3 | |
---|
4 | bool 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 | |
---|
15 | WatchdogThread::~WatchdogThread() |
---|
16 | { |
---|
17 | if (!set_) |
---|
18 | return; |
---|
19 | |
---|
20 | pthread_cancel(thread_); |
---|
21 | pthread_join(thread_, NULL); |
---|
22 | pthread_mutex_destroy(&mutex_); |
---|
23 | } |
---|
24 | |
---|
25 | void *WatchdogThread::watchDelegate(void *obj) |
---|
26 | { |
---|
27 | WatchdogThread *wd = static_cast<WatchdogThread*>(obj); |
---|
28 | wd->watch(); |
---|
29 | return NULL; |
---|
30 | } |
---|
31 | |
---|
32 | void 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 | |
---|
49 | bool 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 | |
---|
61 | void WatchdogThread::bark() |
---|
62 | { |
---|
63 | ROS_FATAL("Watchdog timed out"); |
---|
64 | ROS_ISSUE_BREAK(); |
---|
65 | } |
---|
Note: See
TracBrowser
for help on using the repository browser.