keyboard_publisher.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #include <termios.h>
00019 #include <signal.h>
00020 #include <stdio.h>
00021 #include <stdlib.h>
00022 
00023 #include "ros/ros.h"
00024 
00025 #include <sensor_msgs/Joy.h>
00026 
00027 
00028 
00029 
00030 #define KEYCODE_N 0x6E  //run
00031 #define KEYCODE_M 0x6D  //mode
00032 #define KEYCODE_H 0x68  //help
00033 
00034 
00035 //platform
00036 #define KEYCODE_A 0x61
00037 #define KEYCODE_D 0x64
00038 #define KEYCODE_S 0x73
00039 #define KEYCODE_W 0x77
00040 #define KEYCODE_Q 0x71
00041 #define KEYCODE_E 0x65
00042 
00043 
00044 //arm+tray
00045 #define KEYCODE_1 0x31
00046 #define KEYCODE_2 0x32
00047 #define KEYCODE_3 0x33
00048 #define KEYCODE_4 0x34
00049 #define KEYCODE_5 0x35
00050 #define KEYCODE_6 0x36
00051 #define KEYCODE_7 0x37
00052 #define KEYCODE_8 0x38
00053 
00054 //#define KEYCODE_Q 0x61
00055 //#define KEYCODE_W 0x61
00056 //#define KEYCODE_E 0x61
00057 #define KEYCODE_R 0x72
00058 #define KEYCODE_T 0x74
00059 #define KEYCODE_Z 0x7A
00060 #define KEYCODE_U 0x75
00061 #define KEYCODE_I 0x69
00062 
00063 
00064 //torso+head
00065 //#define KEYCODE_A 0x61
00066 #define KEYCODE_S 0x73
00067 #define KEYCODE_D 0x64
00068 #define KEYCODE_F 0x66
00069 #define KEYCODE_G 0x67
00070 
00071 #define KEYCODE_Y 0x79
00072 #define KEYCODE_X 0x78
00073 #define KEYCODE_C 0x63
00074 #define KEYCODE_V 0x76
00075 #define KEYCODE_B 0x62
00076 
00077 
00078 
00079 const int PUBLISH_FREQ = 5.0;
00080 
00081 int kfd = 0;
00082 struct termios cooked, raw;
00083 
00084 bool fast_toggle,mode;
00085 
00086 void quit(int sig)
00087 {
00088   tcsetattr(kfd, TCSANOW, &cooked);
00089   exit(0);
00090 }
00091 
00092 
00093 void composeJoyMessage(sensor_msgs::Joy &msg, char c)
00094 {
00095   msg.buttons[5]=0;
00096 
00097   if(fast_toggle)
00098     msg.buttons[7]=1;
00099 
00100   if(mode) //joint_mode
00101   {
00102     switch(c)
00103     {
00104     //positive
00105     case KEYCODE_1: //arm
00106       msg.buttons[0]=1;
00107       msg.axes[4]=1.0;
00108       msg.buttons[5]=1;
00109       break;
00110     case KEYCODE_2:
00111       msg.buttons[0]=1;
00112       msg.axes[5]=1.0;
00113       msg.buttons[5]=1;
00114       break;
00115     case KEYCODE_3:
00116       msg.buttons[1]=1;
00117       msg.axes[4]=1.0;
00118       msg.buttons[5]=1;
00119       break;
00120     case KEYCODE_4:
00121       msg.buttons[1]=1;
00122       msg.axes[5]=1.0;
00123       msg.buttons[5]=1;
00124       break;
00125     case KEYCODE_5:
00126       msg.buttons[2]=1;
00127       msg.axes[4]=1.0;
00128       msg.buttons[5]=1;
00129       break;
00130     case KEYCODE_6:
00131       msg.buttons[2]=1;
00132       msg.axes[5]=1.0;
00133       msg.buttons[5]=1;
00134       break;
00135     case KEYCODE_7:
00136       msg.buttons[3]=1;
00137       msg.axes[4]=1.0;
00138       msg.buttons[5]=1;
00139       break;
00140     case KEYCODE_8: //tray
00141       msg.buttons[3]=1;
00142       msg.axes[5]=1.0;
00143       msg.buttons[5]=1;
00144       break;
00145     case KEYCODE_Y: //torso
00146       msg.buttons[6]=1;
00147       msg.axes[4]=1.0;
00148       msg.buttons[5]=1;
00149       break;
00150     case KEYCODE_S:
00151       msg.buttons[6]=1;
00152       msg.axes[5]=1.0;
00153       msg.buttons[5]=1;
00154       break;
00155     case KEYCODE_C:
00156       msg.buttons[4]=1;
00157       msg.axes[4]=1.0;
00158       msg.buttons[5]=1;
00159       break;
00160     case KEYCODE_F:
00161       msg.buttons[4]=1;
00162       msg.axes[5]=1.0;
00163       msg.buttons[5]=1;
00164       break;
00165     //case KEYCODE_G: //head
00166     //  msg.buttons[2]=1;
00167     //  msg.axes[4]=1.0;
00168     //  msg.buttons[5]=1;
00169     //  break;
00170 
00171     //negative
00172     case KEYCODE_Q: //arm
00173       msg.buttons[0]=1;
00174       msg.axes[4]=-1.0;
00175       msg.buttons[5]=1;
00176       break;
00177     case KEYCODE_W:
00178       msg.buttons[0]=1;
00179       msg.axes[5]=-1.0;
00180       msg.buttons[5]=1;
00181       break;
00182     case KEYCODE_E:
00183       msg.buttons[1]=1;
00184       msg.axes[4]=-1.0;
00185       msg.buttons[5]=1;
00186       break;
00187     case KEYCODE_R:
00188       msg.buttons[1]=1;
00189       msg.axes[5]=-1.0;
00190       msg.buttons[5]=1;
00191       break;
00192     case KEYCODE_T:
00193       msg.buttons[2]=1;
00194       msg.axes[4]=-1.0;
00195       msg.buttons[5]=1;
00196       break;
00197     case KEYCODE_Z:
00198       msg.buttons[2]=1;
00199       msg.axes[5]=-1.0;
00200       msg.buttons[5]=1;
00201       break;
00202     case KEYCODE_U:
00203       msg.buttons[3]=1;
00204       msg.axes[4]=-1.0;
00205       msg.buttons[5]=1;
00206       break;
00207     case KEYCODE_I: //tray
00208       msg.buttons[3]=1;
00209       msg.axes[5]=-1.0;
00210       msg.buttons[5]=1;
00211       break;
00212     case KEYCODE_A: //torso
00213       msg.buttons[6]=1;
00214       msg.axes[4]=-1.0;
00215       msg.buttons[5]=1;
00216       break;
00217     case KEYCODE_X:
00218       msg.buttons[6]=1;
00219       msg.axes[5]=-1.0;
00220       msg.buttons[5]=1;
00221       break;
00222     case KEYCODE_D:
00223       msg.buttons[4]=1;
00224       msg.axes[4]=-1.0;
00225       msg.buttons[5]=1;
00226       break;
00227     case KEYCODE_V:
00228       msg.buttons[4]=1;
00229       msg.axes[5]=-1.0;
00230       msg.buttons[5]=1;
00231       break;
00232     //case KEYCODE_B: //head
00233     //  msg.buttons[2]=1;
00234     //  msg.axes[4]=-1.0;
00235     //  msg.buttons[5]=1;
00236     //  break;
00237     }
00238   }
00239   else //platform_mode
00240   {
00241     switch(c)
00242     {
00243     case KEYCODE_W:
00244       msg.axes[1]=1.0;
00245       msg.buttons[5]=1;
00246       break;
00247     case KEYCODE_S:
00248       msg.axes[1]=-1.0;
00249       msg.buttons[5]=1;
00250       break;
00251     case KEYCODE_A:
00252       msg.axes[0]=1.0;
00253       msg.buttons[5]=1;
00254       break;
00255     case KEYCODE_D:
00256       msg.axes[0]=-1.0;
00257       msg.buttons[5]=1;
00258       break;
00259     case KEYCODE_Q:
00260       msg.axes[2]=1.0;
00261       msg.buttons[5]=1;
00262       break;
00263     case KEYCODE_E:
00264       msg.axes[2]=-1.0;
00265       msg.buttons[5]=1;
00266       break;
00267     }
00268   }
00269 }
00270 
00271 
00272 void showHelp()
00273 {
00274     puts("");
00275     puts("Reading from keyboard");
00276     puts("---------------------------");
00277     puts("Use 'm' to toggle modes (joint/platform)");
00278     puts("Use 'n' to toggle run");
00279     puts("---------------------------");
00280     puts("In platform_mode");
00281     puts("Use 'wasd' to translate");
00282     puts("Use 'qe' to yaw");
00283     puts("---------------------------");
00284     puts("In joint_mode");
00285     puts("Use '1'-'7' and 'q'-'u' for arm");
00286     puts("Use '8' and 'i' for tray");
00287     puts("Use 'a'-'f' and 'y'-'v' for torso");
00288     puts("---------------------------");
00289     puts("Use 'h' to show this help");
00290     puts("Hit 'SPACE' to stop movement");
00291     puts("");
00292 }
00293 
00294 
00295 int main(int argc, char **argv)
00296 {
00297   ros::init(argc, argv, "keyboard_publisher");
00298   ros::NodeHandle n;
00299 
00300     puts("");
00301     puts("Reading from keyboard");
00302     puts("---------------------------");
00303     puts("Use 'm' to toggle modes (joint/platform)");
00304     puts("Use 'n' to toggle run");
00305     puts("---------------------------");
00306     puts("In platform_mode");
00307     puts("Use 'wasd' to translate");
00308     puts("Use 'qe' to yaw");
00309     puts("---------------------------");
00310     puts("In joint_mode");
00311     puts("Use '1'-'7' and 'q'-'u' for arm");
00312     puts("Use '8' and 'i' for tray");
00313     puts("Use 'a'-'f' and 'y'-'v' for torso");
00314     puts("---------------------------");
00315     puts("Use 'h' to show this help");
00316     puts("Hit 'SPACE' to stop movement");
00317     puts("");
00318 
00319   signal(SIGINT,quit);
00320 
00321   // get the console in raw mode
00322   tcgetattr(kfd, &cooked);
00323   memcpy(&raw, &cooked, sizeof(struct termios));
00324   raw.c_lflag &=~ (ICANON | ECHO);
00325   // Setting a new line, then end of file
00326   raw.c_cc[VEOL] = 1;
00327   raw.c_cc[VEOF] = 2;
00328   //raw.c_cc[VMIN] = 0;
00329   //raw.c_cc[VTIME] = 0;
00330   tcsetattr(kfd, TCSANOW, &raw);
00331 
00332 
00333   fast_toggle=false;
00334   mode=false;
00335 
00336   ros::Publisher keyboard_pub = n.advertise<sensor_msgs::Joy>("joy", 1);
00337   char c;
00338 
00339   while (ros::ok())
00340   {
00341     ros::spinOnce();
00342 
00343     sensor_msgs::Joy msg;
00344   msg.axes.resize(6);
00345   msg.buttons.resize(12);
00346 
00347     // get the next event from the keyboard
00348     if(read(kfd, &c, 1) < 0)
00349     {
00350       perror("read():");
00351       exit(-1);
00352     }
00353 
00354   //ROS_INFO("I got key %d",c);
00355 
00356   switch(c)
00357     {
00358       // Fast_Toggle
00359     case KEYCODE_N:
00360     if(fast_toggle)
00361     {
00362       fast_toggle=false;
00363       ROS_INFO("fast_mode: OFF");
00364     }
00365     else
00366     {
00367       fast_toggle=true;
00368       ROS_INFO("fast_mode: ON");
00369     }
00370       break;
00371       // Mode_Toggle
00372     case KEYCODE_M:
00373       if(mode)
00374       {
00375         mode=false;
00376         ROS_INFO("Mode: Platform_Mode");
00377       }
00378       else
00379       {
00380         mode=true;
00381         ROS_INFO("Mode: Joint_Mode");
00382       }
00383     break;
00384   case KEYCODE_H:
00385     showHelp();
00386     break;
00387   }
00388 
00389 
00390   composeJoyMessage(msg, c);
00391 
00392     keyboard_pub.publish(msg);
00393   }
00394 
00395 
00396   return 0;
00397 }
00398 


cob_teleop
Author(s): Florian Weisshardt, Maximilian Sieber
autogenerated on Sun Jun 9 2019 20:20:22