test_cop.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2009, U. Klank   klank@in.tum.de
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 }


test_client
Author(s): Ulrich F Klank
autogenerated on Mon Oct 6 2014 09:13:32