$search
00001 /* 00002 * Copyright (c) 2010, Arizona Robotics Research Group, University of Arizona 00003 * Copyright (c) 2008, Willow Garage, Inc. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * * Neither the name of the <ORGANIZATION> nor the names of its 00015 * contributors may be used to endorse or promote products derived from 00016 * this software without specific prior written permission. 00017 * 00018 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 * POSSIBILITY OF SUCH DAMAGE. 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 // get the console in raw mode 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 // get the next event from the keyboard 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