keyboard_publisher.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9 
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 
18 #include <termios.h>
19 #include <signal.h>
20 #include <stdio.h>
21 #include <stdlib.h>
22 
23 #include "ros/ros.h"
24 
25 #include <sensor_msgs/Joy.h>
26 
27 
28 
29 
30 #define KEYCODE_N 0x6E //run
31 #define KEYCODE_M 0x6D //mode
32 #define KEYCODE_H 0x68 //help
33 
34 
35 //platform
36 #define KEYCODE_A 0x61
37 #define KEYCODE_D 0x64
38 #define KEYCODE_S 0x73
39 #define KEYCODE_W 0x77
40 #define KEYCODE_Q 0x71
41 #define KEYCODE_E 0x65
42 
43 
44 //arm+tray
45 #define KEYCODE_1 0x31
46 #define KEYCODE_2 0x32
47 #define KEYCODE_3 0x33
48 #define KEYCODE_4 0x34
49 #define KEYCODE_5 0x35
50 #define KEYCODE_6 0x36
51 #define KEYCODE_7 0x37
52 #define KEYCODE_8 0x38
53 
54 //#define KEYCODE_Q 0x61
55 //#define KEYCODE_W 0x61
56 //#define KEYCODE_E 0x61
57 #define KEYCODE_R 0x72
58 #define KEYCODE_T 0x74
59 #define KEYCODE_Z 0x7A
60 #define KEYCODE_U 0x75
61 #define KEYCODE_I 0x69
62 
63 
64 //torso+head
65 //#define KEYCODE_A 0x61
66 #define KEYCODE_S 0x73
67 #define KEYCODE_D 0x64
68 #define KEYCODE_F 0x66
69 #define KEYCODE_G 0x67
70 
71 #define KEYCODE_Y 0x79
72 #define KEYCODE_X 0x78
73 #define KEYCODE_C 0x63
74 #define KEYCODE_V 0x76
75 #define KEYCODE_B 0x62
76 
77 
78 
79 const int PUBLISH_FREQ = 5.0;
80 
81 int kfd = 0;
82 struct termios cooked, raw;
83 
84 bool fast_toggle,mode;
85 
86 void quit(int sig)
87 {
88  tcsetattr(kfd, TCSANOW, &cooked);
89  exit(0);
90 }
91 
92 
93 void composeJoyMessage(sensor_msgs::Joy &msg, char c)
94 {
95  msg.buttons[5]=0;
96 
97  if(fast_toggle)
98  msg.buttons[7]=1;
99 
100  if(mode) //joint_mode
101  {
102  switch(c)
103  {
104  //positive
105  case KEYCODE_1: //arm
106  msg.buttons[0]=1;
107  msg.axes[4]=1.0;
108  msg.buttons[5]=1;
109  break;
110  case KEYCODE_2:
111  msg.buttons[0]=1;
112  msg.axes[5]=1.0;
113  msg.buttons[5]=1;
114  break;
115  case KEYCODE_3:
116  msg.buttons[1]=1;
117  msg.axes[4]=1.0;
118  msg.buttons[5]=1;
119  break;
120  case KEYCODE_4:
121  msg.buttons[1]=1;
122  msg.axes[5]=1.0;
123  msg.buttons[5]=1;
124  break;
125  case KEYCODE_5:
126  msg.buttons[2]=1;
127  msg.axes[4]=1.0;
128  msg.buttons[5]=1;
129  break;
130  case KEYCODE_6:
131  msg.buttons[2]=1;
132  msg.axes[5]=1.0;
133  msg.buttons[5]=1;
134  break;
135  case KEYCODE_7:
136  msg.buttons[3]=1;
137  msg.axes[4]=1.0;
138  msg.buttons[5]=1;
139  break;
140  case KEYCODE_8: //tray
141  msg.buttons[3]=1;
142  msg.axes[5]=1.0;
143  msg.buttons[5]=1;
144  break;
145  case KEYCODE_Y: //torso
146  msg.buttons[6]=1;
147  msg.axes[4]=1.0;
148  msg.buttons[5]=1;
149  break;
150  case KEYCODE_S:
151  msg.buttons[6]=1;
152  msg.axes[5]=1.0;
153  msg.buttons[5]=1;
154  break;
155  case KEYCODE_C:
156  msg.buttons[4]=1;
157  msg.axes[4]=1.0;
158  msg.buttons[5]=1;
159  break;
160  case KEYCODE_F:
161  msg.buttons[4]=1;
162  msg.axes[5]=1.0;
163  msg.buttons[5]=1;
164  break;
165  //case KEYCODE_G: //head
166  // msg.buttons[2]=1;
167  // msg.axes[4]=1.0;
168  // msg.buttons[5]=1;
169  // break;
170 
171  //negative
172  case KEYCODE_Q: //arm
173  msg.buttons[0]=1;
174  msg.axes[4]=-1.0;
175  msg.buttons[5]=1;
176  break;
177  case KEYCODE_W:
178  msg.buttons[0]=1;
179  msg.axes[5]=-1.0;
180  msg.buttons[5]=1;
181  break;
182  case KEYCODE_E:
183  msg.buttons[1]=1;
184  msg.axes[4]=-1.0;
185  msg.buttons[5]=1;
186  break;
187  case KEYCODE_R:
188  msg.buttons[1]=1;
189  msg.axes[5]=-1.0;
190  msg.buttons[5]=1;
191  break;
192  case KEYCODE_T:
193  msg.buttons[2]=1;
194  msg.axes[4]=-1.0;
195  msg.buttons[5]=1;
196  break;
197  case KEYCODE_Z:
198  msg.buttons[2]=1;
199  msg.axes[5]=-1.0;
200  msg.buttons[5]=1;
201  break;
202  case KEYCODE_U:
203  msg.buttons[3]=1;
204  msg.axes[4]=-1.0;
205  msg.buttons[5]=1;
206  break;
207  case KEYCODE_I: //tray
208  msg.buttons[3]=1;
209  msg.axes[5]=-1.0;
210  msg.buttons[5]=1;
211  break;
212  case KEYCODE_A: //torso
213  msg.buttons[6]=1;
214  msg.axes[4]=-1.0;
215  msg.buttons[5]=1;
216  break;
217  case KEYCODE_X:
218  msg.buttons[6]=1;
219  msg.axes[5]=-1.0;
220  msg.buttons[5]=1;
221  break;
222  case KEYCODE_D:
223  msg.buttons[4]=1;
224  msg.axes[4]=-1.0;
225  msg.buttons[5]=1;
226  break;
227  case KEYCODE_V:
228  msg.buttons[4]=1;
229  msg.axes[5]=-1.0;
230  msg.buttons[5]=1;
231  break;
232  //case KEYCODE_B: //head
233  // msg.buttons[2]=1;
234  // msg.axes[4]=-1.0;
235  // msg.buttons[5]=1;
236  // break;
237  }
238  }
239  else //platform_mode
240  {
241  switch(c)
242  {
243  case KEYCODE_W:
244  msg.axes[1]=1.0;
245  msg.buttons[5]=1;
246  break;
247  case KEYCODE_S:
248  msg.axes[1]=-1.0;
249  msg.buttons[5]=1;
250  break;
251  case KEYCODE_A:
252  msg.axes[0]=1.0;
253  msg.buttons[5]=1;
254  break;
255  case KEYCODE_D:
256  msg.axes[0]=-1.0;
257  msg.buttons[5]=1;
258  break;
259  case KEYCODE_Q:
260  msg.axes[2]=1.0;
261  msg.buttons[5]=1;
262  break;
263  case KEYCODE_E:
264  msg.axes[2]=-1.0;
265  msg.buttons[5]=1;
266  break;
267  }
268  }
269 }
270 
271 
272 void showHelp()
273 {
274  puts("");
275  puts("Reading from keyboard");
276  puts("---------------------------");
277  puts("Use 'm' to toggle modes (joint/platform)");
278  puts("Use 'n' to toggle run");
279  puts("---------------------------");
280  puts("In platform_mode");
281  puts("Use 'wasd' to translate");
282  puts("Use 'qe' to yaw");
283  puts("---------------------------");
284  puts("In joint_mode");
285  puts("Use '1'-'7' and 'q'-'u' for arm");
286  puts("Use '8' and 'i' for tray");
287  puts("Use 'a'-'f' and 'y'-'v' for torso");
288  puts("---------------------------");
289  puts("Use 'h' to show this help");
290  puts("Hit 'SPACE' to stop movement");
291  puts("");
292 }
293 
294 
295 int main(int argc, char **argv)
296 {
297  ros::init(argc, argv, "keyboard_publisher");
298  ros::NodeHandle n;
299 
300  puts("");
301  puts("Reading from keyboard");
302  puts("---------------------------");
303  puts("Use 'm' to toggle modes (joint/platform)");
304  puts("Use 'n' to toggle run");
305  puts("---------------------------");
306  puts("In platform_mode");
307  puts("Use 'wasd' to translate");
308  puts("Use 'qe' to yaw");
309  puts("---------------------------");
310  puts("In joint_mode");
311  puts("Use '1'-'7' and 'q'-'u' for arm");
312  puts("Use '8' and 'i' for tray");
313  puts("Use 'a'-'f' and 'y'-'v' for torso");
314  puts("---------------------------");
315  puts("Use 'h' to show this help");
316  puts("Hit 'SPACE' to stop movement");
317  puts("");
318 
319  signal(SIGINT,quit);
320 
321  // get the console in raw mode
322  tcgetattr(kfd, &cooked);
323  memcpy(&raw, &cooked, sizeof(struct termios));
324  raw.c_lflag &=~ (ICANON | ECHO);
325  // Setting a new line, then end of file
326  raw.c_cc[VEOL] = 1;
327  raw.c_cc[VEOF] = 2;
328  //raw.c_cc[VMIN] = 0;
329  //raw.c_cc[VTIME] = 0;
330  tcsetattr(kfd, TCSANOW, &raw);
331 
332 
333  fast_toggle=false;
334  mode=false;
335 
336  ros::Publisher keyboard_pub = n.advertise<sensor_msgs::Joy>("joy", 1);
337  char c;
338 
339  while (ros::ok())
340  {
341  ros::spinOnce();
342 
343  sensor_msgs::Joy msg;
344  msg.axes.resize(6);
345  msg.buttons.resize(12);
346 
347  // get the next event from the keyboard
348  if(read(kfd, &c, 1) < 0)
349  {
350  perror("read():");
351  exit(-1);
352  }
353 
354  //ROS_INFO("I got key %d",c);
355 
356  switch(c)
357  {
358  // Fast_Toggle
359  case KEYCODE_N:
360  if(fast_toggle)
361  {
362  fast_toggle=false;
363  ROS_INFO("fast_mode: OFF");
364  }
365  else
366  {
367  fast_toggle=true;
368  ROS_INFO("fast_mode: ON");
369  }
370  break;
371  // Mode_Toggle
372  case KEYCODE_M:
373  if(mode)
374  {
375  mode=false;
376  ROS_INFO("Mode: Platform_Mode");
377  }
378  else
379  {
380  mode=true;
381  ROS_INFO("Mode: Joint_Mode");
382  }
383  break;
384  case KEYCODE_H:
385  showHelp();
386  break;
387  }
388 
389 
390  composeJoyMessage(msg, c);
391 
392  keyboard_pub.publish(msg);
393  }
394 
395 
396  return 0;
397 }
398 
KEYCODE_1
#define KEYCODE_1
Definition: keyboard_publisher.cpp:45
KEYCODE_7
#define KEYCODE_7
Definition: keyboard_publisher.cpp:51
KEYCODE_Z
#define KEYCODE_Z
Definition: keyboard_publisher.cpp:59
kfd
int kfd
Definition: keyboard_publisher.cpp:81
msg
msg
ros::Publisher
showHelp
void showHelp()
Definition: keyboard_publisher.cpp:272
KEYCODE_F
#define KEYCODE_F
Definition: keyboard_publisher.cpp:68
KEYCODE_W
#define KEYCODE_W
Definition: keyboard_publisher.cpp:39
KEYCODE_T
#define KEYCODE_T
Definition: keyboard_publisher.cpp:58
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
composeJoyMessage
void composeJoyMessage(sensor_msgs::Joy &msg, char c)
Definition: keyboard_publisher.cpp:93
ros::spinOnce
ROSCPP_DECL void spinOnce()
mode
bool mode
Definition: keyboard_publisher.cpp:84
KEYCODE_R
#define KEYCODE_R
Definition: keyboard_publisher.cpp:57
KEYCODE_X
#define KEYCODE_X
Definition: keyboard_publisher.cpp:72
KEYCODE_Y
#define KEYCODE_Y
Definition: keyboard_publisher.cpp:71
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
ros::ok
ROSCPP_DECL bool ok()
KEYCODE_C
#define KEYCODE_C
Definition: keyboard_publisher.cpp:73
KEYCODE_2
#define KEYCODE_2
Definition: keyboard_publisher.cpp:46
KEYCODE_8
#define KEYCODE_8
Definition: keyboard_publisher.cpp:52
KEYCODE_V
#define KEYCODE_V
Definition: keyboard_publisher.cpp:74
KEYCODE_3
#define KEYCODE_3
Definition: keyboard_publisher.cpp:47
KEYCODE_H
#define KEYCODE_H
Definition: keyboard_publisher.cpp:32
PUBLISH_FREQ
const int PUBLISH_FREQ
Definition: keyboard_publisher.cpp:79
KEYCODE_E
#define KEYCODE_E
Definition: keyboard_publisher.cpp:41
KEYCODE_S
#define KEYCODE_S
Definition: keyboard_publisher.cpp:66
KEYCODE_I
#define KEYCODE_I
Definition: keyboard_publisher.cpp:61
KEYCODE_Q
#define KEYCODE_Q
Definition: keyboard_publisher.cpp:40
raw
struct termios cooked raw
Definition: keyboard_publisher.cpp:82
fast_toggle
bool fast_toggle
Definition: keyboard_publisher.cpp:84
KEYCODE_6
#define KEYCODE_6
Definition: keyboard_publisher.cpp:50
KEYCODE_U
#define KEYCODE_U
Definition: keyboard_publisher.cpp:60
KEYCODE_M
#define KEYCODE_M
Definition: keyboard_publisher.cpp:31
quit
void quit(int sig)
Definition: keyboard_publisher.cpp:86
main
int main(int argc, char **argv)
Definition: keyboard_publisher.cpp:295
KEYCODE_N
#define KEYCODE_N
Definition: keyboard_publisher.cpp:30
ROS_INFO
#define ROS_INFO(...)
KEYCODE_D
#define KEYCODE_D
Definition: keyboard_publisher.cpp:67
KEYCODE_A
#define KEYCODE_A
Definition: keyboard_publisher.cpp:36
KEYCODE_4
#define KEYCODE_4
Definition: keyboard_publisher.cpp:48
ros::NodeHandle
KEYCODE_5
#define KEYCODE_5
Definition: keyboard_publisher.cpp:49

cob_teleop
Author(s): Florian Weisshardt, Maximilian Sieber
autogenerated on Fri Aug 2 2024 09:45:55