root / mikrokopter / mikrokopter / src / mk_wrapper.cpp @ 98711613
History | View | Annotate | Download (1.76 KB)
1 |
#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 |
ros::param::param<int>("~max_quiet_time", max_quiet_time_ms, 1000); |
28 |
ROS_INFO("Max quiet time: %d", max_quiet_time_ms);
|
29 |
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 |
control_dead.thrust = 0;
|
41 |
|
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 |
} |