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