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 #include <std_msgs/String.h>
00033 #include <sstream>
00034
00035 ros::Subscriber sr;
00036
00037 std::map<long, std::string> mapping;
00038
00039 void cb_logging(const std_msgs::StringConstPtr& str)
00040 {
00041 long test = atoi((*str).data.c_str());
00042 if(test != 0)
00043 {
00044 mapping[test] = (*str).data;
00045 }
00046 }
00047
00048 int main(int argc, char* argv[])
00049 {
00050 ros::init( argc, argv, "test_cop");
00051 ros::NodeHandle nh;
00052 CopClient cop(nh);
00053 sr = nh.subscribe("/cop/logging", 1, cb_logging);
00054
00055
00056 ros::Rate r(100);
00057 bool refine = false;
00058 bool grasp = true;
00059 bool arm = true;
00060 bool cheap = true;
00061 double xoff = 0.0, yoff = 0.0, zoff = 0.0;
00062
00063 unsigned long id_base_link = cop.LONameQuery("/base_link");
00064 unsigned long id_startpos = cop.LOFrameQuery(id_base_link, 1);
00065 unsigned long id_cam = cop.LONameQuery("/RightEyeCalc");
00066 if(argc > 1)
00067 refine = true;
00068
00069 unsigned long id_searchspace = cop.LONameQuery("/openni_rgb_optical_frame");
00070 if(id_searchspace == 0)
00071 id_searchspace = cop.LONameQuery("/narrow_stereo_optical_frame");
00072 std::string object = "Cluster";
00073 while(true)
00074 {
00075 long vision_primitive = cop.CallCop(object, id_searchspace, 10);
00076
00077
00078 size_t num_results_expected = 1;
00079
00080
00081 std::vector<vision_msgs::cop_answer> results;
00082 while(nh.ok())
00083 {
00084 if(cop.HasResult(vision_primitive) >= num_results_expected)
00085 break;
00086 ros::spinOnce();
00087 r.sleep();
00088 }
00089 std::vector<vision_msgs::cop_answer> result = cop.GetResult (vision_primitive);
00090 int j = 0;
00091 int i = 0;
00092 if(result.size() > 0)
00093 {
00094 system("reset");
00095 printf("============================\n Refine\n============================\n\n");
00096 for(j = 0; j < result[i].found_poses.size(); j++)
00097 {
00098 printf("%d: Refine %s (%ld at %ld)\n", j, result[i].found_poses[j].models[0].sem_class.c_str(),
00099 result[i].found_poses[j].objectId,
00100 result[i].found_poses[j].position);
00101 long vision_primitive = cop.CallCop("", result[i].found_poses[j].position, 1, result[i].found_poses[j].objectId, 256);
00102 printf("Wait for refine\n");
00103 while(nh.ok())
00104 {
00105 if(cop.HasResult(vision_primitive) >= num_results_expected)
00106 break;
00107 ros::spinOnce();
00108 r.sleep();
00109 }
00110 std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive);
00111
00112 result.push_back(result_tmp[0]);
00113 }
00114 }
00115 for(int bla = 1 ; bla < 2 ; bla++)
00116 for (int trials = 0; trials < 20; trials++)
00117 {
00118 system("reset");
00119 printf("============================\n Redetect \n============================\n\n");
00120 int i = 0;
00121 long vision_primitive_ss = cop.CallCop(object, id_searchspace, 10);
00122 size_t num_results_expected_ss = 1;
00123
00124
00125 std::vector<vision_msgs::cop_answer> results_ss;
00126 while(nh.ok())
00127 {
00128 if(cop.HasResult(vision_primitive_ss) >= num_results_expected_ss)
00129 break;
00130 ros::spinOnce();
00131 r.sleep();
00132 }
00133 results_ss = cop.GetResult (vision_primitive);
00134 std::vector<unsigned long> vec ;
00135
00136 for(i = 0; i < results_ss[0].found_poses.size(); i++)
00137 {
00138 vec.push_back(results_ss[0].found_poses[i].position);
00139 }
00140
00141 for(i = 1; i < result.size(); i++)
00142 {
00143 for(j = 0; j < result[i].found_poses.size(); j++)
00144 {
00145 printf("%d: Redetect %s (%ld at %ld)\n", i, result[i].found_poses[j].models[result[i].found_poses[j].models.size() - 1].sem_class.c_str(),
00146 result[i].found_poses[j].objectId,
00147 result[i].found_poses[j].position);
00148
00149 }
00150 int select = i;
00151
00152 if(result.size() > select && result[select].found_poses.size() > 0)
00153 {
00154 long vision_primitive = cop.CallCop("", vec, 1, result[select].found_poses[0].objectId, 0);
00155 printf("Wait for relocate\n");
00156 while(nh.ok())
00157 {
00158 if(cop.HasResult(vision_primitive) >= num_results_expected)
00159 break;
00160 ros::spinOnce();
00161 r.sleep();
00162 }
00163 std::vector<vision_msgs::cop_answer> result_tmp = cop.GetResult (vision_primitive);
00164 char txt[512];
00165 bool succes = false;
00166 double dist = 0.0, dist_cam = 0.0;
00167 if(result_tmp.size() > 0 && result_tmp[0].found_poses.size() > 0)
00168 {
00169 dist = cop.LODistanceQuery(result[select].found_poses[0].position, result_tmp[0].found_poses[0].position);
00170 dist_cam = cop.LODistanceQuery(result[select].found_poses[0].position, id_cam);
00171
00172 succes = true;
00173 }
00174 if(bla > 0)
00175 {
00176 if(dist < dist_cam*0.15)
00177 {
00178 cop.CopFeedBack(vision_primitive, 1.0, 0);
00179 }
00180 else
00181 {
00182 cop.CopFeedBack(vision_primitive, 0.1, 2048);
00183 }
00184 }
00185 std::string text;
00186 if(mapping.find(vision_primitive) != mapping.end())
00187 text = mapping[vision_primitive];
00188 sprintf(txt, "%ld %d %f %f %f %f %f %d %s\n", result[select].found_poses[0].objectId, succes ? 1 : 0, dist, xoff, yoff, zoff, dist_cam, bla, text.c_str());
00189
00190 FILE *file = fopen("results.txt", "a");
00191 fwrite(txt, sizeof(*txt), strlen(txt), file);
00192 fclose(file);
00193 }
00194 }
00195
00196
00197 while(true)
00198 {
00199
00200 xoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25;
00201 yoff = 0.5 * ((double)rand() / RAND_MAX) - 0.25;
00202
00203 if(xoff > 0.25)
00204 xoff = 0.25;
00205 if(xoff < -0.25)
00206 xoff = -0.25;
00207
00208
00209 if(yoff > 0.25)
00210 yoff = 0.25;
00211 if(yoff < -0.25)
00212 yoff = -0.25;
00213 unsigned long newpos = cop.LOOffset(0, id_base_link, xoff, yoff, zoff);
00214
00215 printf("GENERATED LO ID %ld\n\n\n", newpos);
00216 double dist = cop.LODistanceQuery(newpos, id_startpos);
00217 if(dist < 0.5)
00218 {
00219 char cmddrive[256];
00220 sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", newpos);
00221 printf(cmddrive);
00222 system(cmddrive);
00223 break;
00224 }
00225 else
00226 {
00227 printf("Its FAAAR away (dist = %f), ignore\n", dist);
00228 }
00229 cop.LOFreeID(newpos);
00230 }
00231
00232 }
00233
00234 while(true)
00235 {
00236 system("reset");
00237 printf("============================\n END\n============================\n\n");
00238
00239 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");
00240
00241 int select2;
00242 int test = scanf("%d", &select2);
00243 printf("Selected %d\n", select2);
00244 if(select2 == 0)
00245 {
00246 char cmddrive[256];
00247 sprintf(cmddrive, "rosrun drive_at drive_at.py -l %ld", id_startpos);
00248 system(cmddrive);
00249 break;
00250 }
00251 else if(select2 == 1)
00252 exit(0);
00253 else if(select2 == 2)
00254 arm = !arm;
00255 else if(select2 == 3)
00256 refine = !refine;
00257 else if(select2 == 4)
00258 grasp = !grasp;
00259 else if(select2 == 5)
00260 {
00261 char crash[200];
00262 test = scanf("%s", &crash);
00263 if(atoi(crash) != 0)
00264 id_searchspace = atoi(crash);
00265 else
00266 {
00267 unsigned long test = cop.LONameQuery(crash);
00268 if(test != 0)
00269 id_searchspace = test;
00270 }
00271 }
00272 else if(select2 == 6)
00273 {
00274 char crash[200];
00275 test = scanf("%s", &crash);
00276 object = crash;
00277 }
00278 else if (select2 == 7)
00279 {
00280 break;
00281 }
00282 }
00283 }
00284 return 0;
00285 }
