keyboard.cpp
Go to the documentation of this file.
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 


erratic_teleop
Author(s): Antons Rebguns
autogenerated on Mon Sep 14 2015 13:46:07