Quadrotor

/**

 * @brief A node responsible for keyboard commands which will be sent to

 *        the MikroKopter

 *

 * @author CMU Quadrotor Project

 * @author Alex Zirbel

 */

#include <ros/ros.h>

#include <signal.h>

#include <termios.h>

#include <stdio.h>

#include <ncurses.h>

#include "nav_lib.h"

#define KEYCODE_R 0x43 

#define KEYCODE_L 0x44

#define KEYCODE_U 0x41

#define KEYCODE_D 0x42

#define KEYCODE_Q 0x71

class KeyboardControl

{

    public:

        KeyboardControl();

        void keyLoop();

    private:

        ros::NodeHandle n;

};

KeyboardControl::KeyboardControl()

{

}

void KeyboardControl::keyLoop()

{

    ros::NodeHandle n;

    ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);

    MikrokopterControl control;

    ros::Rate loop_rate(25);

    char c;

    // get the console in raw mode

    initscr();

    timeout(0);

    noecho();

    cbreak();

    puts("Reading from keyboard");

    puts("---------------------------");

    puts("Use arrow keys to command the quadrotor.");

    while(ros::ok())

    {

        // get the next event from the keyboard  

        c = getch();

        //ROS_INFO("key press: 0x%02X\n", c);

        //ROS_INFO("key press: %c\n", c);

        switch(c)

        {

            case KEYCODE_L:

            case 'a':

                ROS_INFO("A");

                control.left();

                break;

            case KEYCODE_R:

            case 'd':

                ROS_INFO("D");

                control.right();

                break;

            case KEYCODE_U:

            case 'w':

                ROS_INFO("W");

                control.forward();

                break;

            case KEYCODE_D:

            case 's':

                ROS_INFO("B");

                control.backward();

                break;

            case ' ':

                ROS_INFO("space");

                control.level();

                break;

            case 'r':

                ROS_INFO("UP");

                //set_throttle(&pub, );

                break;

            case 'f':

                ROS_INFO("DOWN");

                //set_throttle(&pub);

                break;

            default:

                control.level();

        }

        control.publish_on(pub);

        ros::spinOnce();

        loop_rate.sleep();

    }

    endwin();

    return;

}

int main(int argc, char** argv)

{

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

    KeyboardControl teleop_turtle;

    // This commands loops infinitely

    teleop_turtle.keyLoop();

    return(0);

}