Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #include <termios.h>
00032 #include <signal.h>
00033 #include <math.h>
00034 #include <stdio.h>
00035 #include <stdlib.h>
00036 #include <sys/poll.h>
00037
00038 #include <boost/thread/thread.hpp>
00039 #include <ros/ros.h>
00040 #include <geometry_msgs/Twist.h>
00041
00042 #define KEYCODE_W 0x77
00043 #define KEYCODE_A 0x61
00044 #define KEYCODE_S 0x73
00045 #define KEYCODE_D 0x64
00046
00047 #define KEYCODE_A_CAP 0x41
00048 #define KEYCODE_D_CAP 0x44
00049 #define KEYCODE_S_CAP 0x53
00050 #define KEYCODE_W_CAP 0x57
00051
00052 class ErraticKeyboardTeleopNode
00053 {
00054 private:
00055 double walk_vel_;
00056 double run_vel_;
00057 double yaw_rate_;
00058 double yaw_rate_run_;
00059
00060 geometry_msgs::Twist cmdvel_;
00061 ros::NodeHandle n_;
00062 ros::Publisher pub_;
00063
00064 public:
00065 ErraticKeyboardTeleopNode()
00066 {
00067 pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00068
00069 ros::NodeHandle n_private("~");
00070 n_private.param("walk_vel", walk_vel_, 0.5);
00071 n_private.param("run_vel", run_vel_, 1.0);
00072 n_private.param("yaw_rate", yaw_rate_, 1.0);
00073 n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
00074 }
00075
00076 ~ErraticKeyboardTeleopNode() { }
00077 void keyboardLoop();
00078
00079 void stopRobot()
00080 {
00081 cmdvel_.linear.x = 0.0;
00082 cmdvel_.angular.z = 0.0;
00083 pub_.publish(cmdvel_);
00084 }
00085 };
00086
00087 ErraticKeyboardTeleopNode* tbk;
00088 int kfd = 0;
00089 struct termios cooked, raw;
00090 bool done;
00091
00092 int main(int argc, char** argv)
00093 {
00094 ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
00095 ErraticKeyboardTeleopNode tbk;
00096
00097 boost::thread t = boost::thread(boost::bind(&ErraticKeyboardTeleopNode::keyboardLoop, &tbk));
00098
00099 ros::spin();
00100
00101 t.interrupt();
00102 t.join();
00103 tbk.stopRobot();
00104 tcsetattr(kfd, TCSANOW, &cooked);
00105
00106 return(0);
00107 }
00108
00109 void ErraticKeyboardTeleopNode::keyboardLoop()
00110 {
00111 char c;
00112 double max_tv = walk_vel_;
00113 double max_rv = yaw_rate_;
00114 bool dirty = false;
00115 int speed = 0;
00116 int turn = 0;
00117
00118
00119 tcgetattr(kfd, &cooked);
00120 memcpy(&raw, &cooked, sizeof(struct termios));
00121 raw.c_lflag &=~ (ICANON | ECHO);
00122 raw.c_cc[VEOL] = 1;
00123 raw.c_cc[VEOF] = 2;
00124 tcsetattr(kfd, TCSANOW, &raw);
00125
00126 puts("Reading from keyboard");
00127 puts("Use WASD keys to control the robot");
00128 puts("Press Shift to move faster");
00129
00130 struct pollfd ufd;
00131 ufd.fd = kfd;
00132 ufd.events = POLLIN;
00133
00134 for(;;)
00135 {
00136 boost::this_thread::interruption_point();
00137
00138
00139 int num;
00140
00141 if ((num = poll(&ufd, 1, 250)) < 0)
00142 {
00143 perror("poll():");
00144 return;
00145 }
00146 else if(num > 0)
00147 {
00148 if(read(kfd, &c, 1) < 0)
00149 {
00150 perror("read():");
00151 return;
00152 }
00153 }
00154 else
00155 {
00156 if (dirty == true)
00157 {
00158 stopRobot();
00159 dirty = false;
00160 }
00161
00162 continue;
00163 }
00164
00165 switch(c)
00166 {
00167 case KEYCODE_W:
00168 max_tv = walk_vel_;
00169 speed = 1;
00170 turn = 0;
00171 dirty = true;
00172 break;
00173 case KEYCODE_S:
00174 max_tv = walk_vel_;
00175 speed = -1;
00176 turn = 0;
00177 dirty = true;
00178 break;
00179 case KEYCODE_A:
00180 max_rv = yaw_rate_;
00181 speed = 0;
00182 turn = 1;
00183 dirty = true;
00184 break;
00185 case KEYCODE_D:
00186 max_rv = yaw_rate_;
00187 speed = 0;
00188 turn = -1;
00189 dirty = true;
00190 break;
00191
00192 case KEYCODE_W_CAP:
00193 max_tv = run_vel_;
00194 speed = 1;
00195 turn = 0;
00196 dirty = true;
00197 break;
00198 case KEYCODE_S_CAP:
00199 max_tv = run_vel_;
00200 speed = -1;
00201 turn = 0;
00202 dirty = true;
00203 break;
00204 case KEYCODE_A_CAP:
00205 max_rv = yaw_rate_run_;
00206 speed = 0;
00207 turn = 1;
00208 dirty = true;
00209 break;
00210 case KEYCODE_D_CAP:
00211 max_rv = yaw_rate_run_;
00212 speed = 0;
00213 turn = -1;
00214 dirty = true;
00215 break;
00216
00217 default:
00218 max_tv = walk_vel_;
00219 max_rv = yaw_rate_;
00220 speed = 0;
00221 turn = 0;
00222 dirty = false;
00223 }
00224
00225 cmdvel_.linear.x = speed * max_tv;
00226 cmdvel_.angular.z = turn * max_rv;
00227 pub_.publish(cmdvel_);
00228 }
00229 }
00230