query_table_memory.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009 Dejan Pangercic <pangercic -=- cs.tum.edu>
00003  *
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *
00015  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025  * POSSIBILITY OF SUCH DAMAGE.
00026  *
00027  * $Id: query_table_memory.cpp 17089 2009-06-15 18:52:12Z pangercic $
00028  *
00029  */
00030 
00050 #include <ros/ros.h>
00051 
00052 #include <ias_table_srvs/ias_table_clusters_service.h>
00053 //prolog wrapper
00054 #include <cstdlib>
00055 #include <ctype.h>
00056 #include <SWI-Prolog.h>
00057 #include <vector>
00058 //#include <ias_table_msgs/PrologReturn.h>
00059 
00060 using namespace ias_table_srvs;
00061 
00062 extern "C"
00063 { 
00064   
00065   using namespace std;
00066 
00067   foreign_t
00068   pl_getTableMemoryObjects(term_t l)    
00069   {
00070     int argc;
00071     argc=1;
00072     char **argv;
00073     argv = (char **)malloc(sizeof(char*));
00074     argv[0] = (char *)malloc(sizeof(char) *2);
00075     argv[0][0] = 'd';
00076     argv[0][1] = '\0';
00077 
00078     term_t tmp = PL_new_term_ref();
00079     
00080     ros::init(argc, argv, "testclient") ;
00081     ias_table_clusters_service srv;
00082     ros::NodeHandle n;
00083     std::string table_memory_srvs("/table_memory/table_memory_clusters_service");
00085     ros::service::waitForService(table_memory_srvs);
00086     ros::ServiceClient client = n.serviceClient<ias_table_clusters_service>(table_memory_srvs, true);
00087     float object_center[3];
00088     int table_center_size = 19;
00089     float table_center[table_center_size];
00090     for (int t = 0; t < 19; t++)
00091     {
00092       table_center[t] = 0.0;
00093     }
00094     double old_stamp = 0.0;
00095     int old_stamp_diff = 0;
00096     if(!client.call(srv))
00097     {
00098       ROS_ERROR("ERROR Calling %s", table_memory_srvs.c_str());
00099       return 0;
00100     }
00101     else
00102     {
00103       ROS_DEBUG("Called %s", table_memory_srvs.c_str());
00104     }
00105     ROS_DEBUG("Calling %s, prolog_return size %ld", table_memory_srvs.c_str(),  srv.response.prolog_return.size());
00106     //if (srv.response.prolog_return.size() != 0)
00107     //old_stamp = srv.response.prolog_return[0].stamp.toSec();
00108     for (unsigned int i = 0; i < srv.response.prolog_return.size(); i++)
00109           { 
00110             //add response variables of choice
00111             //table id
00112             if (!PL_unify_list(l, tmp, l) ||
00113           !PL_unify_integer(tmp, srv.response.prolog_return[i].table_id))
00114               PL_fail;
00115             ROS_DEBUG("table_id");
00116 
00117             //check if the timestamp changed
00118             // ROS_DEBUG("old_stamp_diff: %lf", srv.response.prolog_return[i].stamp.toSec() - old_stamp);
00119 //          if ((srv.response.prolog_return[i].stamp.toSec() - old_stamp) > 0.0)
00120 //       {
00121 //         ROS_DEBUG("old_stamp_diff: %f, %d", srv.response.prolog_return[i].stamp.toSec() - old_stamp, old_stamp_diff);
00122 //         old_stamp_diff++;
00123 //              old_stamp = srv.response.prolog_return[i].stamp.toSec();
00124 //       }
00125 
00126             //time stamp as a difference to the beginning of the table memory life cycle
00127             if (!PL_unify_list(l, tmp, l) ||
00128           !PL_unify_float(tmp, (srv.response.prolog_return[i].stamp.toSec())))
00129               PL_fail;
00130             ROS_DEBUG("stamp: %f", srv.response.prolog_return[i].stamp.toSec());
00131 
00132             //table center
00133             //table_center[0] = srv.response.prolog_return[i].table_center.x;
00134             //table_center[1] = srv.response.prolog_return[i].table_center.y;
00135             //table_center[2] = srv.response.prolog_return[i].table_center.z;
00136       for (int j = 0; j < 3; j++)
00137       {
00138         //orientation
00139         table_center[j*4] = srv.response.prolog_return[i].coeff[3*j+6];
00140         table_center[j*4 + 1] = srv.response.prolog_return[i].coeff[3*j+7];
00141         table_center[j*4 + 2] = srv.response.prolog_return[i].coeff[3*j+8];
00142         //position
00143         table_center[j*4 + 3] = srv.response.prolog_return[i].coeff[j];
00144         //dimensions
00145         table_center[j + 16] = srv.response.prolog_return[i].coeff[j+3];
00146       }
00147       table_center[15] = 1.0;
00148             for (int ii = 0; ii < table_center_size; ii++)
00149       {
00150         if(!PL_unify_list(l, tmp, l) ||  !PL_unify_float(tmp, table_center[ii]))
00151         {
00152           PL_fail;
00153           break;
00154         }
00155       }
00156             ROS_DEBUG("table_center");
00157             
00158 
00159             //object type
00160       if (srv.response.prolog_return[i].object_type != "")
00161       {
00162         if (!PL_unify_list(l, tmp, l) ||
00163             !PL_unify_string_chars(tmp, srv.response.prolog_return[i].object_type.c_str()))
00164           PL_fail;
00165         ROS_DEBUG("object type %s",  srv.response.prolog_return[i].object_type.c_str());
00166       }
00167       else
00168       {
00169         if (!PL_unify_list(l, tmp, l) ||
00170             !PL_unify_string_chars(tmp, "nn"))
00171           PL_fail;
00172         ROS_DEBUG("object type nn");
00173       }
00174 
00175       //object color
00176       if (srv.response.prolog_return[i].object_color != "")
00177       {
00178         if (!PL_unify_list(l, tmp, l) ||
00179             !PL_unify_string_chars(tmp, srv.response.prolog_return[i].object_color.c_str()))
00180           PL_fail;
00181         ROS_DEBUG("object color %s",  srv.response.prolog_return[i].object_color.c_str());
00182       }
00183       else
00184       {
00185         if (!PL_unify_list(l, tmp, l) ||
00186             !PL_unify_string_chars(tmp, "nn"))
00187           PL_fail;
00188         ROS_DEBUG("object color nn");
00189       }
00190 
00191       //object geometric type
00192       if (srv.response.prolog_return[i].object_geometric_type != "")
00193       {
00194         if (!PL_unify_list(l, tmp, l) ||
00195             !PL_unify_string_chars(tmp, srv.response.prolog_return[i].object_geometric_type.c_str()))
00196           PL_fail;
00197         ROS_DEBUG("object geometric type %s",  srv.response.prolog_return[i].object_geometric_type.c_str());
00198       }
00199       else
00200       {
00201         if (!PL_unify_list(l, tmp, l) ||
00202             !PL_unify_string_chars(tmp, "nn"))
00203           PL_fail;
00204         ROS_DEBUG("object geometric type nn");
00205       }
00206 
00207              
00208       //cluster id
00209       if (!PL_unify_list(l, tmp, l) ||
00210           !PL_unify_integer(tmp, srv.response.prolog_return[i].object_id))
00211         PL_fail;
00212       ROS_DEBUG("cluster_id");
00213 
00214       if (!PL_unify_list(l, tmp, l) ||
00215           !PL_unify_integer(tmp, srv.response.prolog_return[i].object_cop_id))
00216         PL_fail;
00217       ROS_DEBUG("object_cop_id");
00218              
00219       if (!PL_unify_list(l, tmp, l) ||
00220           !PL_unify_integer(tmp, srv.response.prolog_return[i].lo_id))
00221         PL_fail;
00222       ROS_DEBUG("cluster_id");
00223 
00224             //cluster center
00225             object_center[0] = srv.response.prolog_return[i].object_center.x;
00226             object_center[1] = srv.response.prolog_return[i].object_center.y;
00227             object_center[2] = srv.response.prolog_return[i].object_center.z;
00228             for (int ii = 0; ii < 3; ii++)
00229       {
00230         if(!PL_unify_list(l, tmp, l) ||  !PL_unify_float(tmp, object_center[ii]))
00231         {
00232           PL_fail;
00233           break;
00234         }
00235       }
00236             ROS_DEBUG("object_center");
00237           }
00238     free(argv[0]);
00239     free(argv);
00240     return PL_unify_nil(l);
00241   }
00242 
00244   // register foreign functions
00246   install_t
00247   install()
00248   { 
00249     PL_register_foreign("getTableMemoryObjects", 1, (void *)pl_getTableMemoryObjects, 0);
00250   }
00251 }


prolog_perception
Author(s): Dejan Pangercic
autogenerated on Mon Oct 6 2014 09:06:39