query_jlo.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/srvjlo.h>
00032 #include <stdio.h>
00033 using namespace vision_srvs;
00034 
00035 #define JLO_IDQUERY "idquery"
00036 #define JLO_FRAMEQUERY "framequery"
00037 #define JLO_DELETE "del"
00038 #define JLO_UPDATE "update"
00039 #define JLO_NAMEQUERY "namequery"
00040 #define ID_WORLD 1
00041 
00042 unsigned long ResolveName(ros::ServiceClient &client, const char* name_or_id)
00043 {
00044   if (atoi(name_or_id) == 0 && strlen(name_or_id) > 1)
00045   {
00046     srvjlo msg;
00047     printf("Resolving name: %s ->", name_or_id);
00048     msg.request.command = "namequery";
00049     msg.request.query.name = name_or_id;
00050     if (!client.call(msg))
00051     {
00052        printf("Error calling jlo!\n");
00053     }
00054     if (msg.response.error.length() > 0)
00055     {
00056        printf("Error from jlo: %s!\n", msg.response.error.c_str());
00057        return 0;
00058     }
00059     printf("%ld\n", msg.response.answer.id);
00060     return msg.response.answer.id;
00061   }
00062   else
00063       return atoi(name_or_id);
00064 }
00065 
00066 int main(int argc, char* argv[])
00067 {
00068   ros::init(argc, argv, "queryjlo") ;
00069   srvjlo msg;
00070   ros::NodeHandle n;
00071   ros::ServiceClient client = n.serviceClient<srvjlo>("/located_object", true);
00072 
00073   msg.request.command = JLO_IDQUERY;
00074   if (argc > 3)
00075   {
00076      msg.request.command = "coutlo";
00077       msg.request.query.id = ResolveName(client, argv[1]);
00078       msg.request.query.parent_id = ResolveName(client, argv[2]);
00079   }
00080   else if(argc > 2)
00081   {
00082      if(strcmp(argv[1], "del") == 0)
00083      {
00084        msg.request.command = JLO_DELETE;
00085        msg.request.query.id = atoi(argv[2]);
00086      }
00087      else
00088      {
00089       msg.request.command = JLO_FRAMEQUERY;
00090       msg.request.query.id = ResolveName(client, argv[1]);
00091       msg.request.query.parent_id = ResolveName(client, argv[2]);
00092     }
00093   }
00094   else if(argc > 1)
00095   {
00096     if(atoi(argv[1]) == 0)
00097     {
00098        msg.request.command = JLO_NAMEQUERY;
00099        msg.request.query.name = argv[1];
00100     }
00101     else
00102       msg.request.query.id = ResolveName(client, argv[1]);
00103 
00104   }
00105   else
00106   {
00107     printf("Usage: ./query_jlo (ID [PARENT_ID]  |  NAME)\n");
00108     msg.request.query.id = ID_WORLD;
00109   }
00110   //printf("Asking command=%s with id=%ld, parent=%ld and name=%s\n", msg.request.command.c_str(), msg.request.query.id, msg.request.query.parent_id, msg.request.query.name.c_str());
00111 
00112   if (!client.call(msg))
00113   {
00114     printf("Error calling jlo!\n");
00115   }
00116   if (msg.response.error.length() > 0)
00117   {
00118     printf("Error from jlo: %s!\n", msg.response.error.c_str());
00119     return 0;
00120   }
00121   int width2 = 4;
00122   printf("Showing PosId %d (%s) with parent %d:\n", (int)msg.response.answer.id, msg.response.answer.name.c_str(), (int)msg.response.answer.parent_id);
00123 
00124   for(int r = 0; r < width2; r++)
00125   {
00126     for(int c = 0; c < width2; c++)
00127     {
00128         printf( "%f ", msg.response.answer.pose[r * width2 + c]);
00129 
00130     }
00131     printf("\n");
00132   }
00133   printf("Cov:\n");
00134   width2 = 6;
00135   for(int r = 0; r < width2; r++)
00136   {
00137     for(int c = 0; c < width2; c++)
00138     {
00139         printf( "%f ", msg.response.answer.cov[r * width2 + c]);
00140 
00141     }
00142     printf("\n");
00143   }
00144   printf("Type: %d\n", msg.response.answer.type);
00145   printf("pos dist: %f\n", sqrt(msg.response.answer.pose[3] * msg.response.answer.pose[3] + msg.response.answer.pose[7]*msg.response.answer.pose[7]+msg.response.answer.pose[11]*msg.response.answer.pose[11]) );
00146   return 0;
00147 }


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