pr2_teleop_general_keyboard.cpp
Go to the documentation of this file.
1 /*
2  * pr2_teleop_general_keyboard
3  * Copyright (c) 2010, 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: E. Gil Jones
32 
33 #include <termios.h>
34 #include <signal.h>
35 #include <cstdio>
36 #include <cstdlib>
37 #include <unistd.h>
38 #include <math.h>
39 
40 #include <ros/ros.h>
41 
42 #include "pr2_teleop_general/pr2_teleop_general_commander.h"
43 
44 int kfd = 0;
45 struct termios cooked, raw;
46 
47 class Pr2TeleopGeneralKeyboard
48 {
49 public:
50  Pr2TeleopGeneralKeyboard()
51  {
52  gc = NULL;
53  }
54 
55  ~Pr2TeleopGeneralKeyboard() {
56  if(gc != NULL) {
57  delete gc;
58  }
59  }
60 
61  void init()
62  {
63  ros::NodeHandle n_local("~");
64 
65  // Head pan/tilt parameters
66  n_local.param("max_pan", max_pan_, 2.7);
67  n_local.param("max_tilt", max_tilt_, 1.4);
68  n_local.param("min_tilt", min_tilt_, -0.4);
69 
70  n_local.param("tilt_diff", tilt_scale_, .5);
71  n_local.param("pan_diff", pan_scale_, 1.2);
72 
73  n_local.param("vx_scale", vx_scale_, 0.6);
74  n_local.param("vy_scale", vy_scale_, 0.6);
75  n_local.param("vw_scale", vw_scale_, 0.8);
76 
77  n_local.param("torso_step", torso_step_, 0.01);
78  n_local.param("min_torso", min_torso_, 0.0);
79  n_local.param("max_torso", max_torso_, 1.8);
80 
81  n_local.param("arm_x_scale", arm_x_scale_, .10);
82  n_local.param("arm_y_scale", arm_y_scale_, .10);
83  n_local.param("arm_z_scale", arm_z_scale_, .10);
84  n_local.param("arm_roll_scale", arm_roll_scale_, .10);
85  n_local.param("arm_pitch_scale", arm_pitch_scale_, .10);
86 
87  n_local.param("wrist_velocity",wrist_velocity_, 4.5);
88 
89  n_local.param("control_prosilica", control_prosilica, true);
90 
91  n_local.param("control_body", control_body, true);
92 
93  n_local.param("control_larm", control_larm, true);
94 
95  n_local.param("control_rarm", control_rarm, true);
96 
97  n_local.param("control_head", control_head, true);
98 
99  std::string arm_controller_name;
100  n_local.param("arm_controller_name", arm_controller_name,std::string(""));
101 
102  if(arm_controller_name.empty()) {
103  gc = new GeneralCommander(control_body,
104  control_head,
105  control_rarm,
106  control_larm,
107  control_prosilica);
108  } else {
109  gc = new GeneralCommander(control_body,
110  control_head,
111  control_rarm,
112  control_larm,
113  control_prosilica,
114  arm_controller_name);
115  }
116 
117  head_init_ = false;
118  torso_init_ = false;
119  }
120 
121  void sendHeadCommand(double pan_diff, double tilt_diff) {
122  if(!head_init_) {
123  double cur_pan_pos = 0.0;
124  double cur_tilt_pos = 0.0;
125  bool ok = gc->getJointPosition("head_pan_joint", cur_pan_pos);
126  if(!ok) return;
127  ok = gc->getJointPosition("head_tilt_joint", cur_tilt_pos);
128  if(!ok) return;
129  des_pan_pos_ = cur_pan_pos;
130  des_tilt_pos_ = cur_tilt_pos;
131  head_init_ = true;
132  }
133  des_pan_pos_ += pan_diff;
134  des_pan_pos_ = std::min(des_pan_pos_, max_pan_);
135  des_pan_pos_ = std::max(des_pan_pos_, -max_pan_);
136  des_tilt_pos_ += tilt_diff;
137  des_tilt_pos_ = std::min(des_tilt_pos_, max_tilt_);
138  des_tilt_pos_ = std::max(des_tilt_pos_, min_tilt_);
139  gc->sendHeadCommand(des_pan_pos_, des_tilt_pos_);
140  }
141 
142  void sendTorsoCommand(double torso_diff) {
143  // if(!torso_init_) {
144  // double cur_torso_pos = 0.0;
145  // bool ok = gc->getJointPosition("torso_lift_joint", cur_torso_pos);
146  // if(!ok) return;
147  // des_torso_pos_ = cur_torso_pos;
148  // torso_init_ = true;
149  // }
150  double cur_torso_pos = 0.0;
151  gc->getJointPosition("torso_lift_joint", cur_torso_pos);
152  des_torso_pos_ = cur_torso_pos+torso_diff;
153  des_torso_pos_ = std::min(des_torso_pos_, max_torso_);
154  des_torso_pos_ = std::max(des_torso_pos_, min_torso_);
155  gc->sendTorsoCommand(des_torso_pos_,.1);
156  }
157 
158 public:
159 
160  GeneralCommander* gc;
161 
162  double max_pan_, max_tilt_, min_tilt_;
163  double tilt_scale_, pan_scale_;
164 
165  double des_pan_pos_;
166  double des_tilt_pos_;
167 
168  bool head_init_;
169  bool torso_init_;
170 
171  double min_torso_;
172  double max_torso_;
173  double des_torso_pos_;
174  double torso_step_;
175 
176  double des_vx_;
177  double des_vy_;
178  double des_vw_;
179 
180  double vx_scale_;
181  double vy_scale_;
182  double vw_scale_;
183 
184  double arm_x_scale_;
185  double arm_y_scale_;
186  double arm_z_scale_;
187  double arm_roll_scale_;
188  double arm_pitch_scale_;
189 
190  double wrist_velocity_;
191 
192  bool control_body;
193  bool control_head;
194  bool control_larm;
195  bool control_rarm;
196  bool control_prosilica;
197 };
198 
199 void spin_function() {
200  ros::spin();
201 }
202 
203 Pr2TeleopGeneralKeyboard* generalkey = NULL;
204 
205 void quit(int sig)
206 {
207  tcsetattr(kfd, TCSANOW, &cooked);
208  ros::shutdown();
209  if(generalkey) {
210  delete generalkey;
211  }
212  exit(0);
213 }
214 
215 int main(int argc, char** argv)
216 {
217  ros::init(argc, argv, "pr2_telep_general_keyboard", ros::init_options::NoSigintHandler);
218  signal(SIGINT,quit);
219 
220  boost::thread spin_thread(boost::bind(&spin_function));
221 
222  // get the console in raw mode
223  tcgetattr(kfd, &cooked);
224  memcpy(&raw, &cooked, sizeof(struct termios));
225  raw.c_lflag &=~ (ICANON | ECHO);
226  // Setting a new line, then end of file
227  raw.c_cc[VEOL] = 1;
228  raw.c_cc[VEOF] = 2;
229  tcsetattr(kfd, TCSANOW, &raw);
230 
231  generalkey = new Pr2TeleopGeneralKeyboard();
232  generalkey->init();
233  char c;
234 
235  bool stop = false;
236  while(!stop)
237  {
238  puts("Reading from keyboard");
239  puts("---------------------------");
240  if(generalkey->control_head) {
241  puts("Use 'h' for head commands");
242  }
243  if(generalkey->control_body) {
244  puts("Use 'b' for body commands");
245  }
246  if(generalkey->control_larm) {
247  puts("Use 'l' for left arm commands");
248  }
249  if(generalkey->control_rarm) {
250  puts("Use 'r' for right arm commands");
251  }
252  if(generalkey->control_larm && generalkey->control_rarm) {
253  puts("Use 'a' for both arm commands");
254  }
255  puts("Use 'q' to quit");
256 
257  // get the next event from the keyboard
258  if(read(kfd, &c, 1) < 0)
259  {
260  perror("read():");
261  exit(-1);
262  }
263 
264  switch(c)
265  {
266  case 'h':
267  {
268  if(!generalkey->control_head) {
269  ROS_INFO("No head control enabled");
270  break;
271  }
272  puts("---------------------------");
273  puts("Use 'z' for projector on");
274  puts("Use 'x' for projector off");
275  puts("Use 's' for laser slow tilt");
276  puts("Use 'f' for laser fast tilt");
277  puts("Use 'g' for laser tilt off");
278  puts("Use 'm' to set head mannequin mode");
279  puts("Use 'y' to set to keyboard head control");
280  puts("Use 'i/j/k/l' in keyboard head control mode to move head");
281  puts("Use 'q' to quit head mode and return to main menu");
282  bool head_stop = false;
283 
284  while(!head_stop)
285  {
286  // get the next event from the keyboard
287  if(read(kfd, &c, 1) < 0)
288  {
289  perror("read():");
290  exit(-1);
291  }
292  switch(c) {
293  case 'z':
294  generalkey->gc->sendProjectorStartStop(true);
295  break;
296  case 'x':
297  generalkey->gc->sendProjectorStartStop(false);
298  break;
299  case 's':
300  generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_SLOW);
301  break;
302  case 'f':
303  generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_FAST);
304  break;
305  case 'g':
306  generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_OFF);
307  break;
308  case 'm':
309  generalkey->gc->setHeadMode(GeneralCommander::HEAD_MANNEQUIN);
310  generalkey->head_init_ = true;
311  break;
312  case 'y':
313  generalkey->gc->setHeadMode(GeneralCommander::HEAD_JOYSTICK);
314  break;
315  case 'i':
316  if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
317  generalkey->sendHeadCommand(0.0, -generalkey->tilt_scale_);
318  }
319  break;
320  case 'k':
321  if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
322  generalkey->sendHeadCommand(0.0, generalkey->tilt_scale_);
323  }
324  break;
325  case 'j':
326  if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
327  generalkey->sendHeadCommand(generalkey->pan_scale_,0.0);
328  }
329  break;
330  case 'l':
331  if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
332  generalkey->sendHeadCommand(-generalkey->pan_scale_, 0.0);
333  }
334  break;
335  case 'q':
336  head_stop = true;
337  break;
338  }
339  }
340  }
341  break;
342  case 'b':
343  {
344  if(!generalkey->control_body) {
345  ROS_INFO("No head control enabled");
346  break;
347  }
348  puts("---------------------------");
349  puts("Use 'u' for torso up");
350  puts("Use 'd' for torso down");
351  puts("Use 'i/k' for forward/back");
352  puts("Use 'j/l' for turning left and right");
353  puts("Use 'n/m' for straffing left and right");
354  puts("Use 'q' to quit body mode and return to main menu");
355 
356  bool body_stop = false;
357 
358  while(!body_stop)
359  {
360  // get the next event from the keyboard
361  if(read(kfd, &c, 1) < 0)
362  {
363  perror("read():");
364  exit(-1);
365  }
366  switch(c) {
367  case 'u':
368  generalkey->sendTorsoCommand(generalkey->torso_step_);
369  break;
370  case 'd':
371  generalkey->sendTorsoCommand(-generalkey->torso_step_);
372  break;
373  case 'i':
374  generalkey->gc->sendBaseCommand(generalkey->vx_scale_, 0.0, 0.0);
375  break;
376  case 'k':
377  generalkey->gc->sendBaseCommand(-generalkey->vx_scale_, 0.0, 0.0);
378  break;
379  case 'j':
380  generalkey->gc->sendBaseCommand(0.0, 0.0, generalkey->vw_scale_);
381  break;
382  case 'l':
383  generalkey->gc->sendBaseCommand(0.0, 0.0, -generalkey->vw_scale_);
384  break;
385  case 'n':
386  generalkey->gc->sendBaseCommand(0.0, generalkey->vy_scale_, 0.0);
387  break;
388  case 'm':
389  generalkey->gc->sendBaseCommand(0.0, -generalkey->vy_scale_, 0.0);
390  break;
391  case 'q':
392  body_stop = true;
393  break;
394  }
395  }
396  }
397  break;
398  case 'l': case 'r': case 'a':
399  {
400  if(c == 'l' && !generalkey->control_larm) {
401  ROS_INFO("No left arm control enabled");
402  break;
403  }
404  if(c == 'r' && !generalkey->control_rarm) {
405  ROS_INFO("No right arm control enabled");
406  break;
407  }
408  if(c == 'a' && (!generalkey->control_larm || !generalkey->control_rarm)) {
409  ROS_INFO("Both arms not enabled");
410  break;
411  }
412  GeneralCommander::WhichArm arm;
413  if(c == 'l') {
414  arm = GeneralCommander::ARMS_LEFT;
415  } else if(c == 'r') {
416  arm = GeneralCommander::ARMS_RIGHT;
417  } else {
418  arm = GeneralCommander::ARMS_BOTH;
419  }
420  puts("---------------------------");
421  puts("Use 'o' for gripper open");
422  puts("Use 'p' for gripper close");
423  puts("Use 'r' for wrist rotate clockwise");
424  puts("Use 'u' for wrist flex up");
425  puts("Use 'd' for wrist flex down");
426  puts("Use 't' for wrist rotate counter-clockwise");
427  puts("Use 'i/k' for hand pose forward/back");
428  puts("Use 'j/l' for hand pose left/right");
429  puts("Use 'h/n' for hand pose up/down");
430  puts("Use 'q' to quit arm mode and return to main menu");
431  bool arm_stop = false;
432 
433  while(!arm_stop)
434  {
435  // get the next event from the keyboard
436  if(read(kfd, &c, 1) < 0)
437  {
438  perror("read():");
439  exit(-1);
440  }
441 
442  switch(c) {
443  case 'o':
444  generalkey->gc->sendGripperCommand(arm, false);
445  break;
446  case 'p':
447  generalkey->gc->sendGripperCommand(arm, true);
448  break;
449  case 'i':
450  if(arm == GeneralCommander::ARMS_LEFT) {
451  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
452  generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,0.0,
453  20.0);
454  } else if(arm == GeneralCommander::ARMS_RIGHT) {
455  generalkey->gc->sendArmVelCommands(generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,0.0,
456  0.0, 0.0,0.0,0.0,0.0,0.0,
457  20.0);
458  } else {
459  generalkey->gc->sendArmVelCommands(generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,0.0,
460  generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,0.0,
461  20.0);
462  }
463  break;
464  case 'k':
465  if(arm == GeneralCommander::ARMS_LEFT) {
466  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
467  -generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,0.0,
468  20.0);
469  } else if(arm == GeneralCommander::ARMS_RIGHT) {
470  generalkey->gc->sendArmVelCommands(-generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,0.0,
471  0.0, 0.0,0.0,0.0,0.0,0.0,
472  20.0);
473  } else {
474  generalkey->gc->sendArmVelCommands(-generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,0.0,
475  -generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,0.0,
476  20.0);
477  }
478  break;
479  case 'j':
480  if(arm == GeneralCommander::ARMS_LEFT) {
481  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
482  0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
483  20.0);
484  } else if(arm == GeneralCommander::ARMS_RIGHT) {
485  generalkey->gc->sendArmVelCommands(0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
486  0.0, 0.0,0.0,0.0,0.0,0.0,
487  20.0);
488  } else {
489  generalkey->gc->sendArmVelCommands(0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
490  0.0,generalkey->arm_y_scale_, 0.0,0.0,0.0,0.0,
491  20.0);
492  }
493  break;
494  case 'l':
495  if(arm == GeneralCommander::ARMS_LEFT) {
496  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
497  0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
498  20.0);
499  } else if(arm == GeneralCommander::ARMS_RIGHT) {
500  generalkey->gc->sendArmVelCommands(0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
501  0.0, 0.0,0.0,0.0,0.0,0.0,
502  20.0);
503  } else {
504  generalkey->gc->sendArmVelCommands(0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,0.0,
505  0.0,-generalkey->arm_y_scale_, 0.0,0.0,0.0,0.0,
506  20.0);
507  }
508  break;
509  case 'h':
510  if(arm == GeneralCommander::ARMS_LEFT) {
511  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
512  0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,0.0,
513  20.0);
514  } else if(arm == GeneralCommander::ARMS_RIGHT) {
515  generalkey->gc->sendArmVelCommands(0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,0.0,
516  0.0, 0.0,0.0,0.0,0.0,0.0,
517  20.0);
518  } else {
519  generalkey->gc->sendArmVelCommands(0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,0.0,
520  0.0,0.0,generalkey->arm_z_scale_, 0.0,0.0,0.0,
521  20.0);
522  }
523  break;
524  case 'n':
525  if(arm == GeneralCommander::ARMS_LEFT) {
526  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
527  0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,0.0,
528  20.0);
529  } else if(arm == GeneralCommander::ARMS_RIGHT) {
530  generalkey->gc->sendArmVelCommands(0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,0.0,
531  0.0, 0.0,0.0,0.0,0.0,0.0,
532  20.0);
533  } else {
534  generalkey->gc->sendArmVelCommands(0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,0.0,
535  0.0,0.0,-generalkey->arm_z_scale_, 0.0,0.0,0.0,
536  20.0);
537  }
538  break;
539  case 'r':
540  if(arm == GeneralCommander::ARMS_LEFT) {
541  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
542  0.0,0.0,0.0,generalkey->arm_roll_scale_,0.0,0.0,
543  20.0);
544  } else if(arm == GeneralCommander::ARMS_RIGHT) {
545  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,generalkey->arm_roll_scale_,0.0,0.0,
546  0.0, 0.0,0.0,0.0,0.0,0.0,
547  20.0);
548  } else {
549  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,generalkey->arm_roll_scale_,0.0,0.0,
550  0.0,0.0,0.0,generalkey->arm_roll_scale_,0.0,0.0,
551  20.0);
552  }
553  break;
554  case 't':
555  if(arm == GeneralCommander::ARMS_LEFT) {
556  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
557  0.0,0.0,0.0,-generalkey->arm_roll_scale_,0.0,0.0,
558  20.0);
559  } else if(arm == GeneralCommander::ARMS_RIGHT) {
560  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,-generalkey->arm_roll_scale_,0.0,0.0,
561  0.0,0.0,0.0,0.0,0.0,0.0,
562  20.0);
563  } else {
564  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,-generalkey->arm_roll_scale_,0.0,0.0,
565  0.0,0.0,0.0,-generalkey->arm_roll_scale_,0.0,0.0,
566  20.0);
567  }
568  break;
569  case 'd':
570  if(arm == GeneralCommander::ARMS_LEFT) {
571  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
572  0.0,0.0,0,0.0,generalkey->arm_pitch_scale_,0.0,
573  20.0);
574  } else if(arm == GeneralCommander::ARMS_RIGHT) {
575  generalkey->gc->sendArmVelCommands(0.0,0.0,0,0.0,generalkey->arm_pitch_scale_,0.0,
576  0.0, 0.0,0.0,0.0,0.0,0.0,
577  20.0);
578  } else {
579  generalkey->gc->sendArmVelCommands(0.0,0.0,0,0.0,generalkey->arm_pitch_scale_,0.0,
580  0.0,0.0,0,0.0,generalkey->arm_pitch_scale_,0.0,
581  20.0);
582  }
583  break;
584  case 'u':
585  if(arm == GeneralCommander::ARMS_LEFT) {
586  generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,0.0,
587  0.0,0.0,0,0.0,-generalkey->arm_pitch_scale_,0.0,
588  20.0);
589  } else if(arm == GeneralCommander::ARMS_RIGHT) {
590  generalkey->gc->sendArmVelCommands(0.0,0.0,0,0.0,-generalkey->arm_pitch_scale_,0.0,
591  0.0, 0.0,0.0,0.0,0.0,0.0,
592  20.0);
593  } else {
594  generalkey->gc->sendArmVelCommands(0.0,0.0,0,0.0,-generalkey->arm_pitch_scale_,0.0,
595  0.0,0.0,0,0.0,-generalkey->arm_pitch_scale_,0.0,
596  20.0);
597  }
598  break;
599  case 'q':
600  arm_stop = true;
601  break;
602  }
603  }
604  }
605  break;
606  case 'q':
607  stop = true;
608  break;
609  default:
610  ROS_INFO_STREAM("Keycode is " << c);
611  break;
612  }
613  }
614 
615  tcsetattr(kfd, TCSANOW, &cooked);
616  ros::shutdown();
617  spin_thread.join();
618  return(0);
619 }
ros::NodeHandle
ros::init_options::NoSigintHandler
NoSigintHandler
generalkey
Pr2TeleopGeneralKeyboard * generalkey
Definition: pr2_teleop_general_keyboard.cpp:203
main
int main(int argc, char **argv)
Definition: pr2_teleop_general_keyboard.cpp:215
GeneralCommander::setHeadMode
void setHeadMode(HeadControlMode mode)
Definition: pr2_teleop_general_commander.cpp:380
Pr2TeleopGeneralKeyboard::sendTorsoCommand
void sendTorsoCommand(double torso_diff)
Definition: pr2_teleop_general_keyboard.cpp:142
GeneralCommander::sendArmVelCommands
void sendArmVelCommands(double r_x_vel, double r_y_vel, double r_z_vel, double r_roll_vel, double r_pitch_vel, double r_yaw_vel, double l_x_vel, double l_y_vel, double l_z_vel, double l_roll_vel, double l_pitch_vel, double l_yaw_vel, double hz)
Definition: pr2_teleop_general_commander.cpp:899
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Pr2TeleopGeneralKeyboard::sendHeadCommand
void sendHeadCommand(double pan_diff, double tilt_diff)
Definition: pr2_teleop_general_keyboard.cpp:121
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
quit
void quit(int sig)
Definition: pr2_teleop_general_keyboard.cpp:205
raw
struct termios cooked raw
Definition: pr2_teleop_general_keyboard.cpp:45
GeneralCommander::sendBaseCommand
void sendBaseCommand(double vx, double vy, double vw)
Definition: pr2_teleop_general_commander.cpp:593
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
GeneralCommander::sendProjectorStartStop
void sendProjectorStartStop(bool start)
Definition: pr2_teleop_general_commander.cpp:327
GeneralCommander::sendHeadCommand
void sendHeadCommand(double req_pan, double req_tilt)
Definition: pr2_teleop_general_commander.cpp:477
GeneralCommander::getJointPosition
bool getJointPosition(const std::string &name, double &pos) const
Definition: pr2_teleop_general_commander.cpp:295
kfd
int kfd
Definition: pr2_teleop_general_keyboard.cpp:44
pr2_teleop_general_commander.h
ROS_INFO_STREAM
#define ROS_INFO_STREAM(args)
GeneralCommander::setLaserMode
void setLaserMode(LaserControlMode mode)
Definition: pr2_teleop_general_commander.cpp:346
GeneralCommander::sendGripperCommand
void sendGripperCommand(WhichArm which, bool close)
Definition: pr2_teleop_general_commander.cpp:536
ros::shutdown
ROSCPP_DECL void shutdown()
GeneralCommander::sendTorsoCommand
void sendTorsoCommand(double pos, double vel)
Definition: pr2_teleop_general_commander.cpp:575
spin_function
void spin_function()
Definition: pr2_teleop_general_keyboard.cpp:199

pr2_teleop_general
Author(s): Gil Jones
autogenerated on Sat Feb 27 2021 04:01:05