pr2_teleop_general: pr2_teleop_general: pr2_teleop_general

00001 /*
00002  * pr2_teleop_general_keyboard
00003  * Copyright (c) 2010, 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: E. Gil Jones
00032 
00033 #include <termios.h>
00034 #include <signal.h>
00035 #include <cstdio>
00036 #include <cstdlib>
00037 #include <unistd.h>
00038 #include <math.h>
00039 
00040 #include <ros/ros.h>
00041 
00042 #include "pr2_teleop_general/pr2_teleop_general_commander.h"
00043 
00044 int kfd = 0;
00045 struct termios cooked, raw;
00046 
00047 class Pr2TeleopGeneralKeyboard
00048 {
00049 public:
00050   Pr2TeleopGeneralKeyboard()
00051   {
00052     gc = NULL;
00053   }
00054 
00055   ~Pr2TeleopGeneralKeyboard() {  
00056     if(gc != NULL) {
00057       delete gc;
00058     }
00059   }
00060 
00061   void init()
00062   {
00063     ros::NodeHandle n_local("~");
00064     
00065     // Head pan/tilt parameters
00066     n_local.param("max_pan", max_pan_, 2.7);
00067     n_local.param("max_tilt", max_tilt_, 1.4);
00068     n_local.param("min_tilt", min_tilt_, -0.4);
00069     
00070     n_local.param("tilt_diff", tilt_scale_, .5);
00071     n_local.param("pan_diff", pan_scale_, 1.2);
00072 
00073     n_local.param("vx_scale", vx_scale_, 0.6);
00074     n_local.param("vy_scale", vy_scale_, 0.6);
00075     n_local.param("vw_scale", vw_scale_, 0.8);
00076 
00077     n_local.param("torso_step", torso_step_, 0.01);
00078     n_local.param("min_torso", min_torso_, 0.0);
00079     n_local.param("max_torso", max_torso_, 1.8);
00080 
00081     n_local.param("arm_x_scale", arm_x_scale_, .10);
00082     n_local.param("arm_y_scale", arm_y_scale_, .10);
00083     n_local.param("arm_z_scale", arm_z_scale_, .10);
00084     n_local.param("arm_roll_scale", arm_roll_scale_, .10);
00085     n_local.param("arm_pitch_scale", arm_pitch_scale_, .10);
00086 
00087     n_local.param("wrist_velocity",wrist_velocity_, 4.5);
00088     
00089     n_local.param("control_prosilica", control_prosilica, true);
00090     
00091     n_local.param("control_body", control_body, true);
00092 
00093     n_local.param("control_larm", control_larm, true);
00094 
00095     n_local.param("control_rarm", control_rarm, true);
00096 
00097     n_local.param("control_head", control_head, true);
00098 
00099     std::string arm_controller_name;
00100     n_local.param("arm_controller_name", arm_controller_name,std::string(""));
00101     
00102     if(arm_controller_name.empty()) {
00103       gc = new GeneralCommander(control_body, 
00104                                 control_head, 
00105                                 control_rarm,
00106                                 control_larm,
00107                                 control_prosilica);
00108     } else {
00109       gc = new GeneralCommander(control_body, 
00110                                 control_head, 
00111                                 control_rarm,
00112                                 control_larm,
00113                                 control_prosilica,
00114                                 arm_controller_name);
00115     }
00116 
00117     head_init_ = false;
00118     torso_init_ = false;
00119   }
00120 
00121   void sendHeadCommand(double pan_diff, double tilt_diff) {
00122     if(!head_init_) {
00123       double cur_pan_pos = 0.0;
00124       double cur_tilt_pos = 0.0;
00125       bool ok = gc->getJointPosition("head_pan_joint", cur_pan_pos);
00126       if(!ok) return;
00127       ok = gc->getJointPosition("head_tilt_joint", cur_tilt_pos);
00128       if(!ok) return;
00129       des_pan_pos_ = cur_pan_pos;
00130       des_tilt_pos_ = cur_tilt_pos;
00131       head_init_ = true;
00132     }
00133     des_pan_pos_ += pan_diff;
00134     des_pan_pos_ = std::min(des_pan_pos_, max_pan_);
00135     des_pan_pos_ = std::max(des_pan_pos_, -max_pan_);
00136     des_tilt_pos_ += tilt_diff;
00137     des_tilt_pos_ = std::min(des_tilt_pos_, max_tilt_);
00138     des_tilt_pos_ = std::max(des_tilt_pos_, min_tilt_);
00139     gc->sendHeadCommand(des_pan_pos_, des_tilt_pos_);
00140   }
00141 
00142   void sendTorsoCommand(double torso_diff) {
00143     // if(!torso_init_)  {
00144     //   double cur_torso_pos = 0.0;
00145     //   bool ok = gc->getJointPosition("torso_lift_joint", cur_torso_pos);
00146     //   if(!ok) return;
00147     //   des_torso_pos_ = cur_torso_pos;
00148     //   torso_init_ = true;
00149     // }
00150     double cur_torso_pos = 0.0;
00151     gc->getJointPosition("torso_lift_joint", cur_torso_pos);
00152     des_torso_pos_ = cur_torso_pos+torso_diff;
00153     des_torso_pos_ = std::min(des_torso_pos_, max_torso_);
00154     des_torso_pos_ = std::max(des_torso_pos_, min_torso_);
00155     gc->sendTorsoCommand(des_torso_pos_,.1);
00156   }
00157 
00158 public:
00159 
00160   GeneralCommander* gc;
00161   
00162   double max_pan_, max_tilt_, min_tilt_;
00163   double tilt_scale_, pan_scale_;
00164 
00165   double des_pan_pos_;
00166   double des_tilt_pos_;
00167 
00168   bool head_init_;
00169   bool torso_init_;
00170 
00171   double min_torso_;
00172   double max_torso_;
00173   double des_torso_pos_;
00174   double torso_step_;
00175 
00176   double des_vx_;
00177   double des_vy_;
00178   double des_vw_;
00179 
00180   double vx_scale_;
00181   double vy_scale_;
00182   double vw_scale_;
00183 
00184   double arm_x_scale_;
00185   double arm_y_scale_;
00186   double arm_z_scale_;
00187   double arm_roll_scale_;
00188   double arm_pitch_scale_;
00189 
00190   double wrist_velocity_;
00191 
00192   bool control_body;
00193   bool control_head;
00194   bool control_larm;
00195   bool control_rarm;
00196   bool control_prosilica;
00197 };
00198 
00199 void spin_function() {
00200   ros::spin();
00201 }
00202 
00203 Pr2TeleopGeneralKeyboard* generalkey = NULL;
00204 
00205 void quit(int sig)
00206 {
00207   tcsetattr(kfd, TCSANOW, &cooked);
00208   ros::shutdown();
00209   if(generalkey) {
00210     delete generalkey;
00211   }
00212   exit(0);
00213 }
00214 
00215 int main(int argc, char** argv)
00216 {
00217   ros::init(argc, argv, "pr2_telep_general_keyboard", ros::init_options::NoSigintHandler);
00218   signal(SIGINT,quit);
00219   
00220   boost::thread spin_thread(boost::bind(&spin_function));
00221 
00222   // get the console in raw mode
00223   tcgetattr(kfd, &cooked);
00224   memcpy(&raw, &cooked, sizeof(struct termios));
00225   raw.c_lflag &=~ (ICANON | ECHO);
00226   // Setting a new line, then end of file
00227   raw.c_cc[VEOL] = 1;
00228   raw.c_cc[VEOF] = 2;
00229   tcsetattr(kfd, TCSANOW, &raw);
00230   
00231   generalkey = new Pr2TeleopGeneralKeyboard();
00232   generalkey->init();
00233   char c;
00234   
00235   bool stop = false;
00236   while(!stop)
00237   {
00238     puts("Reading from keyboard");
00239     puts("---------------------------");
00240     if(generalkey->control_head) {
00241       puts("Use 'h' for head commands");
00242     }
00243     if(generalkey->control_body) {
00244       puts("Use 'b' for body commands");
00245     }
00246     if(generalkey->control_larm) {
00247       puts("Use 'l' for left arm commands");
00248     }
00249     if(generalkey->control_rarm) {
00250       puts("Use 'r' for right arm commands");
00251     }
00252     if(generalkey->control_larm && generalkey->control_rarm) {
00253       puts("Use 'a' for both arm commands");
00254     }
00255     puts("Use 'q' to quit");
00256       
00257     // get the next event from the keyboard
00258     if(read(kfd, &c, 1) < 0)
00259     {
00260       perror("read():");
00261       exit(-1);
00262     }
00263       
00264     switch(c)
00265     {
00266     case 'h':
00267       {
00268         if(!generalkey->control_head) {
00269           ROS_INFO("No head control enabled");
00270           break;
00271         }
00272         puts("---------------------------");
00273         puts("Use 'z' for projector on");
00274         puts("Use 'x' for projector off");
00275         puts("Use 's' for laser slow tilt");
00276         puts("Use 'f' for laser fast tilt");
00277         puts("Use 'g' for laser tilt off");
00278         puts("Use 'm' to set head mannequin mode");
00279         puts("Use 'y' to set to keyboard head control");
00280         puts("Use 'i/j/k/l' in keyboard head control mode to move head");
00281         puts("Use 'q' to quit head mode and return to main menu");
00282         bool head_stop = false;
00283 
00284         while(!head_stop) 
00285         {
00286           // get the next event from the keyboard
00287           if(read(kfd, &c, 1) < 0)
00288           {
00289             perror("read():");
00290             exit(-1);
00291           }
00292           switch(c) {
00293           case 'z':
00294             generalkey->gc->sendProjectorStartStop(true);
00295             break;
00296           case 'x':
00297             generalkey->gc->sendProjectorStartStop(false);
00298             break;
00299           case 's':
00300             generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_SLOW);
00301             break;
00302           case 'f':
00303             generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_FAST);
00304             break;
00305           case 'g':
00306             generalkey->gc->setLaserMode(GeneralCommander::LASER_TILT_OFF);
00307             break;
00308           case 'm':
00309             generalkey->gc->setHeadMode(GeneralCommander::HEAD_MANNEQUIN);
00310             generalkey->head_init_ = true;
00311             break;
00312           case 'y':
00313             generalkey->gc->setHeadMode(GeneralCommander::HEAD_JOYSTICK);
00314             break;
00315           case 'i':
00316             if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
00317               generalkey->sendHeadCommand(0.0, -generalkey->tilt_scale_);
00318             }
00319             break;
00320           case 'k':
00321             if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
00322               generalkey->sendHeadCommand(0.0, generalkey->tilt_scale_);
00323             }
00324             break;
00325           case 'j':
00326             if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
00327               generalkey->sendHeadCommand(generalkey->pan_scale_,0.0);
00328             }
00329             break;
00330           case 'l':
00331             if(generalkey->gc->getHeadMode() == GeneralCommander::HEAD_JOYSTICK) {
00332               generalkey->sendHeadCommand(-generalkey->pan_scale_, 0.0);
00333             }
00334             break;
00335           case 'q':
00336             head_stop = true;
00337             break;
00338           }
00339         }
00340       }
00341       break;
00342     case 'b':
00343       {
00344         if(!generalkey->control_body) {
00345           ROS_INFO("No head control enabled");
00346           break;
00347         }
00348         puts("---------------------------");
00349         puts("Use 'u' for torso up");
00350         puts("Use 'd' for torso down");
00351         puts("Use 'i/k' for forward/back");
00352         puts("Use 'j/l' for turning left and right");
00353         puts("Use 'n/m' for straffing left and right");
00354         puts("Use 'q' to quit body mode and return to main menu");
00355 
00356         bool body_stop = false;
00357 
00358         while(!body_stop) 
00359         {
00360           // get the next event from the keyboard
00361           if(read(kfd, &c, 1) < 0)
00362           {
00363             perror("read():");
00364             exit(-1);
00365           }
00366           switch(c) {
00367           case 'u':
00368             generalkey->sendTorsoCommand(generalkey->torso_step_);
00369             break;
00370           case 'd':
00371             generalkey->sendTorsoCommand(-generalkey->torso_step_);
00372             break;
00373           case 'i':
00374             generalkey->gc->sendBaseCommand(generalkey->vx_scale_, 0.0, 0.0);
00375             break;
00376           case 'k':
00377             generalkey->gc->sendBaseCommand(-generalkey->vx_scale_, 0.0, 0.0);
00378             break;
00379           case 'j':
00380             generalkey->gc->sendBaseCommand(0.0, 0.0, generalkey->vw_scale_);
00381             break;
00382           case 'l':
00383             generalkey->gc->sendBaseCommand(0.0, 0.0, -generalkey->vw_scale_);
00384             break;
00385           case 'n':
00386             generalkey->gc->sendBaseCommand(0.0, generalkey->vy_scale_, 0.0);
00387             break;
00388           case 'm':
00389             generalkey->gc->sendBaseCommand(0.0, -generalkey->vy_scale_, 0.0);
00390             break;
00391           case 'q':
00392             body_stop = true;
00393             break;
00394           }
00395         }
00396       }
00397       break;
00398     case 'l': case 'r': case 'a':
00399       {
00400         if(c == 'l' && !generalkey->control_larm) {
00401           ROS_INFO("No left arm control enabled");
00402           break;
00403         }
00404         if(c == 'r' && !generalkey->control_rarm) {
00405           ROS_INFO("No right arm control enabled");
00406           break;
00407         }
00408         if(c == 'a' && (!generalkey->control_larm || !generalkey->control_rarm)) {
00409           ROS_INFO("Both arms not enabled");
00410           break;
00411         }
00412         GeneralCommander::WhichArm arm;
00413         if(c == 'l') {
00414           arm = GeneralCommander::ARMS_LEFT;
00415         } else if(c == 'r') {
00416           arm = GeneralCommander::ARMS_RIGHT;
00417         } else {
00418           arm = GeneralCommander::ARMS_BOTH;
00419         }
00420         puts("---------------------------");
00421         puts("Use 'o' for gripper open");
00422         puts("Use 'p' for gripper close");
00423         puts("Use 'r' for wrist rotate clockwise");
00424         puts("Use 'u' for wrist flex up");
00425         puts("Use 'd' for wrist flex down");
00426         puts("Use 't' for wrist rotate counter-clockwise");
00427         puts("Use 'i/k' for hand pose forward/back");
00428         puts("Use 'j/l' for hand pose left/right");
00429         puts("Use 'h/n' for hand pose up/down");
00430         puts("Use 'q' to quit arm mode and return to main menu");
00431         bool arm_stop = false;
00432 
00433         while(!arm_stop) 
00434         {
00435           // get the next event from the keyboard
00436           if(read(kfd, &c, 1) < 0)
00437           {
00438             perror("read():");
00439             exit(-1);
00440           }
00441 
00442           switch(c) {
00443           case 'o':
00444             generalkey->gc->sendGripperCommand(arm, false);
00445             break;
00446           case 'p':
00447             generalkey->gc->sendGripperCommand(arm, true);
00448             break;
00449           case 'i':
00450             if(arm == GeneralCommander::ARMS_LEFT) {
00451               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00452                                              generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,
00453                                              20.0);
00454             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00455               generalkey->gc->sendArmVelCommands(generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,
00456                                              0.0, 0.0,0.0,0.0,0.0,
00457                                              20.0);
00458             } else {
00459               generalkey->gc->sendArmVelCommands(generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,
00460                                              generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,
00461                                              20.0);
00462             }
00463             break;
00464           case 'k':
00465             if(arm == GeneralCommander::ARMS_LEFT) {
00466               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00467                                              -generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,
00468                                              20.0);
00469             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00470               generalkey->gc->sendArmVelCommands(-generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,
00471                                              0.0, 0.0,0.0,0.0,0.0,
00472                                              20.0);
00473             } else {
00474               generalkey->gc->sendArmVelCommands(-generalkey->arm_x_scale_,0.0,0.0,0.0,0.0,
00475                                              -generalkey->arm_x_scale_, 0.0,0.0,0.0,0.0,
00476                                              20.0);
00477             }
00478             break;
00479           case 'j':
00480             if(arm == GeneralCommander::ARMS_LEFT) {
00481               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00482                                              0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,
00483                                              20.0);
00484             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00485               generalkey->gc->sendArmVelCommands(0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,
00486                                              0.0, 0.0,0.0,0.0,0.0,
00487                                              20.0);
00488             } else {
00489               generalkey->gc->sendArmVelCommands(0.0,generalkey->arm_y_scale_,0.0,0.0,0.0,
00490                                                  0.0,generalkey->arm_y_scale_, 0.0,0.0,0.0,
00491                                                  20.0);
00492             }
00493             break;
00494           case 'l':
00495             if(arm == GeneralCommander::ARMS_LEFT) {
00496               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00497                                              0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,
00498                                              20.0);
00499             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00500               generalkey->gc->sendArmVelCommands(0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,
00501                                              0.0, 0.0,0.0,0.0,0.0,
00502                                              20.0);
00503             } else {
00504               generalkey->gc->sendArmVelCommands(0.0,-generalkey->arm_y_scale_,0.0,0.0,0.0,
00505                                                  0.0,-generalkey->arm_y_scale_, 0.0,0.0,0.0,
00506                                                  20.0);
00507             }
00508             break;
00509           case 'h':
00510             if(arm == GeneralCommander::ARMS_LEFT) {
00511               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00512                                                  0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,
00513                                                  20.0);
00514             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00515               generalkey->gc->sendArmVelCommands(0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,
00516                                                  0.0, 0.0,0.0,0.0,0.0,
00517                                                  20.0);
00518             } else {
00519               generalkey->gc->sendArmVelCommands(0.0,0.0,generalkey->arm_z_scale_,0.0,0.0,
00520                                                  0.0,0.0,generalkey->arm_z_scale_, 0.0,0.0,
00521                                                  20.0);
00522             }
00523             break;
00524           case 'n':
00525             if(arm == GeneralCommander::ARMS_LEFT) {
00526               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00527                                                  0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,
00528                                                  20.0);
00529             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00530               generalkey->gc->sendArmVelCommands(0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,
00531                                                  0.0, 0.0,0.0,0.0,0.0,
00532                                                  20.0);
00533             } else {
00534               generalkey->gc->sendArmVelCommands(0.0,0.0,-generalkey->arm_z_scale_,0.0,0.0,
00535                                                  0.0,0.0,-generalkey->arm_z_scale_, 0.0,0.0,
00536                                                  20.0);
00537             }
00538             break;
00539           case 'd':
00540             if(arm == GeneralCommander::ARMS_LEFT) {
00541               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00542                                                  0.0,0.0,0,generalkey->arm_pitch_scale_,0.0,
00543                                                  20.0);
00544             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00545               generalkey->gc->sendArmVelCommands(0.0,0.0,0,generalkey->arm_pitch_scale_,0.0,
00546                                                  0.0, 0.0,0.0,0.0,0.0,
00547                                                  20.0);
00548             } else {
00549               generalkey->gc->sendArmVelCommands(0.0,0.0,0,generalkey->arm_pitch_scale_,0.0,
00550                                                  0.0,0.0,0,generalkey->arm_pitch_scale_,0.0,
00551                                                  20.0);
00552             }
00553             break;
00554           case 'u':
00555             if(arm == GeneralCommander::ARMS_LEFT) {
00556               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00557                                                  0.0,0.0,0,-generalkey->arm_pitch_scale_,0.0,
00558                                                  20.0);
00559             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00560               generalkey->gc->sendArmVelCommands(0.0,0.0,0,-generalkey->arm_pitch_scale_,0.0,
00561                                                  0.0, 0.0,0.0,0.0,0.0,
00562                                                  20.0);
00563             } else {
00564               generalkey->gc->sendArmVelCommands(0.0,0.0,0,-generalkey->arm_pitch_scale_,0.0,
00565                                                  0.0,0.0,0,-generalkey->arm_pitch_scale_,0.0,
00566                                                  20.0);
00567             }
00568             break;
00569           case 'r':
00570             if(arm == GeneralCommander::ARMS_LEFT) {
00571               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00572                                                  0.0,0.0,0.0,0.0,generalkey->arm_roll_scale_,
00573                                                  20.0);
00574             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00575               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,generalkey->arm_roll_scale_,
00576                                                  0.0, 0.0,0.0,0.0,0.0,
00577                                                  20.0);
00578             } else {
00579               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,generalkey->arm_roll_scale_,
00580                                                  0.0,0.0,0.0,0.0,generalkey->arm_roll_scale_,
00581                                                  20.0);
00582             }
00583             break;
00584           case 't':
00585             if(arm == GeneralCommander::ARMS_LEFT) {
00586               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,0.0,
00587                                                  0.0,0.0,0.0,0.0,-generalkey->arm_roll_scale_,
00588                                                  20.0);
00589             } else if(arm == GeneralCommander::ARMS_RIGHT) {
00590               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,-generalkey->arm_roll_scale_,
00591                                                  0.0, 0.0,0.0,0.0,0.0,
00592                                                  20.0);
00593             } else {
00594               generalkey->gc->sendArmVelCommands(0.0,0.0,0.0,0.0,-generalkey->arm_roll_scale_,
00595                                                  0.0,0.0,0.0,0.0,-generalkey->arm_roll_scale_,
00596                                                  20.0);
00597             }
00598             break;
00599           case 'q':
00600             arm_stop = true;
00601             break;
00602           }
00603         }
00604       }
00605       break;
00606     case 'q':
00607       stop = true;
00608       break;
00609     default:
00610       ROS_INFO_STREAM("Keycode is " << c);
00611       break;
00612     }
00613   }
00614   
00615   tcsetattr(kfd, TCSANOW, &cooked);
00616   ros::shutdown();
00617   spin_thread.join();
00618   return(0);
00619 }
pr2_teleop_general_keyboard.cpp
