teleop_pr2_keyboard.cpp
Go to the documentation of this file.
1 /*
2  * teleop_pr2_keyboard
3  * Copyright (c) 2008, Willow Garage, Inc.
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the <ORGANIZATION> nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 // Author: Kevin Watts
32 
33 #include <termios.h>
34 #include <signal.h>
35 #include <math.h>
36 #include <stdio.h>
37 #include <stdlib.h>
38 
39 #include <ros/ros.h>
40 #include <geometry_msgs/Twist.h>
41 
42 #define KEYCODE_A 0x61
43 #define KEYCODE_D 0x64
44 #define KEYCODE_S 0x73
45 #define KEYCODE_W 0x77
46 #define KEYCODE_Q 0x71
47 #define KEYCODE_E 0x65
48 
49 #define KEYCODE_A_CAP 0x41
50 #define KEYCODE_D_CAP 0x44
51 #define KEYCODE_S_CAP 0x53
52 #define KEYCODE_W_CAP 0x57
53 #define KEYCODE_Q_CAP 0x51
54 #define KEYCODE_E_CAP 0x45
55 
57 {
58  private:
60  geometry_msgs::Twist cmd;
61 
64 
65  public:
66  void init()
67  {
68  cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
69 
70  vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
71 
72  ros::NodeHandle n_private("~");
73  n_private.param("walk_vel", walk_vel, 0.5);
74  n_private.param("run_vel", run_vel, 1.0);
75  n_private.param("yaw_rate", yaw_rate, 1.0);
76  n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
77 
78  }
79 
81  void keyboardLoop();
82 
83 };
84 
85 int kfd = 0;
86 struct termios cooked, raw;
87 
88 void quit(int sig)
89 {
90  tcsetattr(kfd, TCSANOW, &cooked);
91  exit(0);
92 }
93 
94 int main(int argc, char** argv)
95 {
96  ros::init(argc, argv, "pr2_base_keyboard");
97 
99  tpk.init();
100 
101  signal(SIGINT,quit);
102 
103  tpk.keyboardLoop();
104 
105  return(0);
106 }
107 
109 {
110  char c;
111  bool dirty=false;
112 
113  // get the console in raw mode
114  tcgetattr(kfd, &cooked);
115  memcpy(&raw, &cooked, sizeof(struct termios));
116  raw.c_lflag &=~ (ICANON | ECHO);
117  // Setting a new line, then end of file
118  raw.c_cc[VEOL] = 1;
119  raw.c_cc[VEOF] = 2;
120  tcsetattr(kfd, TCSANOW, &raw);
121 
122  puts("Reading from keyboard");
123  puts("---------------------------");
124  puts("Use 'WASD' to translate");
125  puts("Use 'QE' to yaw");
126  puts("Press 'Shift' to run");
127 
128 
129  for(;;)
130  {
131  // get the next event from the keyboard
132  if(read(kfd, &c, 1) < 0)
133  {
134  perror("read():");
135  exit(-1);
136  }
137 
138  cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
139 
140  switch(c)
141  {
142  // Walking
143  case KEYCODE_W:
144  cmd.linear.x = walk_vel;
145  dirty = true;
146  break;
147  case KEYCODE_S:
148  cmd.linear.x = - walk_vel;
149  dirty = true;
150  break;
151  case KEYCODE_A:
152  cmd.linear.y = walk_vel;
153  dirty = true;
154  break;
155  case KEYCODE_D:
156  cmd.linear.y = - walk_vel;
157  dirty = true;
158  break;
159  case KEYCODE_Q:
160  cmd.angular.z = yaw_rate;
161  dirty = true;
162  break;
163  case KEYCODE_E:
164  cmd.angular.z = - yaw_rate;
165  dirty = true;
166  break;
167 
168  // Running
169  case KEYCODE_W_CAP:
170  cmd.linear.x = run_vel;
171  dirty = true;
172  break;
173  case KEYCODE_S_CAP:
174  cmd.linear.x = - run_vel;
175  dirty = true;
176  break;
177  case KEYCODE_A_CAP:
178  cmd.linear.y = run_vel;
179  dirty = true;
180  break;
181  case KEYCODE_D_CAP:
182  cmd.linear.y = - run_vel;
183  dirty = true;
184  break;
185  case KEYCODE_Q_CAP:
186  cmd.angular.z = yaw_rate_run;
187  dirty = true;
188  break;
189  case KEYCODE_E_CAP:
190  cmd.angular.z = - yaw_rate_run;
191  dirty = true;
192  break;
193  }
194 
195 
196  if (dirty == true)
197  {
199  }
200 
201 
202  }
203 }
#define KEYCODE_D
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
void quit(int sig)
struct termios cooked raw
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define KEYCODE_S
#define KEYCODE_E_CAP
#define KEYCODE_Q_CAP
#define KEYCODE_W
#define KEYCODE_A
#define KEYCODE_E
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
geometry_msgs::Twist cmd
#define KEYCODE_S_CAP
#define KEYCODE_Q
#define KEYCODE_W_CAP
#define KEYCODE_A_CAP
#define KEYCODE_D_CAP


pr2_teleop
Author(s):
autogenerated on Thu Jun 6 2019 19:18:47