$search
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 }