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
00024 ros::NodeHandle private_nh("~");
00025
00026 private_nh.param<bool>("use_teleop_safety", use_teleop_safety, false);
00027
00028
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
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
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
00078
00079
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
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
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
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
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
00240 was_pressed = deadman;
00241
00242 if (joy->buttons.at(4) == 1)
00243 {
00244
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
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
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
00286 twist.linear.x = 0.0;
00287 twist.angular.z = 0.0;
00288 }
00289
00290
00291 if (deadman || was_pressed)
00292 cmd_vel.publish(twist);
00293
00294 break;
00295 case ARM_CONTROL:
00296
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
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
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
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
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
00361 if (joy->buttons.at(button1Index) == 1 || joy->buttons.at(button3Index) == 1 || joy->buttons.at(button4Index) == 1)
00362 {
00363
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
00393
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
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
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
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
00444 if (joy->buttons.at(button2Index) == 1 || joy->buttons.at(button3Index) == 1 || joy->buttons.at(button4Index) == 1)
00445 {
00446
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
00474 if (controllerType == DIGITAL)
00475 cameraTiltCommand.data = joy->axes.at(3) * 10;
00476 else
00477 cameraTiltCommand.data = joy->axes.at(4) * 10;
00478 if (cameraTiltCommand.data != 0)
00479 {
00480 asus_servo_tilt_cmd.publish(cameraTiltCommand);
00481 }
00482
00483
00484 cameraPanCommand.data = joy->axes.at(0) * 10;
00485 if (cameraPanCommand.data != 0)
00486 {
00487 creative_servo_pan_cmd.publish(cameraPanCommand);
00488 }
00489
00490
00491 if (joy->buttons.at(4) != leftBumperPrev)
00492 {
00493 if (joy->buttons.at(4) == 1)
00494 {
00495
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
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
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
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
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
00559
00560
00561 if (EStopEnabled)
00562 return;
00563
00564 switch (mode)
00565 {
00566 case ARM_CONTROL:
00567
00568
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
00582 cartesian_cmd.publish(cartesianCmd);
00583
00584 stopMessageSentArm = false;
00585 }
00586 break;
00587 case FINGER_CONTROL:
00588
00589
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
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
00691 ros::init(argc, argv, "carl_joy_teleop");
00692
00693
00694 carl_joy_teleop controller;
00695
00696 ros::Rate loop_rate(60);
00697 while (ros::ok())
00698 {
00699 controller.publish_velocity();
00700 ros::spinOnce();
00701 loop_rate.sleep();
00702 }
00703
00704 return EXIT_SUCCESS;
00705 }