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 <vision_srvs/cop_call.h>
00032 #include <vision_srvs/srvjlo.h>
00033 #include <vision_msgs/cop_answer.h>
00034 #include <stdio.h>
00035
00036 using namespace vision_srvs;
00037 using namespace vision_msgs;
00038
00039 bool breaker = false;
00040 unsigned long vision_primitive = 0;
00041
00042 void callback(const boost::shared_ptr<const cop_answer> &msg)
00043 {
00044 printf("got answer from cop! (Errors: %s)\n", msg->error.c_str());
00045 if(vision_primitive == msg->perception_primitive)
00046 {
00047 for(size_t i = 0; i < msg->found_poses.size(); i++)
00048 {
00049 const aposteriori_position &pos = msg->found_poses[i];
00050 printf("Found Obj nr %d with prob %f at pos %d\n", (int)pos.objectId, pos.probability, (int)pos.position);
00051 for(size_t j = 0; j < pos.models.size(); j++)
00052 {
00053 printf(" %s", pos.models[j].sem_class.c_str());
00054 }
00055 if(pos.models.size() > 0)
00056 printf("\n");
00057 }
00058 printf("End!\n");
00059 breaker = true;
00060 }
00061 else
00062 {
00063 printf("wrong vision primitive, continue waiting\n");
00064 }
00065 }
00066
00067 int main(int argc, char* argv[])
00068 {
00069 if(argc == 1)
00070 {
00071 printf("Usage: \n%s ClassName LoID NumObjects Command\n\nClassName can be Cluster, Mug, IceTea, black, white, AssamBlend, ...\nLoID is a search space (use querylo to resolve tf-names to LoIDs)\nNumObjects specifies the maximum number of objects returned\nCommand Defaul is 0:=Locate 256:=Track 768:=Refine 2048:=StopTrack 25600:=LookUp, Show\n", argv[0]);
00072 return 0;
00073 }
00074 ros::init(argc, argv, "cop_testclient", ros::init_options::AnonymousName) ;
00075 cop_call call;
00076 cop_answer answer;
00077 ros::NodeHandle n;
00078
00080 std::string stTopicName = "/tracking/cop_handler";
00082 call.request.outputtopic = stTopicName;
00083 int index_of_args = 1;
00084 char name[75];
00085 if(argc > index_of_args)
00086 {
00087 if(strcmp("--name", argv[1]) == 0)
00088 {
00089 strcpy(name, argv[2]);
00090 index_of_args += 2;
00091 }
00092 else
00093 {
00094 strcpy(name, "cop");
00095 }
00096 if(atoi(argv[index_of_args]) == 0)
00097 call.request.object_classes.push_back(argv[index_of_args]);
00098 else
00099 call.request.object_ids.push_back(atoi(argv[index_of_args]));
00100 }
00101 else
00102 call.request.object_classes.push_back( "Mug");
00103 printf("Index of args %ld\n",index_of_args);
00104 call.request.action_type = argc > index_of_args+3 ? atoi(argv[index_of_args + 3]) : 0;
00105 if(argc > index_of_args+3 && call.request.action_type == 0 && strlen(argv[index_of_args + 3]) > 3)
00106 {
00107 if(strcmp("Refine", argv[index_of_args+3]) == 0)
00108 {
00109 call.request.action_type = 768;
00110 }
00111 else if(strcmp("Track", argv[index_of_args+3]) == 0)
00112 {
00113 call.request.action_type = 256;
00114 }
00115 else if(strcmp("StopTrack", argv[index_of_args+3]) == 0)
00116 {
00117 call.request.action_type = 2048;
00118 }
00119 else if(strcmp("Show", argv[index_of_args+3]) == 0 || strcmp("LookUp", argv[index_of_args+3]) == 0)
00120 {
00121 call.request.action_type = 25600;
00122 }
00123 }
00124 if( argc > index_of_args+4)
00125 {
00126 call.request.object_classes.push_back(argv[index_of_args+4]);
00127 }
00128 call.request.number_of_objects = argc > index_of_args+2 ? atoi(argv[index_of_args+2]): 1;
00129 apriori_position pos;
00130 pos.probability = 1.0;
00131 if(argc > index_of_args+1)
00132 {
00133 pos.positionId = atoi(argv[index_of_args+1]);
00134 if( pos.positionId==0 && strlen(argv[index_of_args+1]) > 1)
00135 {
00136 vision_srvs::srvjlo msg;
00137 msg.request.command = "namequery";
00138 msg.request.query.name = argv[index_of_args+1];
00139 ros::ServiceClient client = n.serviceClient<srvjlo>("/located_object", true);
00140 if (!client.call(msg))
00141 {
00142 printf("Error calling jlo!\n");
00143 }
00144 if (msg.response.error.length() > 0)
00145 {
00146 printf("Error from jlo: %s!\n", msg.response.error.c_str());
00147 return 0;
00148 }
00149 pos.positionId = msg.response.answer.id;
00150 }
00151 printf("Sending position ID : %ld\n", pos.positionId );
00152 call.request.list_of_poses.push_back(pos);
00153 if(argc > index_of_args+5)
00154 {
00155 if(atoi(argv[index_of_args+3]) == 768)
00156 {
00157 for(int i = index_of_args+5; i < argc; i++)
00158 {
00159 call.request.object_classes.push_back(argv[i]);
00160 }
00161 }
00162 else
00163 {
00164 for(int i = index_of_args+5; i < argc; i++)
00165 {
00166 pos.positionId = atoi(argv[i]);
00167 call.request.list_of_poses.push_back(pos);
00168 }
00169 }
00170 }
00171 }
00173 ros::Subscriber read = n.subscribe<cop_answer>(stTopicName, 1000, &callback);
00175 char srvname[80];
00176 sprintf(srvname, "/%s/in", name);
00177 ros::service::waitForService(srvname);
00178 ros::ServiceClient client = n.serviceClient<cop_call>(srvname, true);
00179 if(!client.call(call))
00180 {
00181 printf("Error calling cop\n");
00182 return 0;
00183 }
00184 else
00185 printf("Called cop \n");
00186
00187 vision_primitive = call.response.perception_primitive;
00188
00189 ros::Rate r(100);
00190 while(n.ok() && !breaker)
00191 {
00192 ros::spinOnce();
00193 r.sleep();
00194 }
00195 return 0;
00196 }
