uos_diffdrive_teleop_key.cpp
Go to the documentation of this file.
00001 /*
00002  *
00003  * Copyright (C) 2015 University of Osnabrück, Germany
00004  * 
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * uos_diffdrive_teleop_key.cpp
00020  *
00021  *  Created on: 16.02.2015
00022  *      Author: Sebastian Pütz <spuetz@uos.de>
00023  */
00024 
00025 #include <uos_diffdrive_teleop_key.h>
00026 
00027 int kfd = 0;
00028 struct termios cooked, raw;
00029 
00030 TeleopKeyboard::TeleopKeyboard(){
00031   ros::NodeHandle n_private("~");
00032   n_private.param("normal_x", normal_x, 0.5);
00033   n_private.param("normal_y", normal_y, 0.5);
00034   n_private.param("high_x", high_x, 1.0);
00035   n_private.param("high_y", high_y, 1.0);
00036   
00037   // get the console in raw mode
00038   tcgetattr(kfd, &cooked);
00039   memcpy(&raw, &cooked, sizeof(struct termios));
00040   raw.c_lflag &=~ (ICANON | ECHO);
00041   // Setting a new line, then end of file
00042   raw.c_cc[VEOL] = 1;
00043   raw.c_cc[VEOF] = 2;
00044   raw.c_cc[VMIN] = 0;
00045   raw.c_cc[VTIME] = 5;
00046   tcsetattr(kfd, TCSANOW, &raw);
00047 
00048   puts("Reading from keyboard");
00049   puts("---------------------------");
00050   puts("Use 'WS' to translate");
00051   puts("Use 'AD' to yaw");
00052   puts("Use 'QE' to translate and yaw");
00053   puts("Press 'Shift' to run");
00054 }
00055 
00056 void TeleopKeyboard::readKeyboard()
00057 {
00058   c = 0;
00059   // get the next event from
00060   // the keyboard
00061   if(read(kfd, &c, 1) < 0)
00062   {
00063     perror("read():");
00064     exit(-1);
00065   }
00066 
00067   in.updated = true;
00068   
00069   switch(c)
00070   {
00071     // Walking
00072     case
00073       KEYCODE_W:
00074       in.forwards = normal_y;
00075       break;
00076     case
00077       KEYCODE_S:
00078       in.forwards = -normal_y;
00079       break;
00080     case
00081       KEYCODE_A:
00082       in.left = normal_x;
00083       break;
00084     case
00085       KEYCODE_D:
00086       in.left = -normal_x;
00087       break;
00088     case
00089       KEYCODE_Q:
00090       in.forwards = normal_y;
00091       in.left = normal_x;
00092       break;
00093     case
00094       KEYCODE_E:
00095       in.forwards = normal_y;
00096       in.left = -normal_x;
00097       break;
00098 
00099     // Running 
00100     case
00101       KEYCODE_W_CAP:
00102       in.forwards = high_y;
00103       break;
00104     case
00105       KEYCODE_S_CAP:
00106       in.forwards = -high_y;
00107       break;
00108     case
00109       KEYCODE_A_CAP:
00110       in.left = high_x;
00111       break;
00112     case
00113       KEYCODE_D_CAP:
00114       in.left = -high_x;
00115       break;
00116     case
00117       KEYCODE_Q_CAP:
00118       in.forwards = high_y;
00119       in.left = high_x;
00120       break;
00121     case
00122       KEYCODE_E_CAP:
00123       in.forwards = high_y;
00124       in.left = -high_x;
00125       break;
00126     default:
00127       in.updated = false;
00128   }
00129 }
00130 
00131 void quit(int sig)
00132 {
00133   tcsetattr(kfd, TCSANOW, &cooked);
00134   exit(0);
00135 }
00136 
00137 int main(int argc, char** argv)
00138 {
00139   ros::init(argc, argv, "uos_diffdrive_teleop_key");
00140   TeleopKeyboard teleop;
00141   signal(SIGINT,quit);
00142   ros::AsyncSpinner spinner(1);
00143   spinner.start();
00144 
00145   while(ros::ok()){
00146     teleop.readKeyboard();
00147   }
00148   return EXIT_SUCCESS;
00149 }


uos_diffdrive_teleop
Author(s): Jochen Sprickerhof, Sebastian Pütz
autogenerated on Wed May 24 2017 03:03:01