elektron_teleop_keyboard.cpp
Go to the documentation of this file.
00001 #include <termios.h>
00002 #include <signal.h>
00003 #include <stdio.h>
00004 #include <stdlib.h>
00005 
00006 #include <ros/ros.h>
00007 #include <geometry_msgs/Twist.h>
00008 
00009 #define KEYCODE_A 0x61
00010 #define KEYCODE_D 0x64
00011 #define KEYCODE_S 0x73
00012 #define KEYCODE_W 0x77
00013 
00014 #define KEYCODE_A_CAP 0x41
00015 #define KEYCODE_D_CAP 0x44
00016 #define KEYCODE_S_CAP 0x53
00017 #define KEYCODE_W_CAP 0x57
00018 
00019 #define KEYCODE_SPC 0x20
00020 
00021 class ElektronTeleopKeyboard {
00022 private:
00023         geometry_msgs::Twist vel_;
00024 
00025         ros::NodeHandle nh_;
00026 
00027         double l_scale_, a_scale_, walk_scale_;
00028         ros::Publisher vel_pub_;
00029 
00030 public:
00031         void init() {
00032 
00033                 nh_.param("scale_angular", a_scale_, 1.0);
00034                 nh_.param("scale_linear", l_scale_, 0.23);
00035                 nh_.param("walk_scale", walk_scale_, 0.6);
00036 
00037                 vel_pub_ = nh_.advertise<geometry_msgs::Twist> ("cmd_vel", 1);
00038 
00039                 ros::NodeHandle n_private("~");
00040         }
00041 
00042         void keyboardLoop();
00043 };
00044 
00045 int kfd = 0;
00046 struct termios cooked, raw;
00047 
00048 void quit(int sig) {
00049         tcsetattr(kfd, TCSANOW, &cooked);
00050         exit(0);
00051 }
00052 
00053 int main(int argc, char** argv) {
00054         ros::init(argc, argv, "elektron_teleop_keyboard");
00055 
00056         ElektronTeleopKeyboard tpk;
00057         tpk.init();
00058 
00059         signal(SIGINT, quit);
00060 
00061         tpk.keyboardLoop();
00062 
00063         return (0);
00064 }
00065 
00066 void ElektronTeleopKeyboard::keyboardLoop() {
00067         char c;
00068         bool dirty = false;
00069 
00070         // get the console in raw mode
00071         tcgetattr(kfd, &cooked);
00072         memcpy(&raw, &cooked, sizeof(struct termios));
00073         raw.c_lflag &= ~(ICANON | ECHO);
00074         // Setting a new line, then end of file
00075         raw.c_cc[VEOL] = 1;
00076         raw.c_cc[VEOF] = 2;
00077         tcsetattr(kfd, TCSANOW, &raw);
00078 
00079         puts("Reading from keyboard");
00080         puts("---------------------------");
00081         puts("Use 'WS' to forward/back");
00082         puts("Use 'AD' to turn left/right");
00083         puts("Press 'Shift' to run");
00084         puts("Press 'Space' to stop");
00085 
00086 
00087         for (;;) {
00088                 // get the next event from the keyboard
00089                 if (read(kfd, &c, 1) < 0) {
00090                         perror("read():");
00091                         exit(-1);
00092                 }
00093                 
00094                 vel_.linear.x = 0;
00095                 vel_.angular.z = 0;
00096 
00097                 switch (c) {
00098                 // Walking
00099                 case KEYCODE_W:
00100                         vel_.linear.x = l_scale_ * walk_scale_;
00101                         dirty = true;
00102                         break;
00103                 case KEYCODE_S:
00104                         vel_.linear.x = -l_scale_ * walk_scale_;
00105                         dirty = true;
00106                         break;
00107                 case KEYCODE_A:
00108                         vel_.angular.z = a_scale_ * walk_scale_;
00109                         dirty = true;
00110                         break;
00111                 case KEYCODE_D:
00112                         vel_.angular.z = -a_scale_ * walk_scale_;
00113                         dirty = true;
00114                         break;
00115 
00116                 // Running
00117                 case KEYCODE_W_CAP:
00118                         vel_.linear.x = l_scale_;
00119                         dirty = true;
00120                         break;
00121                 case KEYCODE_S_CAP:
00122                         vel_.linear.x = -l_scale_;
00123                         dirty = true;
00124                         break;
00125                 case KEYCODE_A_CAP:
00126                         vel_.angular.z = a_scale_;
00127                         dirty = true;
00128                         break;
00129                 case KEYCODE_D_CAP:
00130                         vel_.angular.z = -a_scale_;
00131                         dirty = true;
00132                         break;
00133 
00134                 // Stop on space
00135                 case KEYCODE_SPC:
00136                         vel_.linear.x = 0;
00137                         vel_.angular.z = 0;
00138                         dirty = true;
00139                         break;
00140                 }
00141 
00142                 if (dirty == true) {
00143                         vel_pub_.publish(vel_);
00144                 }
00145 
00146         }
00147 }


elektron_teleop
Author(s): Maciej Stefanczyk
autogenerated on Sun Oct 5 2014 23:43:29