teleop_pr2_keyboard.cpp
Go to the documentation of this file.
00001 /*
00002  * teleop_pr2_keyboard
00003  * Copyright (c) 2008, 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: Kevin Watts
00032 
00033 #include <termios.h>
00034 #include <signal.h>
00035 #include <math.h>
00036 #include <stdio.h>
00037 #include <stdlib.h>
00038 
00039 #include <ros/ros.h>
00040 #include <geometry_msgs/Twist.h>
00041 
00042 #include <ias_drawer_executive/RobotArm.h>
00043 #include <ias_drawer_executive/Gripper.h>
00044 
00045 #define KEYCODE_A 0x61
00046 #define KEYCODE_D 0x64
00047 #define KEYCODE_S 0x73
00048 #define KEYCODE_W 0x77
00049 #define KEYCODE_Q 0x71
00050 #define KEYCODE_E 0x65
00051 #define KEYCODE_R 0x72
00052 
00053 #define KEYCODE_I 0x69
00054 #define KEYCODE_K 0x6B
00055 #define KEYCODE_J 0x6A
00056 #define KEYCODE_L 0x6C
00057 #define KEYCODE_N 0x6E
00058 #define KEYCODE_M 0x6D
00059 
00060 #define KEYCODE_I_CAP 0x49
00061 #define KEYCODE_K_CAP 0x4B
00062 #define KEYCODE_J_CAP 0x4A
00063 #define KEYCODE_L_CAP 0x4C
00064 #define KEYCODE_N_CAP 0x4E
00065 #define KEYCODE_M_CAP 0x4D
00066 
00067 #define KEYCODE_A_CAP 0x41
00068 #define KEYCODE_D_CAP 0x44
00069 #define KEYCODE_S_CAP 0x53
00070 #define KEYCODE_W_CAP 0x57
00071 #define KEYCODE_Q_CAP 0x51
00072 #define KEYCODE_E_CAP 0x45
00073 
00074 #define KEYCODE_BRACK_OP 0x5B
00075 #define KEYCODE_BRACK_CL 0x5D
00076 
00077 class TeleopPR2Keyboard
00078 {
00079 private:
00080     double walk_vel, run_vel, yaw_rate, yaw_rate_run;
00081     geometry_msgs::Twist cmd;
00082 
00083     ros::NodeHandle n_;
00084     ros::Publisher vel_pub_;
00085 
00086     RobotArm *arm_;
00087     Gripper *gripper_;
00088 
00089 public:
00090     void init(int arm_no)
00091     {
00092         cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00093 
00094         arm_ = RobotArm::getInstance(arm_no);
00095         gripper_ = Gripper::getInstance(arm_no);
00096 
00097         //vel_pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00098 
00099         ros::NodeHandle n_private("~");
00100         n_private.param("walk_vel", walk_vel, 0.5);
00101         n_private.param("run_vel", run_vel, 1.0);
00102         n_private.param("yaw_rate", yaw_rate, 1.0);
00103         n_private.param("yaw_run_rate", yaw_rate_run, 1.5);
00104 
00105     }
00106 
00107     ~TeleopPR2Keyboard()   { }
00108     void keyboardLoop();
00109 
00110 };
00111 
00112 int kfd = 0;
00113 struct termios cooked, raw;
00114 
00115 void quit(int sig)
00116 {
00117     tcsetattr(kfd, TCSANOW, &cooked);
00118     exit(0);
00119 }
00120 
00121 int main(int argc, char** argv)
00122 {
00123 
00124     char name[50]  = "pr2_base_keyboard";
00125 
00126     int no_arm = 0;
00127 
00128     if (argc > 1)
00129     {
00130         no_arm = atoi(argv[1]);
00131         sprintf(name,"pr2_base_keyboard_%i",no_arm);
00132     }
00133 
00134     ros::init(argc, argv, name);
00135 
00136     ROS_INFO("INITIATLIZED NODE WITH NAME: %s", name);
00137 
00138     TeleopPR2Keyboard tpk;
00139     tpk.init(no_arm);
00140 
00141     signal(SIGINT,quit);
00142 
00143     tpk.keyboardLoop();
00144 
00145     return(0);
00146 }
00147 
00148 
00149 void TeleopPR2Keyboard::keyboardLoop()
00150 {
00151     char c;
00152     bool dirty=false;
00153     bool grip_dirty = false;
00154 
00155     // get the console in raw mode
00156     tcgetattr(kfd, &cooked);
00157     memcpy(&raw, &cooked, sizeof(struct termios));
00158     raw.c_lflag &=~ (ICANON | ECHO);
00159     // Setting a new line, then end of file
00160     raw.c_cc[VEOL] = 1;
00161     raw.c_cc[VEOF] = 2;
00162     tcsetattr(kfd, TCSANOW, &raw);
00163 
00164     puts("Reading from keyboard");
00165     puts("---------------------------");
00166     puts("Use 'WASD' to translate");
00167     puts("Use 'QE' to yaw");
00168     puts("Press 'Shift' to run");
00169 
00170     //RobotArm *arm = RobotArm::getInstance(1);
00171     arm_->evil_switch = true;
00172 
00173     tf::Stamped<tf::Pose> toolPose = arm_->getToolPose();//const char frame[] = "base_link");
00174     double grip_open = gripper_->getAmountOpen();
00175 
00176     for (;;)
00177     {
00178         // get the next event from the keyboard
00179         if (read(kfd, &c, 1) < 0)
00180         {
00181             perror("read():");
00182             exit(-1);
00183         }
00184 
00185         ROS_INFO("read: %X\n", c);
00186 
00187         cmd.linear.x = cmd.linear.y = cmd.angular.z = 0;
00188 
00189         btVector3 dist(0,0,0);
00190 
00191         switch (c)
00192         {
00193             // Walking
00194         case KEYCODE_W:
00195             //cmd.linear.x = walk_vel;
00196             dirty = true;
00197             dist += btVector3(0.01,0,0);
00198             break;
00199         case KEYCODE_S:
00200             //cmd.linear.x = - walk_vel;
00201             dist += btVector3(-0.01,0,0);
00202             dirty = true;
00203             break;
00204         case KEYCODE_A:
00205             //cmd.linear.y = walk_vel;
00206             dist += btVector3(0,0.01,0);
00207             dirty = true;
00208             break;
00209         case KEYCODE_D:
00210             //cmd.linear.y = - walk_vel;
00211             dist += btVector3(0,-0.01,0);
00212             dirty = true;
00213             break;
00214         case KEYCODE_Q:
00215             //cmd.angular.z = yaw_rate;
00216             dist += btVector3(0,0,0.01);
00217             dirty = true;
00218             break;
00219         case KEYCODE_E:
00220             //cmd.angular.z = - yaw_rate;
00221             dist += btVector3(0,0,-0.01);
00222             dirty = true;
00223             break;
00224 
00225 
00226         case KEYCODE_I:
00227             toolPose = arm_->rotateAroundBaseAxis(toolPose,0.1,0,0);
00228             dirty = true;
00229             break;
00230         case KEYCODE_K:
00231             toolPose = arm_->rotateAroundBaseAxis(toolPose,-0.1,0,0);
00232             dirty = true;
00233             break;
00234         case KEYCODE_J:
00235             toolPose = arm_->rotateAroundBaseAxis(toolPose,0,0,0.1);
00236             dirty = true;
00237             break;
00238         case KEYCODE_L:
00239             toolPose = arm_->rotateAroundBaseAxis(toolPose,0,0,-0.1);
00240             dirty = true;
00241             break;
00242         case KEYCODE_N:
00243             toolPose = arm_->rotateAroundBaseAxis(toolPose,0,0.1,0);
00244             dirty = true;
00245             break;
00246         case KEYCODE_M:
00247             toolPose = arm_->rotateAroundBaseAxis(toolPose,0,-0.1,0);
00248             dirty = true;
00249             break;
00250 
00251         case KEYCODE_I_CAP:
00252             toolPose = arm_->rotateAroundToolframeAxis(toolPose,0.1,0,0);
00253             dirty = true;
00254             break;
00255         case KEYCODE_K_CAP:
00256             toolPose = arm_->rotateAroundToolframeAxis(toolPose,-0.1,0,0);
00257             dirty = true;
00258             break;
00259         case KEYCODE_J_CAP:
00260             toolPose = arm_->rotateAroundToolframeAxis(toolPose,0,0,0.1);
00261             dirty = true;
00262             break;
00263         case KEYCODE_L_CAP:
00264             toolPose = arm_->rotateAroundToolframeAxis(toolPose,0,0,-0.1);
00265             dirty = true;
00266             break;
00267         case KEYCODE_N_CAP:
00268             toolPose = arm_->rotateAroundToolframeAxis(toolPose,0,0.1,0);
00269             dirty = true;
00270             break;
00271         case KEYCODE_M_CAP:
00272             toolPose = arm_->rotateAroundToolframeAxis(toolPose,0,-0.1,0);
00273             dirty = true;
00274             break;
00275 
00276 
00277         case KEYCODE_BRACK_OP:
00278             grip_open += 0.0025;
00279             grip_dirty = true;
00280             break;
00281         case KEYCODE_BRACK_CL:
00282             grip_open -= 0.0025;
00283             grip_dirty = true;
00284             break;
00285 
00286 
00287         case KEYCODE_R:
00288             toolPose = arm_->getToolPose();//const char frame[] = "base_link");
00289             break;
00290 
00291 
00292 
00293             /*
00294 
00295             // Running
00296             case KEYCODE_W_CAP:
00297             cmd.linear.x = run_vel;
00298             dirty = true;
00299             break;
00300             case KEYCODE_S_CAP:
00301             cmd.linear.x = - run_vel;
00302             dirty = true;
00303             break;
00304             case KEYCODE_A_CAP:
00305             cmd.linear.y = run_vel;
00306             dirty = true;
00307             break;
00308             case KEYCODE_D_CAP:
00309             cmd.linear.y = - run_vel;
00310             dirty = true;
00311             break;
00312             case KEYCODE_Q_CAP:
00313             cmd.angular.z = yaw_rate_run;
00314             dirty = true;
00315             break;
00316             case KEYCODE_E_CAP:
00317             cmd.angular.z = - yaw_rate_run;
00318             dirty = true;
00319             break;*/
00320         }
00321 
00322         if (dirty)
00323         {
00324             //toolPose = arm_->getToolPose();//const char frame[] = "base_link");
00325             dist *= 5;
00326             toolPose.setOrigin(toolPose.getOrigin() + dist);
00327             //dist = btVector3(0,0,0);
00328             arm_->move_toolframe_ik_pose(toolPose);
00329             dirty = false;
00330         }
00331 
00332         if (grip_dirty)
00333         {
00334             //gripper->clos
00335             gripper_->closeHard(grip_open);
00336             for (int m = 0 ; m < 5; ++m)
00337             {
00338                 double am_open = gripper_->getAmountOpen();
00339                 ROS_INFO("GRIPPER OPENING :%f (getAmountOpen reports %f)", grip_open, am_open);
00340             }
00341             grip_dirty = false;
00342         }
00343 
00344 
00345         //if (dirty == true)
00346         //{
00347         //vel_pub_.publish(cmd);
00348         //}
00349 
00350 
00351     }
00352 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 09:44:57