00001
00061 #include <termios.h>
00062 #include <signal.h>
00063 #include <stdio.h>
00064 #include <stdlib.h>
00065
00066 #include "ros/ros.h"
00067
00068 #include <sensor_msgs/Joy.h>
00069
00070
00071
00072
00073 #define KEYCODE_N 0x6E //run
00074 #define KEYCODE_M 0x6D //mode
00075 #define KEYCODE_H 0x68 //help
00076
00077
00078
00079 #define KEYCODE_A 0x61
00080 #define KEYCODE_D 0x64
00081 #define KEYCODE_S 0x73
00082 #define KEYCODE_W 0x77
00083 #define KEYCODE_Q 0x71
00084 #define KEYCODE_E 0x65
00085
00086
00087
00088 #define KEYCODE_1 0x31
00089 #define KEYCODE_2 0x32
00090 #define KEYCODE_3 0x33
00091 #define KEYCODE_4 0x34
00092 #define KEYCODE_5 0x35
00093 #define KEYCODE_6 0x36
00094 #define KEYCODE_7 0x37
00095 #define KEYCODE_8 0x38
00096
00097
00098
00099
00100 #define KEYCODE_R 0x72
00101 #define KEYCODE_T 0x74
00102 #define KEYCODE_Z 0x7A
00103 #define KEYCODE_U 0x75
00104 #define KEYCODE_I 0x69
00105
00106
00107
00108
00109 #define KEYCODE_S 0x73
00110 #define KEYCODE_D 0x64
00111 #define KEYCODE_F 0x66
00112 #define KEYCODE_G 0x67
00113
00114 #define KEYCODE_Y 0x79
00115 #define KEYCODE_X 0x78
00116 #define KEYCODE_C 0x63
00117 #define KEYCODE_V 0x76
00118 #define KEYCODE_B 0x62
00119
00120
00121
00122 const int PUBLISH_FREQ = 5.0;
00123
00124 int kfd = 0;
00125 struct termios cooked, raw;
00126
00127 bool fast_toggle,mode;
00128
00129 void quit(int sig)
00130 {
00131 tcsetattr(kfd, TCSANOW, &cooked);
00132 exit(0);
00133 }
00134
00135
00136 void composeJoyMessage(sensor_msgs::Joy &msg, char c)
00137 {
00138 msg.buttons[5]=0;
00139
00140 if(fast_toggle)
00141 msg.buttons[7]=1;
00142
00143 if(mode)
00144 {
00145 switch(c)
00146 {
00147
00148 case KEYCODE_1:
00149 msg.buttons[0]=1;
00150 msg.axes[4]=1.0;
00151 msg.buttons[5]=1;
00152 break;
00153 case KEYCODE_2:
00154 msg.buttons[0]=1;
00155 msg.axes[5]=1.0;
00156 msg.buttons[5]=1;
00157 break;
00158 case KEYCODE_3:
00159 msg.buttons[1]=1;
00160 msg.axes[4]=1.0;
00161 msg.buttons[5]=1;
00162 break;
00163 case KEYCODE_4:
00164 msg.buttons[1]=1;
00165 msg.axes[5]=1.0;
00166 msg.buttons[5]=1;
00167 break;
00168 case KEYCODE_5:
00169 msg.buttons[2]=1;
00170 msg.axes[4]=1.0;
00171 msg.buttons[5]=1;
00172 break;
00173 case KEYCODE_6:
00174 msg.buttons[2]=1;
00175 msg.axes[5]=1.0;
00176 msg.buttons[5]=1;
00177 break;
00178 case KEYCODE_7:
00179 msg.buttons[3]=1;
00180 msg.axes[4]=1.0;
00181 msg.buttons[5]=1;
00182 break;
00183 case KEYCODE_8:
00184 msg.buttons[3]=1;
00185 msg.axes[5]=1.0;
00186 msg.buttons[5]=1;
00187 break;
00188 case KEYCODE_Y:
00189 msg.buttons[6]=1;
00190 msg.axes[4]=1.0;
00191 msg.buttons[5]=1;
00192 break;
00193 case KEYCODE_S:
00194 msg.buttons[6]=1;
00195 msg.axes[5]=1.0;
00196 msg.buttons[5]=1;
00197 break;
00198 case KEYCODE_C:
00199 msg.buttons[4]=1;
00200 msg.axes[4]=1.0;
00201 msg.buttons[5]=1;
00202 break;
00203 case KEYCODE_F:
00204 msg.buttons[4]=1;
00205 msg.axes[5]=1.0;
00206 msg.buttons[5]=1;
00207 break;
00208
00209
00210
00211
00212
00213
00214
00215 case KEYCODE_Q:
00216 msg.buttons[0]=1;
00217 msg.axes[4]=-1.0;
00218 msg.buttons[5]=1;
00219 break;
00220 case KEYCODE_W:
00221 msg.buttons[0]=1;
00222 msg.axes[5]=-1.0;
00223 msg.buttons[5]=1;
00224 break;
00225 case KEYCODE_E:
00226 msg.buttons[1]=1;
00227 msg.axes[4]=-1.0;
00228 msg.buttons[5]=1;
00229 break;
00230 case KEYCODE_R:
00231 msg.buttons[1]=1;
00232 msg.axes[5]=-1.0;
00233 msg.buttons[5]=1;
00234 break;
00235 case KEYCODE_T:
00236 msg.buttons[2]=1;
00237 msg.axes[4]=-1.0;
00238 msg.buttons[5]=1;
00239 break;
00240 case KEYCODE_Z:
00241 msg.buttons[2]=1;
00242 msg.axes[5]=-1.0;
00243 msg.buttons[5]=1;
00244 break;
00245 case KEYCODE_U:
00246 msg.buttons[3]=1;
00247 msg.axes[4]=-1.0;
00248 msg.buttons[5]=1;
00249 break;
00250 case KEYCODE_I:
00251 msg.buttons[3]=1;
00252 msg.axes[5]=-1.0;
00253 msg.buttons[5]=1;
00254 break;
00255 case KEYCODE_A:
00256 msg.buttons[6]=1;
00257 msg.axes[4]=-1.0;
00258 msg.buttons[5]=1;
00259 break;
00260 case KEYCODE_X:
00261 msg.buttons[6]=1;
00262 msg.axes[5]=-1.0;
00263 msg.buttons[5]=1;
00264 break;
00265 case KEYCODE_D:
00266 msg.buttons[4]=1;
00267 msg.axes[4]=-1.0;
00268 msg.buttons[5]=1;
00269 break;
00270 case KEYCODE_V:
00271 msg.buttons[4]=1;
00272 msg.axes[5]=-1.0;
00273 msg.buttons[5]=1;
00274 break;
00275
00276
00277
00278
00279
00280 }
00281 }
00282 else
00283 {
00284 switch(c)
00285 {
00286 case KEYCODE_W:
00287 msg.axes[1]=1.0;
00288 msg.buttons[5]=1;
00289 break;
00290 case KEYCODE_S:
00291 msg.axes[1]=-1.0;
00292 msg.buttons[5]=1;
00293 break;
00294 case KEYCODE_A:
00295 msg.axes[0]=1.0;
00296 msg.buttons[5]=1;
00297 break;
00298 case KEYCODE_D:
00299 msg.axes[0]=-1.0;
00300 msg.buttons[5]=1;
00301 break;
00302 case KEYCODE_Q:
00303 msg.axes[2]=1.0;
00304 msg.buttons[5]=1;
00305 break;
00306 case KEYCODE_E:
00307 msg.axes[2]=-1.0;
00308 msg.buttons[5]=1;
00309 break;
00310 }
00311 }
00312 }
00313
00314
00315 void showHelp()
00316 {
00317 puts("");
00318 puts("Reading from keyboard");
00319 puts("---------------------------");
00320 puts("Use 'm' to toggle modes (joint/platform)");
00321 puts("Use 'n' to toggle run");
00322 puts("---------------------------");
00323 puts("In platform_mode");
00324 puts("Use 'wasd' to translate");
00325 puts("Use 'qe' to yaw");
00326 puts("---------------------------");
00327 puts("In joint_mode");
00328 puts("Use '1'-'7' and 'q'-'u' for arm");
00329 puts("Use '8' and 'i' for tray");
00330 puts("Use 'a'-'f' and 'y'-'v' for torso");
00331 puts("---------------------------");
00332 puts("Use 'h' to show this help");
00333 puts("Hit 'SPACE' to stop movement");
00334 puts("");
00335 }
00336
00337
00338 int main(int argc, char **argv)
00339 {
00340 ros::init(argc, argv, "keyboard_publisher");
00341 ros::NodeHandle n;
00342
00343 puts("");
00344 puts("Reading from keyboard");
00345 puts("---------------------------");
00346 puts("Use 'm' to toggle modes (joint/platform)");
00347 puts("Use 'n' to toggle run");
00348 puts("---------------------------");
00349 puts("In platform_mode");
00350 puts("Use 'wasd' to translate");
00351 puts("Use 'qe' to yaw");
00352 puts("---------------------------");
00353 puts("In joint_mode");
00354 puts("Use '1'-'7' and 'q'-'u' for arm");
00355 puts("Use '8' and 'i' for tray");
00356 puts("Use 'a'-'f' and 'y'-'v' for torso");
00357 puts("---------------------------");
00358 puts("Use 'h' to show this help");
00359 puts("Hit 'SPACE' to stop movement");
00360 puts("");
00361
00362 signal(SIGINT,quit);
00363
00364
00365 tcgetattr(kfd, &cooked);
00366 memcpy(&raw, &cooked, sizeof(struct termios));
00367 raw.c_lflag &=~ (ICANON | ECHO);
00368
00369 raw.c_cc[VEOL] = 1;
00370 raw.c_cc[VEOF] = 2;
00371
00372
00373 tcsetattr(kfd, TCSANOW, &raw);
00374
00375
00376 fast_toggle=false;
00377 mode=false;
00378
00379 ros::Publisher keyboard_pub = n.advertise<sensor_msgs::Joy>("joy", 1);
00380 char c;
00381
00382 while (ros::ok())
00383 {
00384 ros::spinOnce();
00385
00386 sensor_msgs::Joy msg;
00387 msg.axes.resize(6);
00388 msg.buttons.resize(12);
00389
00390
00391 if(read(kfd, &c, 1) < 0)
00392 {
00393 perror("read():");
00394 exit(-1);
00395 }
00396
00397
00398
00399 switch(c)
00400 {
00401
00402 case KEYCODE_N:
00403 if(fast_toggle)
00404 {
00405 fast_toggle=false;
00406 ROS_INFO("fast_mode: OFF");
00407 }
00408 else
00409 {
00410 fast_toggle=true;
00411 ROS_INFO("fast_mode: ON");
00412 }
00413 break;
00414
00415 case KEYCODE_M:
00416 if(mode)
00417 {
00418 mode=false;
00419 ROS_INFO("Mode: Platform_Mode");
00420 }
00421 else
00422 {
00423 mode=true;
00424 ROS_INFO("Mode: Joint_Mode");
00425 }
00426 break;
00427 case KEYCODE_H:
00428 showHelp();
00429 break;
00430 }
00431
00432
00433 composeJoyMessage(msg, c);
00434
00435 keyboard_pub.publish(msg);
00436 }
00437
00438
00439 return 0;
00440 }
00441
