$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_service_prolog.cpp 17089 2009-06-15 18:52:12Z pangercic $ 00028 * 00029 */ 00030 00048 #include "ros/ros.h" 00049 #include "ros/node_handle.h" 00050 #include <mapping_srvs/GetPlaneClusters.h> 00051 #include <cstdlib> 00052 #include <ctype.h> 00053 #include <SWI-Prolog.h> 00054 extern "C" 00055 { 00056 using namespace mapping_srvs; 00057 00058 foreign_t 00059 pl_getPlaneROS(term_t l) 00060 { 00061 int argc; 00062 argc=1; 00063 char **argv; 00064 argv = (char **)malloc(sizeof(char*)); 00065 argv[0] = (char *)malloc(sizeof(char) *2); 00066 argv[0][0] = 'b'; 00067 argv[0][1] = '\0'; 00068 ros::init(argc, argv, "query_service"); 00069 ros::NodeHandle n; 00070 ros::ServiceClient client = n.serviceClient<GetPlaneClusters>("get_detect_planes_service"); 00071 GetPlaneClusters srv; 00072 term_t tmp = PL_new_term_ref(); 00073 if (client.call(srv)) 00074 { 00075 //add response variables of choice 00076 if (!PL_unify_list(l, tmp, l) || 00077 !PL_unify_integer(tmp, srv.response.planeId)) 00078 PL_fail; 00079 ROS_DEBUG("Coeff a: %f", srv.response.a); 00080 if (!PL_unify_list(l, tmp, l) || 00081 !PL_unify_float(tmp, srv.response.a)) 00082 PL_fail; 00083 ROS_DEBUG("Coeff b: %f", srv.response.b); 00084 if (!PL_unify_list(l, tmp, l) || 00085 !PL_unify_float(tmp, srv.response.b)) 00086 PL_fail; 00087 ROS_DEBUG("Coeff c: %f", srv.response.c); 00088 if (!PL_unify_list(l, tmp, l) || 00089 !PL_unify_float(tmp, srv.response.c)) 00090 PL_fail; 00091 ROS_DEBUG("Coeff d: %f", srv.response.d); 00092 if (!PL_unify_list(l, tmp, l) || 00093 !PL_unify_float(tmp, srv.response.d)) 00094 PL_fail; 00095 00096 return PL_unify_nil(l); 00097 } 00098 else 00099 { 00100 ROS_ERROR("Failed to call service get_detect_planes_service"); 00101 return -1; 00102 } 00103 free(argv[0]); 00104 free(argv); 00105 return 0; 00106 } 00107 00109 // register foreign functions 00111 install_t 00112 install() 00113 { 00114 PL_register_foreign("getPlaneROS", 1, (void *)pl_getPlaneROS, 0); 00115 } 00116 }