00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include <ros/ros.h>
00031 #include <cop_client_cpp/cop_client.h>
00032
00033 #include <sstream>
00034
00035 int main(int argc, char* argv[])
00036 {
00037 ros::init( argc, argv, "test_cop");
00038 ros::NodeHandle nh;
00039 CopClient cop(nh);
00040 ros::Rate r(100);
00041 bool refine = false;
00042 bool grasp = true;
00043 bool arm = true;
00044
00045 unsigned long id_startpos = cop.LONameQuery("/base_link");
00046 id_startpos = cop.LOFrameQuery(id_startpos, 1);
00047
00048 if(argc > 1)
00049 refine = true;
00050
00051 unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame");
00052 if(id_searchspace == 0)
00053 id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame");
00054 std::string object = "Cluster";
00055 while(true)
00056 {
00057 long vision_primitive = cop.CallCop(object, id_searchspace, 10);
00058
00059
00060 size_t num_results_expected = 1;
00061
00062
00063 std::vector<vision_msgs::cop_answer> results;
00064 while(nh.ok())
00065 {
00066 if(cop.HasResult(vision_primitive) >= num_results_expected)
00067 break;
00068 ros::spinOnce();
00069 r.sleep();
00070 }
00071 std::vector<vision_msgs::cop_answer> result = cop.GetResult (vision_primitive);
00072 int j = 0;
00073 int i = 0;
00074 while(refine && result.size() > 0)
00075 {
00076 system("reset");
00077 printf("============================\n Refine\n============================\n\n");
00078 for(j = 0; j < result[i].found_poses.size(); j++)
00079 {
00080 printf("%d: Refine %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(),
00081 result[i].found_poses[j].objectId,
00082 result[i].found_poses[j].position);
00083 }
00084
00085 printf("%d: %s\n Select cluster to refine: ", j, grasp ? "Continue to grasping": "End");
00086 int select = result.size();
00087 int test = scanf("%d", &select);
00088 printf("Selected %d\n", select);
00089 if((unsigned)select < result[i].found_poses.size())
00090 {
00091 printf("Refine of obj %ld , pos %ld\n", result[i].found_poses[select].objectId, result[i].found_poses[select].position);
00092 long vision_primitive = cop.CallCop("", result[i].found_poses[select].position, 1, result[i].found_poses[select].objectId, 256);
00093 printf("Wait for refine\n");
00094 while(nh.ok())
00095 {
00096 if(cop.HasResult(vision_primitive) >= num_results_expected)
00097 break;
00098 ros::spinOnce();
00099 r.sleep();
00100 }
00101 }
00102 else
00103 break;
00104
00105 system("reset");
00106 printf("============================\n Redetect\n============================\n\n");
00107 long vision_primitive_ss = cop.CallCop("", result[i].found_poses[select].position, 1, result[i].found_poses[select].objectId);
00108 size_t num_results_expected_ss = 1;
00109
00110
00111 std::vector<vision_msgs::cop_answer> results_ss;
00112 while(nh.ok())
00113 {
00114 if(cop.HasResult(vision_primitive_ss) >= num_results_expected_ss)
00115 break;
00116 ros::spinOnce();
00117 r.sleep();
00118 }
00119 results_ss = cop.GetResult (vision_primitive);
00120 for(j = 0; j < result[i].found_poses.size(); j++)
00121 {
00122 printf("%d: Found %s (%ld at %ld)\n", j, results_ss[0].found_poses[j].models[0].sem_class.c_str(),
00123 results_ss[0].found_poses[j].objectId,
00124 results_ss[0].found_poses[j].position);
00125 }
00126 int test10 = scanf("%d", &select);
00127
00128 }
00129 int result_sys_im = 0;
00130 if(grasp)
00131 {
00132 std::string arm_string = "right";
00133 if(!arm)
00134 arm_string = "left";
00135 system("reset");
00136 printf("============================\n Grasp\n============================\n\n");
00137 for(j = 0; j < result[i].found_poses.size(); j++)
00138 {
00139 printf("%d: Grasp %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(),
00140 result[i].found_poses[j].objectId,
00141 result[i].found_poses[j].position);
00142 }
00143
00144 printf("%d: End\n Select cluster to grab: ", j);
00145 int select = result.size();
00146 int test = scanf("%d", &select);
00147 printf("Selected %d\n", select);
00148 if((unsigned)select < result[i].found_poses.size())
00149 {
00150 std::ostringstream os;
00151 os << "\"";
00152 for(int k = 0; k < result[i].found_poses.size(); k++)
00153 {
00154 if(k != select)
00155 {
00156 os << result[i].found_poses[k].position << " ";
00157 }
00158
00159
00160 }
00161 os << "\"";
00162
00163 char cmd[1024];
00164 if( result[i].found_poses.size() > 1)
00165 sprintf(cmd, "rosrun demo_scripts armhand -s %s -r %ld -o %s", arm_string.c_str(), result[i].found_poses[select].position, os.str().c_str());
00166 else
00167 sprintf(cmd, "rosrun demo_scripts armhand -s %s -r %ld", arm_string.c_str(), result[i].found_poses[select].position);
00168 printf("Calling %s\n", cmd);
00169 result_sys_im = system(cmd);
00170 printf("\n\nSystem result: %d\n\n", result_sys_im);
00171 sleep(0.5);
00172 int result_sys;
00173 if(result_sys_im == 0 || 2560 == result_sys_im)
00174 {
00175 sprintf(cmd, "rosrun demo_scripts armhand -s %s -l 0.2", arm_string.c_str());
00176 result_sys = system(cmd);
00177 sleep(1.5);
00178
00179 sprintf(cmd, "rosrun demo_scripts armhand -s %s -l -0.2", arm_string.c_str());
00180 result_sys = system(cmd);
00181 sleep(1.0);
00182 sprintf(cmd, "rosrun demo_scripts armhand -s %s -g open_relax", arm_string.c_str());
00183 result_sys = system(cmd);
00184 sleep(0.5);
00185 sprintf(cmd, "rosrun demo_scripts armhand -s %s -l 0.3", arm_string.c_str());
00186 result_sys = system(cmd);
00187 sleep(1.0);
00188 }
00189 sprintf(cmd, "rosrun demo_scripts armhand -s %s -p open", arm_string.c_str());
00190 result_sys = system(cmd);
00191 sleep(0.1);
00192 }
00193 }
00194 while(true)
00195 {
00196 system("reset");
00197 printf("============================\n END\n============================\n\n");
00198 printf("\n\nResult of grasp was %d\n\n", result_sys_im);
00199
00200 printf("(0) Home & continue\n(1) END \n(2) Toggle arm\n(3) Toggle refine\n(4) Toggle grasp\n(5) Set search space\n(6) Set search term\n(7)continue w/o move");
00201
00202 int select2;
00203 int test = scanf("%d", &select2);
00204 printf("Selected %d\n", select2);
00205 if(select2 == 0)
00206 {
00207 char cmddrive[256];
00208 sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", id_startpos);
00209 system(cmddrive);
00210 break;
00211 }
00212 else if(select2 == 1)
00213 exit(0);
00214 else if (select2 == 2)
00215 arm = !arm;
00216 else if(select2 == 3)
00217 refine = !refine;
00218 else if(select2 == 4)
00219 grasp = !grasp;
00220 else if(select2 == 5)
00221 {
00222 char crash[200];
00223 test = scanf("%s", &crash);
00224 if(atoi(crash) != 0)
00225 id_searchspace = atoi(crash);
00226 else
00227 {
00228 unsigned long test = cop.LONameQuery(crash);
00229 if(test != 0)
00230 id_searchspace = test;
00231 }
00232 }
00233 else if(select2 == 6)
00234 {
00235 char crash[200];
00236 test = scanf("%s", &crash);
00237 object = crash;
00238 }
00239 else if (select2 == 7)
00240 {
00241 break;
00242 }
00243 }
00244 }
00245 return 0;
00246 }
