root / mikrokopter / src / mk_wrapper.cpp @ 08b4f0df
History | View | Annotate | Download (1.76 KB)
1 | 7a6ed02d | Tom Mullins | #include <ros/ros.h> |
---|---|---|---|
2 | #include <mikrokopter/Control.h> |
||
3 | |||
4 | class MKWrapper |
||
5 | { |
||
6 | public:
|
||
7 | MKWrapper(); |
||
8 | void main_loop();
|
||
9 | void control_callback(const mikrokopter::Control& control); |
||
10 | |||
11 | private:
|
||
12 | ros::Publisher pub; |
||
13 | ros::Subscriber sub; |
||
14 | ros::NodeHandle nh; |
||
15 | ros::Duration max_quiet_time; |
||
16 | ros::Time last_time_recv; |
||
17 | bool started, dead;
|
||
18 | }; |
||
19 | |||
20 | MKWrapper::MKWrapper() |
||
21 | { |
||
22 | pub = nh.advertise<mikrokopter::Control>("/mikrokopter/req_set_control",
|
||
23 | 100);
|
||
24 | sub = nh.subscribe("/mk_wrapper/control", 100, &MKWrapper::control_callback, |
||
25 | this);
|
||
26 | int max_quiet_time_ms;
|
||
27 | bc5901d3 | Tom Mullins | ros::param::param<int>("~max_quiet_time", max_quiet_time_ms, 1000); |
28 | ROS_INFO("Max quiet time: %d", max_quiet_time_ms);
|
||
29 | 7a6ed02d | Tom Mullins | max_quiet_time.fromSec(max_quiet_time_ms / 1000.); |
30 | started = false;
|
||
31 | dead = false;
|
||
32 | } |
||
33 | |||
34 | void MKWrapper::main_loop()
|
||
35 | { |
||
36 | mikrokopter::Control control_dead; |
||
37 | control_dead.pitch = 0;
|
||
38 | control_dead.roll = 0;
|
||
39 | control_dead.yaw = 0;
|
||
40 | 08b4f0df | Tom Mullins | control_dead.thrust = 0;
|
41 | 7a6ed02d | Tom Mullins | |
42 | ros::Rate loop_rate(20);
|
||
43 | while (ros::ok())
|
||
44 | { |
||
45 | if (started)
|
||
46 | { |
||
47 | if (dead)
|
||
48 | { |
||
49 | pub.publish(control_dead); |
||
50 | } |
||
51 | else if (ros::Time::now() - last_time_recv > max_quiet_time) |
||
52 | { |
||
53 | dead = true;
|
||
54 | ROS_ERROR("Lost connection: mk_wrapper must be restarted");
|
||
55 | } |
||
56 | } |
||
57 | loop_rate.sleep(); |
||
58 | ros::spinOnce(); |
||
59 | } |
||
60 | } |
||
61 | |||
62 | void MKWrapper::control_callback(const mikrokopter::Control& control) |
||
63 | { |
||
64 | started = true;
|
||
65 | if (!dead)
|
||
66 | { |
||
67 | pub.publish(control); |
||
68 | last_time_recv = ros::Time::now(); |
||
69 | } |
||
70 | } |
||
71 | |||
72 | int main(int argc, char **argv) |
||
73 | { |
||
74 | ros::init(argc, argv, "mk_wrapper");
|
||
75 | MKWrapper wrapper; |
||
76 | wrapper.main_loop(); |
||
77 | return 0; |
||
78 | } |