query_plane.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_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 }


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