root / mikrokopter / src / keyboard_control.cpp @ 8ecb9700
History | View | Annotate | Download (2.47 KB)
1 | 8ecb9700 | Alex Zirbel | /**
|
---|---|---|---|
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 | |||
14 | #define KEYCODE_R 0x43 |
||
15 | #define KEYCODE_L 0x44 |
||
16 | #define KEYCODE_U 0x41 |
||
17 | #define KEYCODE_D 0x42 |
||
18 | #define KEYCODE_Q 0x71 |
||
19 | |||
20 | // Keyboard control variables
|
||
21 | int kfd = 0; |
||
22 | struct termios cooked, raw;
|
||
23 | |||
24 | class KeyboardControl |
||
25 | { |
||
26 | public:
|
||
27 | KeyboardControl(); |
||
28 | void keyLoop();
|
||
29 | |||
30 | private:
|
||
31 | ros::NodeHandle n; |
||
32 | }; |
||
33 | |||
34 | KeyboardControl::KeyboardControl() |
||
35 | { |
||
36 | } |
||
37 | |||
38 | void quit(int sig) |
||
39 | { |
||
40 | tcsetattr(kfd, TCSANOW, &cooked); |
||
41 | ros::shutdown(); |
||
42 | exit(0);
|
||
43 | } |
||
44 | |||
45 | void KeyboardControl::keyLoop()
|
||
46 | { |
||
47 | char c;
|
||
48 | bool dirty=false; |
||
49 | |||
50 | // get the console in raw mode
|
||
51 | tcgetattr(kfd, &cooked); |
||
52 | memcpy(&raw, &cooked, sizeof(struct termios)); |
||
53 | raw.c_lflag &=~ (ICANON | ECHO); |
||
54 | |||
55 | // Setting a new line, then end of file
|
||
56 | raw.c_cc[VEOL] = 1;
|
||
57 | raw.c_cc[VEOF] = 2;
|
||
58 | tcsetattr(kfd, TCSANOW, &raw); |
||
59 | |||
60 | puts("Reading from keyboard");
|
||
61 | puts("---------------------------");
|
||
62 | puts("Use arrow keys to command the quadrotor.");
|
||
63 | |||
64 | while(true) |
||
65 | { |
||
66 | // get the next event from the keyboard
|
||
67 | if(read(kfd, &c, 1) < 0) |
||
68 | { |
||
69 | perror("read():");
|
||
70 | exit(-1);
|
||
71 | } |
||
72 | |||
73 | //ROS_INFO("key press: 0x%02X\n", c);
|
||
74 | //ROS_INFO("key press: %c\n", c);
|
||
75 | |||
76 | switch(c)
|
||
77 | { |
||
78 | case KEYCODE_L:
|
||
79 | case 'a': |
||
80 | ROS_INFO("W");
|
||
81 | dirty = true;
|
||
82 | break;
|
||
83 | case KEYCODE_R:
|
||
84 | case 'd': |
||
85 | ROS_INFO("E");
|
||
86 | dirty = true;
|
||
87 | break;
|
||
88 | case KEYCODE_U:
|
||
89 | case 'w': |
||
90 | ROS_INFO("N");
|
||
91 | dirty = true;
|
||
92 | break;
|
||
93 | case KEYCODE_D:
|
||
94 | case 's': |
||
95 | ROS_INFO("S");
|
||
96 | dirty = true;
|
||
97 | break;
|
||
98 | case 'r': |
||
99 | ROS_INFO("UP");
|
||
100 | break;
|
||
101 | case 'f': |
||
102 | ROS_INFO("DOWN");
|
||
103 | break;
|
||
104 | } |
||
105 | |||
106 | if(dirty)
|
||
107 | { |
||
108 | dirty=false;
|
||
109 | } |
||
110 | } |
||
111 | |||
112 | return;
|
||
113 | } |
||
114 | |||
115 | int main(int argc, char** argv) |
||
116 | { |
||
117 | ros::init(argc, argv, "keyboard_control");
|
||
118 | KeyboardControl teleop_turtle; |
||
119 | |||
120 | signal(SIGINT,quit); |
||
121 | |||
122 | // This commands loops infinitely
|
||
123 | teleop_turtle.keyLoop(); |
||
124 | |||
125 | return(0); |
||
126 | } |
||
127 |