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


kurt_teleop
Author(s): Jochen Sprickerhof
autogenerated on Tue May 28 2013 15:11:57