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