root / scout / scoutsim / src / old_scout_teleop.cpp @ 25694a03
History | View | Annotate | Download (3.07 KB)
1 | e1d66e9a | roboclub | #include <ros/ros.h> |
---|---|---|---|
2 | #include <motors/set_motors.h> |
||
3 | #include <signal.h> |
||
4 | #include <termios.h> |
||
5 | #include <stdio.h> |
||
6 | |||
7 | // TODO: add this to a constants.h
|
||
8 | #define QUEUE_SIZE 10 |
||
9 | |||
10 | #define KEYCODE_R 0x43 |
||
11 | #define KEYCODE_L 0x44 |
||
12 | #define KEYCODE_U 0x41 |
||
13 | #define KEYCODE_D 0x42 |
||
14 | #define KEYCODE_Q 0x71 |
||
15 | #define KEYCODE_S 0x73 |
||
16 | |||
17 | class TeleopScout |
||
18 | { |
||
19 | public:
|
||
20 | TeleopScout(); |
||
21 | void keyLoop();
|
||
22 | |||
23 | private:
|
||
24 | ros::NodeHandle nh_; |
||
25 | bool fl_set_, fr_set_, bl_set_, br_set_;
|
||
26 | int fl_speed_, fr_speed_, bl_speed_, br_speed_;
|
||
27 | char last_key;
|
||
28 | ros::Publisher motors_pub_; |
||
29 | }; |
||
30 | |||
31 | TeleopScout::TeleopScout() |
||
32 | { |
||
33 | fl_set_ = fr_set_ = bl_set_ = br_set_ = true;
|
||
34 | fl_speed_ = fr_speed_ = bl_speed_ = br_speed_ = 0;
|
||
35 | motors_pub_ = nh_.advertise<motors::set_motors>( |
||
36 | "scout1/set_motors", QUEUE_SIZE);
|
||
37 | } |
||
38 | |||
39 | int kfd = 0; |
||
40 | struct termios cooked, raw;
|
||
41 | |||
42 | void quit(int sig) |
||
43 | { |
||
44 | tcsetattr(kfd, TCSANOW, &cooked); |
||
45 | ros::shutdown(); |
||
46 | exit(0);
|
||
47 | } |
||
48 | |||
49 | |||
50 | int main(int argc, char** argv) |
||
51 | { |
||
52 | ros::init(argc, argv, "teleop_scout");
|
||
53 | TeleopScout teleop_scout; |
||
54 | |||
55 | signal(SIGINT,quit); |
||
56 | |||
57 | teleop_scout.keyLoop(); |
||
58 | |||
59 | return(0); |
||
60 | } |
||
61 | |||
62 | |||
63 | void TeleopScout::keyLoop()
|
||
64 | { |
||
65 | char c;
|
||
66 | bool dirty=false; |
||
67 | |||
68 | // get the console in raw mode
|
||
69 | tcgetattr(kfd, &cooked); |
||
70 | memcpy(&raw, &cooked, sizeof(struct termios)); |
||
71 | raw.c_lflag &=~ (ICANON | ECHO); |
||
72 | // Setting a new line, then end of file
|
||
73 | raw.c_cc[VEOL] = 1;
|
||
74 | raw.c_cc[VEOF] = 2;
|
||
75 | tcsetattr(kfd, TCSANOW, &raw); |
||
76 | |||
77 | puts("Reading from keyboard");
|
||
78 | puts("---------------------------");
|
||
79 | puts("Use arrow keys to move the scout.");
|
||
80 | |||
81 | |||
82 | for(;;)
|
||
83 | { |
||
84 | ROS_INFO("loop pass engaged");
|
||
85 | // get the next event from the keyboard
|
||
86 | if(read(kfd, &c, 1) < 0) |
||
87 | { |
||
88 | perror("read():");
|
||
89 | exit(-1);
|
||
90 | } |
||
91 | |||
92 | //ROS_DEBUG("value: 0x%02X\n", c);
|
||
93 | //ROS_INFO("value: 0x%02X\n", c);
|
||
94 | switch(c)
|
||
95 | { |
||
96 | case KEYCODE_L:
|
||
97 | ROS_INFO("LEFT");
|
||
98 | fl_speed_ = bl_speed_ = 200;
|
||
99 | fr_speed_ = br_speed_ = -200;
|
||
100 | dirty = true;
|
||
101 | break;
|
||
102 | case KEYCODE_R:
|
||
103 | ROS_INFO("RIGHT");
|
||
104 | fl_speed_ = bl_speed_ = -200;
|
||
105 | fr_speed_ = br_speed_ = 200;
|
||
106 | dirty = true;
|
||
107 | break;
|
||
108 | case KEYCODE_U:
|
||
109 | ROS_INFO("UP");
|
||
110 | fl_speed_ = bl_speed_ = 200;
|
||
111 | fr_speed_ = br_speed_ = 200;
|
||
112 | dirty = true;
|
||
113 | break;
|
||
114 | case KEYCODE_D:
|
||
115 | ROS_INFO("DOWN");
|
||
116 | fl_speed_ = bl_speed_ = -200;
|
||
117 | fr_speed_ = br_speed_ = -200;
|
||
118 | dirty = true;
|
||
119 | break;
|
||
120 | default:
|
||
121 | ROS_INFO("test");
|
||
122 | fl_speed_ = bl_speed_ = 0;
|
||
123 | fr_speed_ = br_speed_ = 0;
|
||
124 | dirty = true;
|
||
125 | break;
|
||
126 | } |
||
127 | |||
128 | motors::set_motors msg; |
||
129 | msg.fl_set = fl_set_; |
||
130 | msg.fr_set = fr_set_; |
||
131 | msg.bl_set = bl_set_; |
||
132 | msg.br_set = br_set_; |
||
133 | msg.fl_speed = fl_speed_; |
||
134 | msg.fr_speed = fr_speed_; |
||
135 | msg.bl_speed = bl_speed_; |
||
136 | msg.br_speed = br_speed_; |
||
137 | |||
138 | if(dirty == true) |
||
139 | { |
||
140 | if (last_key != c)
|
||
141 | { |
||
142 | motors_pub_.publish(msg); |
||
143 | } |
||
144 | dirty=false;
|
||
145 | } |
||
146 | last_key = c; |
||
147 | } |
||
148 | |||
149 | return;
|
||
150 | } |
||
151 | |||
152 |