Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef TOOLS_H
00018 #define TOOLS_H
00019
00020 #include <ros/ros.h>
00021
00022 namespace moveit_simple_actions
00023 {
00024
00025 void printTutorial(const std::string robot_name)
00026 {
00027 std::cout << "\n \n"
00028 << "************************************************\n"
00029 << "Tutorial for simple actions with Romeo, NAO, and Pepper.\n"
00030 << "You have chosen -" << robot_name << "- robot. \n\n"
00031
00032 << "In your scene, you should see:\n"
00033 << "- a robot\n"
00034 << "- an object\n"
00035 << "- [optionally] a table. \n\n"
00036
00037 << "The scenario is\n"
00038 << "1) Try to grasp an object (for Romeo only) by pressing -g- + Enter.\n"
00039 << "2) If the grasp is successful, place the object by typing -place-\n"
00040 << " If not, try an approximative grasp by typing -u- + Enter.\n"
00041 << "3) Try to move the object right/left/top/down/farther/closer\n"
00042 << " by pressing -s- -f- -e- -x- -r- -c- keys and grasp again.\n"
00043 << "4) Try to grasp with another hand.\n"
00044 << "************************************************\n"
00045 << std::endl;
00046 }
00047
00048 void printAllActions()
00049 {
00050 ROS_INFO_STREAM("Possible actions are: \n"
00051 << " g - pick an object, \n"
00052 << " p - place the object, \n"
00053
00054 << " u - reach and grasp, \n"
00055 << " reachtop - reach the object from top, \n"
00056
00057
00058 << " a - plan all possible grasps, \n"
00059
00060
00061 << " i - go to init pose (i1, i2, i3, i0), \n"
00062 << " get_pose - show the pre-grasp pose, \n"
00063 << " hand_open - open hand, \n"
00064 << " hand_close - close hand, \n"
00065 << " look_down - move the head to look down, \n"
00066 << " look_zero - move the head to the zero pose, \n"
00067
00068 << " d - detect objects, \n"
00069 << " lc - add a cylinder on the left, \n"
00070 << " rc - add a cylinder on the right, \n"
00071 << " table - add/remove the table/scene, \n"
00072 << " set_table_height - set the table height, \n"
00073
00074 << " next_obj - process the next object, \n"
00075 << " e/s/f/x/r/c - move the object top/left/right/down/closer/farther \n"
00076
00077 << " switcharm - switch the active arm, \n"
00078
00079 << " test_pick - test the goal space for picking, \n"
00080 << " test_reach - test the goal space for reaching, \n"
00081
00082
00083 << " q - exit, \n"
00084 << " So, what do you choose?"
00085 );
00086 }
00087
00088 int promptUserAction()
00089 {
00090 printAllActions();
00091 char ascii;
00092 std::cin >> ascii;
00093 int in = (int) ascii;
00094 return in;
00095 }
00096
00097 std::string promptUserQuestionString()
00098 {
00099 printAllActions();
00100 std::string input;
00101 std::cin >> input;
00102 return input;
00103 }
00104
00105 bool promptUserQuestion(const std::string command)
00106 {
00107 ROS_INFO_STREAM_NAMED("pick_place",command);
00108 char input;
00109 std::cin >> input;
00110 if( input == 'n' )
00111 return false;
00112
00113 return true;
00114 }
00115
00116 double promptUserValue(const std::string command)
00117 {
00118 ROS_INFO_STREAM_NAMED("pick_place",command);
00119 double input;
00120 std::cin >> input;
00121 return input;
00122 }
00123
00124 double promptUserInt(const std::string command)
00125 {
00126 ROS_INFO_STREAM_NAMED("pick_place",command);
00127 double input;
00128 std::cin >> input;
00129 return input;
00130 }
00131
00132 bool promptUser()
00133 {
00134 ROS_INFO_STREAM_NAMED("pick_place","Retry? (y/n)");
00135 char input;
00136 std::cin >> input;
00137 if( input == 'n' )
00138 return false;
00139
00140 return true;
00141 }
00142 }
00143 #endif // TOOLS_H