query_tabletop_missing_objects.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_tabletop_missing_objects.cpp 17089 2009-06-15 18:52:12Z pangercic $
00028  *
00029  */
00030 
00051 #include "ros/ros.h"
00052 #include "ros/node_handle.h"
00053 #include <ias_table_srvs/ias_table_clusters_service.h>
00054 #include <cstdlib>
00055 #include <ctype.h>
00056 #include <SWI-Prolog.h>
00057 extern "C"
00058 {
00059   using namespace  ias_table_srvs;    
00060 
00061   foreign_t
00062   pl_getFoundObjectsROS(term_t l)    
00063   {
00064     int argc;
00065     argc=1;
00066     char **argv;
00067     argv = (char **)malloc(sizeof(char*));
00068     argv[0] = (char *)malloc(sizeof(char) *2);
00069     argv[0][0] = 'e';
00070     argv[0][1] = '\0';
00071     ros::init(argc, argv, "query_tabletop_missing_objects");
00072     ros::NodeHandle n;
00073     std::string tabletop_missing_objects_srvs("/tabletop_missing_objects_server/tabletop_missing_objects_service");
00074     ros::service::waitForService(tabletop_missing_objects_srvs);
00075     ros::ServiceClient client = n.serviceClient<ias_table_clusters_service>(tabletop_missing_objects_srvs);
00076     ias_table_clusters_service srv;
00077     term_t tmp = PL_new_term_ref();
00078     float cluster_center[3];
00079     float table_center[3];
00080     ROS_DEBUG(" srv.response.table.header.seq");
00081     if (client.call(srv))
00082       {
00083         ROS_DEBUG("Calling %s, prolog_return size %ld", tabletop_missing_objects_srvs.c_str(),  srv.response.prolog_return.size());
00084         for (unsigned int i = 0; i < srv.response.prolog_return.size(); i++)
00085           { 
00086             //add response variables of choice
00087             if (!PL_unify_list(l, tmp, l) ||
00088                 !PL_unify_integer(tmp, srv.response.prolog_return[i].table_id))
00089               PL_fail;
00090             ROS_DEBUG("table_id");
00091             table_center[0] = srv.response.prolog_return[i].table_center.x;
00092             table_center[1] = srv.response.prolog_return[i].table_center.y;
00093             table_center[2] = srv.response.prolog_return[i].table_center.z;
00094             for (int ii = 0; ii < 3; ii++)
00095               {
00096                 if(!PL_unify_list(l, tmp, l) ||  !PL_unify_float(tmp, table_center[ii]))
00097                   {
00098                     PL_fail;
00099                     break;
00100                   }
00101               }
00102             ROS_DEBUG("table_center");
00103             
00104             if (!PL_unify_list(l, tmp, l) ||
00105                 !PL_unify_string_chars(tmp, srv.response.prolog_return[i].cluster_semantic_types[0].c_str()))
00106               PL_fail;
00107             ROS_DEBUG("semantic types %s",  srv.response.prolog_return[i].cluster_semantic_types[0].c_str());
00108 
00109             cluster_center[0] = srv.response.prolog_return[i].cluster_center.x;
00110             cluster_center[1] = srv.response.prolog_return[i].cluster_center.y;
00111             cluster_center[2] = srv.response.prolog_return[i].cluster_center.z;
00112             for (int ii = 0; ii < 3; ii++)
00113               {
00114                 if(!PL_unify_list(l, tmp, l) ||  !PL_unify_float(tmp, cluster_center[ii]))
00115                   {
00116                     PL_fail;
00117                     break;
00118                   }
00119               }
00120             ROS_DEBUG("cluster_center");
00121           }
00122         return PL_unify_nil(l);
00123         ROS_DEBUG("exiting if");
00124       }
00125     else
00126       {
00127         ROS_ERROR("Failed to call service %s",  tabletop_missing_objects_srvs.c_str());
00128         return -1;
00129       }
00130     free(argv[0]);
00131     free(argv);
00132     return 0;
00133   }
00134   
00136   // register foreign functions
00138   install_t
00139   install()
00140   { 
00141     PL_register_foreign("getFoundObjectsROS", 1, (void *)pl_getFoundObjectsROS, 0);
00142   }
00143 }


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