Statistics
| Branch: | Revision:

root / mikrokopter / mikrokopter / src / keyboard_control.cpp @ 98711613

History | View | Annotate | Download (2.5 KB)

1
/**
2
 * @brief A node responsible for keyboard commands which will be sent to
3
 *        the MikroKopter
4
 *
5
 * @author CMU Quadrotor Project
6
 * @author Alex Zirbel
7
 */
8

    
9
#include <ros/ros.h>
10
#include <signal.h>
11
#include <termios.h>
12
#include <stdio.h>
13
#include <ncurses.h>
14

    
15
#include "nav_lib.h"
16

    
17
#define KEYCODE_R 0x43 
18
#define KEYCODE_L 0x44
19
#define KEYCODE_U 0x41
20
#define KEYCODE_D 0x42
21
#define KEYCODE_Q 0x71
22

    
23
class KeyboardControl
24
{
25
    public:
26
        KeyboardControl();
27
        void keyLoop();
28

    
29
    private:
30
        ros::NodeHandle n;
31
};
32

    
33
KeyboardControl::KeyboardControl()
34
{
35
}
36

    
37
void KeyboardControl::keyLoop()
38
{
39
    
40
    ros::NodeHandle n;
41
    ros::Publisher pub = n.advertise<mikrokopter::Control>("/mikrokopter/req_set_control", 100);
42

    
43
    MikrokopterControl control;
44

    
45
    ros::Rate loop_rate(25);
46
    
47
    char c;
48

    
49
    // get the console in raw mode
50
    initscr();
51
    timeout(0);
52
    noecho();
53
    cbreak();
54

    
55
    puts("Reading from keyboard");
56
    puts("---------------------------");
57
    puts("Use arrow keys to command the quadrotor.");
58

    
59
    while(ros::ok())
60
    {
61
        // get the next event from the keyboard  
62
        c = getch();
63

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

    
67
        switch(c)
68
        {
69
            case KEYCODE_L:
70
            case 'a':
71
                ROS_INFO("A");
72
                control.left();
73
                break;
74
            case KEYCODE_R:
75
            case 'd':
76
                ROS_INFO("D");
77
                control.right();
78
                break;
79
            case KEYCODE_U:
80
            case 'w':
81
                ROS_INFO("W");
82
                control.forward();
83
                break;
84
            case KEYCODE_D:
85
            case 's':
86
                ROS_INFO("B");
87
                control.backward();
88
                break;
89
            case ' ':
90
                ROS_INFO("space");
91
                control.level();
92
                break;
93
            case 'r':
94
                ROS_INFO("UP");
95
                //set_throttle(&pub, );
96
                break;
97
            case 'f':
98
                ROS_INFO("DOWN");
99
                //set_throttle(&pub);
100
                break;
101
            default:
102
                control.level();
103
        }
104

    
105
        control.publish_on(pub);
106
        ros::spinOnce();
107
        loop_rate.sleep();
108
    }
109
    
110
    endwin();
111
    return;
112
}
113

    
114
int main(int argc, char** argv)
115
{
116
    ros::init(argc, argv, "keyboard_control");
117
    KeyboardControl teleop_turtle;
118

    
119
    // This commands loops infinitely
120
    teleop_turtle.keyLoop();
121

    
122
    return(0);
123
}
124

    
125