Statistics
| Branch: | Revision:

root / quad2 / mikrokopter / src / mk_wrapper.cpp @ 98e35b23

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
}