Quadrotor

#include <ros/ros.h>

#include <mikrokopter/Control.h>

class MKWrapper

{

public:

    MKWrapper();

    void main_loop();

    void control_callback(const mikrokopter::Control& control);

private:

    ros::Publisher pub;

    ros::Subscriber sub;

    ros::NodeHandle nh;

    ros::Duration max_quiet_time;

    ros::Time last_time_recv;

    bool started, dead;

};

MKWrapper::MKWrapper()

{

    pub = nh.advertise<mikrokopter::Control>("/mikrokopter/req_set_control",

        100); 

    sub = nh.subscribe("/mk_wrapper/control", 100, &MKWrapper::control_callback,

        this);

    int max_quiet_time_ms;

    ros::param::param<int>("~max_quiet_time", max_quiet_time_ms, 1000);

    ROS_INFO("Max quiet time: %d", max_quiet_time_ms);

    max_quiet_time.fromSec(max_quiet_time_ms / 1000.);

    started = false;

    dead = false;

}

void MKWrapper::main_loop()

{

    mikrokopter::Control control_dead;

    control_dead.pitch = 0;

    control_dead.roll = 0;

    control_dead.yaw = 0;

    control_dead.thrust = 0;

    ros::Rate loop_rate(20);

    while (ros::ok())

    {

        if (started)

        {

            if (dead)

            {

                pub.publish(control_dead);

            }

            else if (ros::Time::now() - last_time_recv > max_quiet_time)

            {

                dead = true;

                ROS_ERROR("Lost connection: mk_wrapper must be restarted");

            }

        }

        loop_rate.sleep();

        ros::spinOnce();

    }

}

void MKWrapper::control_callback(const mikrokopter::Control& control)

{

    started = true;

    if (!dead)

    {

        pub.publish(control);

        last_time_recv = ros::Time::now();

    }

}

int main(int argc, char **argv)

{

    ros::init(argc, argv, "mk_wrapper");

    MKWrapper wrapper;

    wrapper.main_loop();

    return 0;

}