keyboard_publisher.cpp
Go to the documentation of this file.
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 //platform
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 //arm+tray
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 //#define KEYCODE_Q 0x61
00098 //#define KEYCODE_W 0x61
00099 //#define KEYCODE_E 0x61
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 //torso+head
00108 //#define KEYCODE_A 0x61
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)        //joint_mode
00144         {
00145                 switch(c)
00146                 {
00147                 //positive
00148                 case KEYCODE_1: //arm
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: //tray
00184                   msg.buttons[3]=1;
00185                   msg.axes[5]=1.0;
00186                   msg.buttons[5]=1;
00187                   break;
00188                 case KEYCODE_Y: //torso
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                 //case KEYCODE_G:       //head
00209                 //  msg.buttons[2]=1;
00210                 //  msg.axes[4]=1.0;
00211                 //  msg.buttons[5]=1;
00212                 //  break;                
00213                 
00214                 //negative  
00215                 case KEYCODE_Q: //arm
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: //tray
00251                   msg.buttons[3]=1;
00252                   msg.axes[5]=-1.0;
00253                   msg.buttons[5]=1;
00254                   break;
00255                 case KEYCODE_A: //torso
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                 //case KEYCODE_B:       //head
00276                 //  msg.buttons[2]=1;
00277                 //  msg.axes[4]=-1.0;
00278                 //  msg.buttons[5]=1;
00279                 //  break;                                                        
00280                 }
00281         }
00282         else            //platform_mode
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   // get the console in raw mode
00365   tcgetattr(kfd, &cooked);
00366   memcpy(&raw, &cooked, sizeof(struct termios));
00367   raw.c_lflag &=~ (ICANON | ECHO);
00368   // Setting a new line, then end of file
00369   raw.c_cc[VEOL] = 1;
00370   raw.c_cc[VEOF] = 2;
00371   //raw.c_cc[VMIN] = 0;
00372   //raw.c_cc[VTIME] = 0;
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         // get the next event from the keyboard
00391     if(read(kfd, &c, 1) < 0)
00392     {
00393       perror("read():");
00394       exit(-1);
00395     }
00396 
00397         //ROS_INFO("I got key %d",c);
00398         
00399         switch(c)
00400     {
00401       // Fast_Toggle
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       // Mode_Toggle
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 


cob_teleop
Author(s): Florian Weisshardt
autogenerated on Sun Oct 5 2014 23:03:10