cob_teleop_keyboard.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 <math.h>
00021 #include <stdio.h>
00022 #include <stdlib.h>
00023 
00024 #include <ros/ros.h>
00025 #include <geometry_msgs/Twist.h>
00026 
00027 #define KEYCODE_A 0x61
00028 #define KEYCODE_D 0x64
00029 #define KEYCODE_S 0x73
00030 #define KEYCODE_W 0x77
00031 #define KEYCODE_Q 0x71
00032 #define KEYCODE_E 0x65
00033 
00034 #define KEYCODE_A_CAP 0x41
00035 #define KEYCODE_D_CAP 0x44
00036 #define KEYCODE_S_CAP 0x53
00037 #define KEYCODE_W_CAP 0x57
00038 #define KEYCODE_Q_CAP 0x51
00039 #define KEYCODE_E_CAP 0x45
00040 
00041 class TeleopKeyboard
00042 {
00043   private:
00044   double walk_vel, run_vel, yaw_rate, yaw_rate_run;
00045   geometry_msgs::Twist cmd;
00046 
00047   ros::NodeHandle n_;
00048   ros::Publisher vel_pub_;
00049 
00050   public:
00051   void init()
00052   {
00053     cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00054 
00055     vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00056 
00057     ros::NodeHandle n_private("~");
00058     n_private.param("walk_vel", walk_vel, 0.5);
00059     n_private.param("run_vel", run_vel, 1.0);
00060     n_private.param("yaw_rate", yaw_rate, 1.0);
00061     n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
00062 
00063   }
00064 
00065   TeleopKeyboard()
00066   {
00067     walk_vel = 0;
00068     run_vel = 0;
00069     yaw_rate = 0;
00070     yaw_rate_run = 0;
00071   }
00072   ~TeleopKeyboard()   { }
00073   void keyboardLoop();
00074 
00075 };
00076 
00077 int kfd = 0;
00078 struct termios cooked, raw;
00079 
00080 void quit(int sig)
00081 {
00082   tcsetattr(kfd, TCSANOW, &cooked);
00083   exit(0);
00084 }
00085 
00086 int main(int argc, char** argv)
00087 {
00088   ros::init(argc, argv, "teleop_keyboard");
00089 
00090   TeleopKeyboard tpk;
00091   tpk.init();
00092 
00093   signal(SIGINT,quit);
00094 
00095   tpk.keyboardLoop();
00096 
00097   return(0);
00098 }
00099 
00100 void TeleopKeyboard::keyboardLoop()
00101 {
00102   char c;
00103   bool dirty=false;
00104 
00105   // get the console in raw mode
00106   tcgetattr(kfd, &cooked);
00107   memcpy(&raw, &cooked, sizeof(struct termios));
00108   raw.c_lflag &=~ (ICANON | ECHO);
00109   // Setting a new line, then end of file
00110   raw.c_cc[VEOL] = 1;
00111   raw.c_cc[VEOF] = 2;
00112   tcsetattr(kfd, TCSANOW, &raw);
00113 
00114   puts("Reading from keyboard");
00115   puts("---------------------------");
00116   puts("Use 'WASD' to translate");
00117   puts("Use 'QE' to yaw");
00118   puts("Press 'Shift' to run");
00119 
00120 
00121   for(;;)
00122   {
00123     // get the next event from the keyboard
00124     if(read(kfd, &c, 1) < 0)
00125     {
00126       perror("read():");
00127       exit(-1);
00128     }
00129 
00130     cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00131 
00132     switch(c)
00133     {
00134       // Walking
00135     case KEYCODE_W:
00136       cmd.linear.x = walk_vel;
00137       dirty = true;
00138       break;
00139     case KEYCODE_S:
00140       cmd.linear.x = - walk_vel;
00141       dirty = true;
00142       break;
00143     case KEYCODE_A:
00144       cmd.linear.y = walk_vel;
00145       dirty = true;
00146       break;
00147     case KEYCODE_D:
00148       cmd.linear.y = - walk_vel;
00149       dirty = true;
00150       break;
00151     case KEYCODE_Q:
00152       cmd.angular.z = yaw_rate;
00153       dirty = true;
00154       break;
00155     case KEYCODE_E:
00156       cmd.angular.z = - yaw_rate;
00157       dirty = true;
00158       break;
00159 
00160       // Running
00161     case KEYCODE_W_CAP:
00162       cmd.linear.x = run_vel;
00163       dirty = true;
00164       break;
00165     case KEYCODE_S_CAP:
00166       cmd.linear.x = - run_vel;
00167       dirty = true;
00168       break;
00169     case KEYCODE_A_CAP:
00170       cmd.linear.y = run_vel;
00171       dirty = true;
00172       break;
00173     case KEYCODE_D_CAP:
00174       cmd.linear.y = - run_vel;
00175       dirty = true;
00176       break;
00177     case KEYCODE_Q_CAP:
00178       cmd.angular.z = yaw_rate_run;
00179       dirty = true;
00180       break;
00181     case KEYCODE_E_CAP:
00182       cmd.angular.z = - yaw_rate_run;
00183       dirty = true;
00184       break;
00185     }
00186 
00187 
00188     if (dirty == true)
00189     {
00190       vel_pub_.publish(cmd);
00191     }
00192 
00193 
00194   }
00195 }


cob_teleop
Author(s): Florian Weisshardt, Maximilian Sieber
autogenerated on Sun Apr 7 2019 03:08:59