root / mikrokopter / src / mk_wrapper.cpp @ 7a6ed02d
History | View | Annotate | Download (1.71 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, 100); |
28 |
max_quiet_time.fromSec(max_quiet_time_ms / 1000.); |
29 |
started = false;
|
30 |
dead = false;
|
31 |
} |
32 |
|
33 |
void MKWrapper::main_loop()
|
34 |
{ |
35 |
mikrokopter::Control control_dead; |
36 |
control_dead.pitch = 0;
|
37 |
control_dead.roll = 0;
|
38 |
control_dead.yaw = 0;
|
39 |
control_dead.thrust = 60;
|
40 |
|
41 |
ros::Rate loop_rate(20);
|
42 |
while (ros::ok())
|
43 |
{ |
44 |
if (started)
|
45 |
{ |
46 |
if (dead)
|
47 |
{ |
48 |
pub.publish(control_dead); |
49 |
} |
50 |
else if (ros::Time::now() - last_time_recv > max_quiet_time) |
51 |
{ |
52 |
dead = true;
|
53 |
ROS_ERROR("Lost connection: mk_wrapper must be restarted");
|
54 |
} |
55 |
} |
56 |
loop_rate.sleep(); |
57 |
ros::spinOnce(); |
58 |
} |
59 |
} |
60 |
|
61 |
void MKWrapper::control_callback(const mikrokopter::Control& control) |
62 |
{ |
63 |
started = true;
|
64 |
if (!dead)
|
65 |
{ |
66 |
pub.publish(control); |
67 |
last_time_recv = ros::Time::now(); |
68 |
} |
69 |
} |
70 |
|
71 |
int main(int argc, char **argv) |
72 |
{ |
73 |
ros::init(argc, argv, "mk_wrapper");
|
74 |
MKWrapper wrapper; |
75 |
wrapper.main_loop(); |
76 |
return 0; |
77 |
} |