carl_joy_teleop.cpp
Go to the documentation of this file.
00001 
00016 #include <carl_teleop/carl_joy_teleop.h>
00017 
00018 using namespace std;
00019 
00020 carl_joy_teleop::carl_joy_teleop() :
00021     acArm("carl_moveit_wrapper/common_actions/arm_action", true)
00022 {
00023   // a private handle for this ROS node (allows retrieval of relative parameters)
00024   ros::NodeHandle private_nh("~");
00025 
00026   private_nh.param<bool>("use_teleop_safety", use_teleop_safety, false);
00027 
00028   // create the ROS topics
00029   if(use_teleop_safety)
00030     cmd_vel = node.advertise<geometry_msgs::Twist>("cmd_vel_safety_check", 10);
00031   else
00032     cmd_vel = node.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00033   angular_cmd = node.advertise<wpi_jaco_msgs::AngularCommand>("jaco_arm/angular_cmd", 10);
00034   cartesian_cmd = node.advertise<wpi_jaco_msgs::CartesianCommand>("jaco_arm/cartesian_cmd", 10);
00035   cartesian_cmd2 = node.advertise<geometry_msgs::Twist>("carl_moveit_wrapper/cartesian_control", 10);
00036   asus_servo_tilt_cmd = node.advertise<std_msgs::Float64>("asus_controller/tilt", 10);
00037   creative_servo_pan_cmd = node.advertise<std_msgs::Float64>("creative_controller/pan", 10);
00038   joy_sub = node.subscribe<sensor_msgs::Joy>("joy", 10, &carl_joy_teleop::joy_cback, this);
00039 
00040   segment_client = node.serviceClient<std_srvs::Empty>("rail_segmentation/segment_auto");
00041   eStopClient = node.serviceClient<wpi_jaco_msgs::EStop>("jaco_arm/software_estop");
00042 
00043   // read in throttle values
00044   private_nh.param<double>("linear_throttle_factor_base", linear_throttle_factor_base, 1.0);
00045   private_nh.param<double>("angular_throttle_factor_base", angular_throttle_factor_base, 1.0);
00046   private_nh.param<double>("linear_throttle_factor_arm", linear_throttle_factor_arm, 1.0);
00047   private_nh.param<double>("angular_throttle_factor_arm", angular_throttle_factor_arm, 1.0);
00048   private_nh.param<double>("finger_throttle_factor", finger_throttle_factor, 1.0);
00049   string str;
00050   private_nh.param<string>("controller_type", str, "digital");
00051   if (str.compare("digital") == 0)
00052     controllerType = DIGITAL;
00053   else
00054     controllerType = ANALOG;
00055 
00056   // initialize everything
00057   leftBumperPrev = 0;
00058   rightBumperPrev = 0;
00059   rightStickPrev = 0;
00060   deadman = false;
00061   stopMessageSentArm = true;
00062   stopMessageSentFinger = true;
00063   EStopEnabled = false;
00064   helpDisplayed = false;
00065   mode = BASE_CONTROL;
00066   fingerCmd.position = false;
00067   fingerCmd.armCommand = false;
00068   fingerCmd.fingerCommand = true;
00069   fingerCmd.repeat = true;
00070   fingerCmd.fingers.resize(3);
00071   cartesianCmd.position = false;
00072   cartesianCmd.armCommand = true;
00073   cartesianCmd.fingerCommand = false;
00074   cartesianCmd.repeat = true;
00075 
00076   /*
00077   ROS_INFO("Waiting for home arm server...");
00078   acArm.waitForServer();
00079   ROS_INFO("Home arm server found.");
00080 */
00081 
00082   ROS_INFO("CARL Joystick Teleop Started");
00083 
00084   puts(" ---------------------------------------------------");
00085   puts("| CARL Joystick Teleop Help                         |");
00086   puts("|---------------------------------------------------|*");
00087   if (mode == BASE_CONTROL)
00088     puts("| Current Mode: Base Control                        |*");
00089   else if (mode == ARM_CONTROL)
00090     puts("| Current Mode: Arm Control                         |*");
00091   else if (mode == FINGER_CONTROL)
00092     puts("| Current Mode: Finger Control                      |*");
00093   puts("|---------------------------------------------------|*");
00094   puts("| For help and controls, press:                     |*");
00095   puts("|                          _                        |*");
00096   puts("|                        _| |_                      |*");
00097   puts("|  show finger controls |_   _| show base controls  |*");
00098   puts("|                         |_|                       |*");
00099   puts("|                  show arm controls                |*");
00100   puts("|                                                   |*");
00101   puts(" ---------------------------------------------------**");
00102   puts("  ****************************************************");
00103 
00104   if (controllerType == ANALOG)
00105   {
00106     initLeftTrigger = false;
00107     initRightTrigger = false;
00108     calibrated = false;
00109 
00110     ROS_INFO(
00111         "You specified a controller with analog triggers. This requires calibration before any teleoperation can begin.  Please press and release both triggers before continuing.");
00112   }
00113   else
00114     calibrated = true;
00115 }
00116 
00117 void carl_joy_teleop::joy_cback(const sensor_msgs::Joy::ConstPtr& joy)
00118 {
00119   // make sure triggers are calibrated before continuing if an analog controller was specified
00120   if (!calibrated)
00121   {
00122     if (!initLeftTrigger && joy->axes.at(2) == 1.0)
00123       initLeftTrigger = true;
00124 
00125     if (!initRightTrigger && joy->axes.at(5) == 1.0)
00126       initRightTrigger = true;
00127 
00128     if (initLeftTrigger && initRightTrigger)
00129     {
00130       calibrated = true;
00131       ROS_INFO("Controller calibration complete!");
00132     }
00133 
00134     return;
00135   }
00136 
00137   //software emergency stop for arm
00138   if (controllerType == DIGITAL)
00139   {
00140     if (joy->buttons.at(8) == 1)
00141     {
00142       EStopEnabled = true;
00143       wpi_jaco_msgs::EStop srv;
00144       srv.request.enableEStop = true;
00145       if (!eStopClient.call(srv))
00146         ROS_INFO("Couldn't call software estop service.");
00147     }
00148     else if (joy->buttons.at(9) == 1)
00149     {
00150       EStopEnabled = false;
00151       wpi_jaco_msgs::EStop srv;
00152       srv.request.enableEStop = false;
00153       if (!eStopClient.call(srv))
00154         ROS_INFO("Couldn't call software estop service.");
00155     }
00156   }
00157   else
00158   {
00159     if (joy->buttons.at(6) == 1)
00160     {
00161       EStopEnabled = true;
00162       wpi_jaco_msgs::EStop srv;
00163       srv.request.enableEStop = true;
00164       if (!eStopClient.call(srv))
00165         ROS_INFO("Couldn't call software estop service.");
00166     }
00167     else if (joy->buttons.at(7) == 1)
00168     {
00169       EStopEnabled = false;
00170       wpi_jaco_msgs::EStop srv;
00171       srv.request.enableEStop = false;
00172       if (!eStopClient.call(srv))
00173         ROS_INFO("Couldn't call software estop service.");
00174     }
00175   }
00176 
00177   //help menu
00178   if ((controllerType == DIGITAL && joy->axes.at(5) == -1.0) || (controllerType == ANALOG && joy->axes.at(7) == -1.0))
00179   {
00180     if (!helpDisplayed)
00181     {
00182       helpDisplayed = true;
00183       displayHelp(1);
00184     }
00185   }
00186   else if ((controllerType == DIGITAL && joy->axes.at(4) == 1.0)
00187       || (controllerType == ANALOG && joy->axes.at(6) == 1.0))
00188   {
00189     if (!helpDisplayed)
00190     {
00191       helpDisplayed = true;
00192       displayHelp(2);
00193     }
00194   }
00195   else if ((controllerType == DIGITAL && joy->axes.at(4) == -1.0)
00196       || (controllerType == ANALOG && joy->axes.at(6) == -1.0))
00197   {
00198     if (!helpDisplayed)
00199     {
00200       helpDisplayed = true;
00201       displayHelp(3);
00202     }
00203   }
00204   else if ((controllerType == DIGITAL && joy->axes.at(5) == 1.0)
00205       || (controllerType == ANALOG && joy->axes.at(7) == 1.0))
00206   {
00207     if (!helpDisplayed)
00208     {
00209       helpDisplayed = true;
00210       displayHelp(4);
00211     }
00212   }
00213   else
00214     helpDisplayed = false;
00215 
00216   //setup button indices for mode switching
00217   int button1Index, button2Index, button3Index, button4Index;
00218   bool modeChange;
00219   if (controllerType == DIGITAL)
00220   {
00221     button1Index = 0;
00222     button2Index = 1;
00223     button3Index = 2;
00224     button4Index = 3;
00225   }
00226   else
00227   {
00228     button1Index = 2;
00229     button2Index = 0;
00230     button3Index = 1;
00231     button4Index = 3;
00232   }
00233 
00234   bool was_pressed;
00235 
00236   switch (mode)
00237   {
00238     case BASE_CONTROL:
00239       // save the deadman switch state
00240       was_pressed = deadman;
00241 
00242       if (joy->buttons.at(4) == 1)
00243       {
00244         // left joystick controls the linear and angular movement
00245         twist.linear.x = joy->axes.at(1) * MAX_TRANS_VEL_BASE * linear_throttle_factor_base;
00246         if (controllerType == DIGITAL)
00247           twist.angular.z = joy->axes.at(2) * MAX_ANG_VEL_BASE * angular_throttle_factor_base;
00248         else
00249           twist.angular.z = joy->axes.at(3) * MAX_ANG_VEL_BASE * angular_throttle_factor_base;
00250 
00251         //boost throttle
00252         if (joy->buttons.at(5) != 1)
00253         {
00254           twist.linear.x *= NON_BOOST_THROTTLE;
00255           twist.angular.z *= NON_BOOST_THROTTLE;
00256         }
00257         deadman = true;
00258       }
00259       else
00260         deadman = false;
00261 
00262       //mode switching
00263       modeChange = false;
00264       if (joy->buttons.at(button1Index) == 1)
00265       {
00266         modeChange = true;
00267         mode = FINGER_CONTROL;
00268         ROS_INFO("Activated finger control mode");
00269       }
00270       else if (joy->buttons.at(button2Index) == 1)
00271       {
00272         modeChange = true;
00273         mode = ARM_CONTROL;
00274         ROS_INFO("Activated arm control mode");
00275       }
00276       else if (joy->buttons.at(button4Index) == 1)
00277       {
00278         modeChange = true;
00279         mode = SENSOR_CONTROL;
00280         ROS_INFO("Activated sensor control mode");
00281       }
00282 
00283       if (modeChange)
00284       {
00285         //cancel base motion
00286         twist.linear.x = 0.0;
00287         twist.angular.z = 0.0;
00288       }
00289 
00290       // send the twist command
00291       if (deadman || was_pressed)
00292         cmd_vel.publish(twist);
00293 
00294       break;
00295     case ARM_CONTROL:
00296       // left joystick controls the linear x and y movement
00297       cartesianCmd.arm.linear.x = joy->axes.at(1) * MAX_TRANS_VEL_ARM * linear_throttle_factor_arm;
00298       cartesianCmd.arm.linear.y = joy->axes.at(0) * MAX_TRANS_VEL_ARM * linear_throttle_factor_arm;
00299 
00300       //triggers control the linear z movement
00301       if (controllerType == DIGITAL)
00302       {
00303         if (joy->buttons.at(7) == 1)
00304           cartesianCmd.arm.linear.z = MAX_TRANS_VEL_ARM * linear_throttle_factor_arm;
00305         else if (joy->buttons.at(6) == 1)
00306           cartesianCmd.arm.linear.z = -MAX_TRANS_VEL_ARM * linear_throttle_factor_arm;
00307         else
00308           cartesianCmd.arm.linear.z = 0.0;
00309       }
00310       else
00311       {
00312         if (joy->axes.at(5) < 1.0)
00313           cartesianCmd.arm.linear.z = (0.5 - joy->axes.at(5) / 2.0) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00314         else
00315           cartesianCmd.arm.linear.z = -(0.5 - joy->axes.at(2) / 2.0) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00316       }
00317 
00318       //bumpers control roll
00319       if (joy->buttons.at(5) == 1)
00320         cartesianCmd.arm.angular.z = MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00321       else if (joy->buttons.at(4) == 1)
00322         cartesianCmd.arm.angular.z = -MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00323       else
00324         cartesianCmd.arm.angular.z = 0.0;
00325 
00326       //right joystick controls pitch and yaw
00327       if (controllerType == DIGITAL)
00328       {
00329         cartesianCmd.arm.angular.x = -joy->axes.at(3) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00330         cartesianCmd.arm.angular.y = joy->axes.at(2) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00331       }
00332       else
00333       {
00334         cartesianCmd.arm.angular.x = -joy->axes.at(4) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00335         cartesianCmd.arm.angular.y = joy->axes.at(3) * MAX_ANG_VEL_ARM * angular_throttle_factor_arm;
00336       }
00337 
00338       //for testing alternate Cartesian controllers...
00339       if (cartesianCmd.arm.linear.x != 0.0 || cartesianCmd.arm.linear.y != 0.0 || cartesianCmd.arm.linear.z != 0.0
00340       || cartesianCmd.arm.angular.x != 0.0 || cartesianCmd.arm.angular.y != 0.0
00341       || cartesianCmd.arm.angular.z != 0.0)
00342       {
00343         cartesianCmd2.linear.x = cartesianCmd.arm.linear.x;
00344         cartesianCmd2.linear.y = cartesianCmd.arm.linear.y;
00345         cartesianCmd2.linear.z = cartesianCmd.arm.linear.z;
00346         cartesianCmd2.angular.x = cartesianCmd.arm.angular.x;
00347         cartesianCmd2.angular.y = cartesianCmd.arm.angular.y;
00348         cartesianCmd2.angular.z = cartesianCmd.arm.angular.z;
00349       }
00350       else
00351       {
00352         cartesianCmd2.linear.x = 0.0;
00353         cartesianCmd2.linear.y = 0.0;
00354         cartesianCmd2.linear.z = 0.0;
00355         cartesianCmd2.angular.x = 0.0;
00356         cartesianCmd2.angular.y = 0.0;
00357         cartesianCmd2.angular.z = 0.0;
00358       }
00359 
00360       //mode switching
00361       if (joy->buttons.at(button1Index) == 1 || joy->buttons.at(button3Index) == 1 || joy->buttons.at(button4Index) == 1)
00362       {
00363         //cancel arm trajectory
00364         cartesianCmd.arm.linear.x = 0.0;
00365         cartesianCmd.arm.linear.y = 0.0;
00366         cartesianCmd.arm.linear.z = 0.0;
00367         cartesianCmd.arm.angular.x = 0.0;
00368         cartesianCmd.arm.angular.y = 0.0;
00369         cartesianCmd.arm.angular.z = 0.0;
00370         cartesian_cmd.publish(cartesianCmd);
00371         
00372         if (joy->buttons.at(button1Index) == 1)
00373         {
00374           mode = FINGER_CONTROL;
00375           ROS_INFO("Activated finger control mode");
00376         }
00377         else if (joy->buttons.at(button3Index) == 1)
00378         {
00379           mode = BASE_CONTROL;
00380           ROS_INFO("Activated base control mode");
00381         }
00382         else if (joy->buttons.at(button4Index) == 1)
00383         {
00384           mode = SENSOR_CONTROL;
00385           ROS_INFO("Activate sensor control mode");
00386         }
00387       }
00388       break;
00389     case FINGER_CONTROL:
00390       if (joy->axes.at(1) == 0.0)
00391       {
00392         //individual finger control
00393         //thumb controlled by right thumbstick
00394         if (controllerType == DIGITAL)
00395           fingerCmd.fingers[0] = -joy->axes.at(3) * MAX_FINGER_VEL * finger_throttle_factor;
00396         else
00397           fingerCmd.fingers[0] = -joy->axes.at(4) * MAX_FINGER_VEL * finger_throttle_factor;
00398 
00399         //top finger controlled by left triggers
00400         if (controllerType == DIGITAL)
00401         {
00402           if (joy->buttons.at(4) == 1)
00403             fingerCmd.fingers[1] = -MAX_FINGER_VEL * finger_throttle_factor;
00404           else if (joy->buttons.at(6) == 1)
00405             fingerCmd.fingers[1] = MAX_FINGER_VEL * finger_throttle_factor;
00406           else
00407             fingerCmd.fingers[1] = 0.0;
00408         }
00409         else
00410         {
00411           if (joy->buttons.at(4) == 1)
00412             fingerCmd.fingers[1] = -MAX_FINGER_VEL * finger_throttle_factor;
00413           else
00414             fingerCmd.fingers[1] = (0.5 - joy->axes.at(2) / 2.0) * MAX_FINGER_VEL * finger_throttle_factor;
00415         }
00416 
00417         //bottom finger controlled by right bumpers
00418         if (controllerType == DIGITAL)
00419         {
00420           if (joy->buttons.at(5) == 1)
00421             fingerCmd.fingers[2] = -MAX_FINGER_VEL * finger_throttle_factor;
00422           else if (joy->buttons.at(7) == 1)
00423             fingerCmd.fingers[2] = MAX_FINGER_VEL * finger_throttle_factor;
00424           else
00425             fingerCmd.fingers[2] = 0.0;
00426         }
00427         else
00428         {
00429           if (joy->buttons.at(5) == 1)
00430             fingerCmd.fingers[2] = -MAX_FINGER_VEL * finger_throttle_factor;
00431           else
00432             fingerCmd.fingers[2] = (0.5 - joy->axes.at(5) / 2.0) * MAX_FINGER_VEL * finger_throttle_factor;
00433         }
00434       }
00435       else
00436       {
00437         //control full gripper (outprioritizes individual finger control)
00438         fingerCmd.fingers[0] = -joy->axes.at(1) * MAX_FINGER_VEL * finger_throttle_factor;
00439         fingerCmd.fingers[1] = fingerCmd.fingers[0];
00440         fingerCmd.fingers[2] = fingerCmd.fingers[0];
00441       }
00442 
00443       //mode switching
00444       if (joy->buttons.at(button2Index) == 1 || joy->buttons.at(button3Index) == 1 || joy->buttons.at(button4Index) == 1)
00445       {
00446         //cancel finger trajectory
00447         fingerCmd.fingers[0] = 0.0;
00448         fingerCmd.fingers[1] = 0.0;
00449         fingerCmd.fingers[2] = 0.0;
00450         angular_cmd.publish(fingerCmd);
00451         
00452         if (joy->buttons.at(button2Index) == 1)
00453         {
00454           mode = ARM_CONTROL;
00455           ROS_INFO("Activated arm control mode");
00456         }
00457         else if (joy->buttons.at(button3Index) == 1)
00458         {
00459           mode=BASE_CONTROL;
00460           ROS_INFO("Activated base control mode");
00461         }
00462         else if (joy->buttons.at(button4Index) == 1)
00463         {
00464           mode = SENSOR_CONTROL;
00465           ROS_INFO("Activate sensor control mode");
00466         }
00467       }
00468     break;
00469     case SENSOR_CONTROL:
00470       std_msgs::Float64 cameraTiltCommand;
00471       std_msgs::Float64 cameraPanCommand;
00472 
00473       //asus tilt      
00474       if (controllerType == DIGITAL)
00475         cameraTiltCommand.data = joy->axes.at(3) * 10; //scaled up for smoother movement
00476       else
00477         cameraTiltCommand.data = joy->axes.at(4) * 10; //scaled up for smoother movement
00478       if (cameraTiltCommand.data != 0)
00479       {
00480         asus_servo_tilt_cmd.publish(cameraTiltCommand);
00481       }
00482       
00483       //creative pan
00484       cameraPanCommand.data = joy->axes.at(0) * 10; //scaled up for smoother movement
00485       if (cameraPanCommand.data != 0)
00486       {
00487         creative_servo_pan_cmd.publish(cameraPanCommand);
00488       }
00489 
00490       //bumpers to issue arm retract and home commands
00491       if (joy->buttons.at(4) != leftBumperPrev)
00492       {
00493         if (joy->buttons.at(4) == 1)
00494         {
00495           //send home command
00496           rail_manipulation_msgs::ArmGoal homeGoal;
00497           homeGoal.action = rail_manipulation_msgs::ArmGoal::READY;
00498           acArm.sendGoal(homeGoal);
00499         }
00500         leftBumperPrev = joy->buttons.at(4);
00501       }
00502       if (joy->buttons.at(5) != rightBumperPrev)
00503       {
00504         if (joy->buttons.at(5) == 1)
00505         {
00506           //send retract command
00507           rail_manipulation_msgs::ArmGoal retractGoal;
00508           retractGoal.action = rail_manipulation_msgs::ArmGoal::RETRACT;
00509           acArm.sendGoal(retractGoal);
00510         }
00511         rightBumperPrev = joy->buttons.at(5);
00512       }
00513 
00514       //stick click for segmentation
00515       int rightStickIndex;
00516       if (controllerType == DIGITAL)
00517         rightStickIndex = 11;
00518       else
00519         rightStickIndex = 10;
00520 
00521       if (joy->buttons.at(rightStickIndex) != rightStickPrev)
00522       {
00523         if (joy->buttons.at(rightStickIndex) == 1)
00524         {
00525           //send segment command
00526           std_srvs::Empty srv;
00527           if (!segment_client.call(srv))
00528           {
00529             ROS_INFO("Could not call segmentation client.");
00530           }
00531         }
00532         rightStickPrev = joy->buttons.at(rightStickIndex);
00533       }
00534 
00535       //mode switch
00536       if (joy->buttons.at(button2Index) == 1)
00537       {
00538         mode = ARM_CONTROL;
00539         ROS_INFO("Activated arm control mode");
00540       }
00541       else if (joy->buttons.at(button1Index) == 1)
00542       {
00543         mode = FINGER_CONTROL;
00544         ROS_INFO("Activate finger control mode");
00545       }
00546       else if (joy->buttons.at(button3Index) == 1)
00547       {
00548         mode=BASE_CONTROL;
00549         ROS_INFO("Activated base control mode");
00550       }
00551       
00552     break;
00553   }
00554 }
00555 
00556 void carl_joy_teleop::publish_velocity()
00557 {
00558   //publish arm stop commands if EStop is enabled (this will stop any teleoperation
00559   //using this node, but for any other nodes controlling the arm this will
00560   //instead significantly slow down any motion)
00561   if (EStopEnabled)
00562     return;
00563 
00564   switch (mode)
00565   {
00566     case ARM_CONTROL:
00567       //only publish stop message once; this allows other nodes to publish velocities
00568       //while the controller is not being used
00569       if (cartesianCmd.arm.linear.x == 0.0 && cartesianCmd.arm.linear.y == 0.0 && cartesianCmd.arm.linear.z == 0.0
00570           && cartesianCmd.arm.angular.x == 0.0 && cartesianCmd.arm.angular.y == 0.0
00571           && cartesianCmd.arm.angular.z == 0.0)
00572       {
00573         if (!stopMessageSentArm)
00574         {
00575           cartesian_cmd.publish(cartesianCmd);
00576           stopMessageSentArm = true;
00577         }
00578       }
00579       else
00580       {
00581         // send the arm command
00582         cartesian_cmd.publish(cartesianCmd);
00583         //cartesian_cmd2.publish(cartesianCmd2);
00584         stopMessageSentArm = false;
00585       }
00586       break;
00587     case FINGER_CONTROL:
00588       //only publish stop message once; this allows other nodes to publish velocities
00589       //while the controller is not being used
00590       if (fingerCmd.fingers[0] == 0.0 && fingerCmd.fingers[1] == 0.0 && fingerCmd.fingers[2] == 0.0)
00591       {
00592         if (!stopMessageSentFinger)
00593         {
00594           angular_cmd.publish(fingerCmd);
00595           stopMessageSentFinger = true;
00596         }
00597       }
00598       else
00599       {
00600         //send the finger velocity command
00601         angular_cmd.publish(fingerCmd);
00602         stopMessageSentFinger = false;
00603       }
00604       break;
00605   }
00606 }
00607 
00608 void carl_joy_teleop::displayHelp(int menuNumber)
00609 {
00610   puts(" ----------------------------------------");
00611   puts("| CARL Joystick Teleop Help              |");
00612   puts("|----------------------------------------|*");
00613   if (mode == ARM_CONTROL)
00614     puts("| Current Mode: Arm Control              |*");
00615   else if (mode == FINGER_CONTROL)
00616     puts("| Current Mode: Finger Control           |*");
00617   else if (mode == BASE_CONTROL)
00618     puts(" Current Mode: Base Control              |*");
00619   puts("|----------------------------------------|*");
00620   switch (menuNumber)
00621   {
00622     case 1:
00623       puts("|              Arm Controls              |*");
00624       puts("|   roll/down                 roll/up    |*");
00625       puts("|    ________                ________    |*");
00626       puts("|   /    _   \\______________/        \\   |*");
00627       puts("|  |   _| |_    < >    < >     (4)    |  |*");
00628       puts("|  |  |_   _|  Estop  start (1)   (3) |  |*");
00629       puts("|  |    |_|    ___      ___    (2)    |  |*");
00630       puts("|  |          /   \\    /   \\          |  |*");
00631       puts("|  |          \\___/    \\___/          |  |*");
00632       puts("|  |       x/y trans  pitch/yaw       |  |*");
00633       puts("|  |        _______/--\\_______        |  |*");
00634     break;
00635     case 2:
00636       puts("|            Finger Controls             |*");
00637       puts("| finger1 open/close  finger2 open/close |*");
00638       puts("|    ________                ________    |*");
00639       puts("|   /    _   \\______________/        \\   |*");
00640       puts("|  |   _| |_    < >    < >     (4)    |  |*");
00641       puts("|  |  |_   _|  Estop  start (1)   (3) |  |*");
00642       puts("|  |    |_|    ___      ___    (2)    |  |*");
00643       puts("|  |          /   \\    /   \\          |  |*");
00644       puts("|  |          \\___/    \\___/          |  |*");
00645       puts("|  | hand open/close  thumb open/close|  |*");
00646       puts("|  |        _______/--\\_______        |  |*");
00647     break;
00648     case 3:
00649       puts("|             Base Controls              |*");
00650       puts("| deadman/no function  boost/no function |*");
00651       puts("|    ________                ________    |*");
00652       puts("|   /    _   \\______________/        \\   |*");
00653       puts("|  |   _| |_    < >    < >     (4)    |  |*");
00654       puts("|  |  |_   _|  Estop  start (1)   (3) |  |*");
00655       puts("|  |    |_|    ___      ___    (2)    |  |*");
00656       puts("|  |          /   \\    /   \\          |  |*");
00657       puts("|  |          \\___/    \\___/          |  |*");
00658       puts("|  |        fwd/bkwd  left/right      |  |*");
00659       puts("|  |        _______/--\\_______        |  |*");
00660     break;
00661     case 4:
00662       puts("|            Sensor Controls             |*");
00663       puts("|    home arm              retract arm   |*");
00664       puts("|    ________                ________    |*");
00665       puts("|   /    _   \\______________/        \\   |*");
00666       puts("|  |   _| |_    < >    < >     (4)    |  |*");
00667       puts("|  |  |_   _|  Estop  start (1)   (3) |  |*");
00668       puts("|  |    |_|    ___      ___    (2)    |  |*");
00669       puts("|  |          /   \\    /   \\          |  |*");
00670       puts("|  |          \\___/    \\___/          |  |*");
00671       puts("|  |               asus tilt/segment|    |*");
00672       puts("|  |        _______/--\\_______        |  |*");
00673     break;
00674   }
00675   puts("|  |       |                  |       |  |*");
00676   puts("|   \\     /                    \\     /   |*");
00677   puts("|    \\___/                      \\___/    |*");
00678   puts("|                                        |*");
00679   puts("| Buttons:                               |*");
00680   puts("|   (1) Switch to finger control mode    |*");
00681   puts("|   (2) Switch to arm control mode       |*");
00682   puts("|   (3) Switch to base control mode      |*");
00683   puts("|   (4) Switch to sensor control mode    |*");
00684   puts(" ----------------------------------------**");
00685   puts("  *****************************************");
00686 }
00687 
00688 int main(int argc, char **argv)
00689 {
00690   // initialize ROS and the node
00691   ros::init(argc, argv, "carl_joy_teleop");
00692 
00693   // initialize the joystick controller
00694   carl_joy_teleop controller;
00695 
00696   ros::Rate loop_rate(60);  //rate at which to publish arm velocity commands
00697   while (ros::ok())
00698   {
00699     controller.publish_velocity();
00700     ros::spinOnce();
00701     loop_rate.sleep();
00702   }
00703 
00704   return EXIT_SUCCESS;
00705 }


carl_teleop
Author(s): Steven Kordell , Russell Toris , David Kent
autogenerated on Thu Jun 6 2019 21:09:59