Quadrotor

rosbuild_add_executable(keyboard_control src/keyboard_control.cpp src/nav_lib.cpp)

/**

 * @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>

#define KEYCODE_R 0x43 

#define KEYCODE_L 0x44

#define KEYCODE_U 0x41

#define KEYCODE_D 0x42

#define KEYCODE_Q 0x71

// Keyboard control variables

int kfd = 0;

struct termios cooked, raw;

class KeyboardControl

{

    public:

        KeyboardControl();

        void keyLoop();

    private:

        ros::NodeHandle n;

};

KeyboardControl::KeyboardControl()

{

}

void quit(int sig)

{

    tcsetattr(kfd, TCSANOW, &cooked);

    ros::shutdown();

    exit(0);

}

void KeyboardControl::keyLoop()

{

    char c;

    bool dirty=false;

    // get the console in raw mode

    tcgetattr(kfd, &cooked);

    memcpy(&raw, &cooked, sizeof(struct termios));

    raw.c_lflag &=~ (ICANON | ECHO);

    // Setting a new line, then end of file                         

    raw.c_cc[VEOL] = 1;

    raw.c_cc[VEOF] = 2;

    tcsetattr(kfd, TCSANOW, &raw);

    puts("Reading from keyboard");

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

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

    while(true)

    {

        // get the next event from the keyboard  

        if(read(kfd, &c, 1) < 0)

        {

            perror("read():");

            exit(-1);

        }

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

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

        switch(c)

        {

            case KEYCODE_L:

            case 'a':

                ROS_INFO("W");

                dirty = true;

                break;

            case KEYCODE_R:

            case 'd':

                ROS_INFO("E");

                dirty = true;

                break;

            case KEYCODE_U:

            case 'w':

                ROS_INFO("N");

                dirty = true;

                break;

            case KEYCODE_D:

            case 's':

                ROS_INFO("S");

                dirty = true;

                break;

            case 'r':

                ROS_INFO("UP");

                break;

            case 'f':

                ROS_INFO("DOWN");

                break;

        }

        if(dirty)

        {

            dirty=false;

        }

    }

    return;

}

int main(int argc, char** argv)

{

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

    KeyboardControl teleop_turtle;

    signal(SIGINT,quit);

    // This commands loops infinitely

    teleop_turtle.keyLoop();

    return(0);

}