$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_clusters_colors.cpp 17089 2009-06-15 18:52:12Z pangercic $ 00028 * 00029 */ 00030 00048 #include <ros/ros.h> 00049 #include <vision_srvs/cop_call.h> 00050 #include <vision_msgs/cop_answer.h> 00051 00052 //prolog wrapper 00053 #include <cstdlib> 00054 #include <ctype.h> 00055 #include <SWI-Prolog.h> 00056 #include <vector> 00057 00058 //callback bind 00059 #include <boost/bind.hpp> 00060 00061 extern "C" 00062 { 00063 using namespace vision_srvs; 00064 using namespace vision_msgs; 00065 using namespace std; 00066 00067 //cop answer format 00068 struct answer_st 00069 { 00070 int objId; 00071 double prob; 00072 int loId; 00073 }; 00074 //storage for cop answers 00075 vector<answer_st> answer_vec; 00076 00077 //if exit_cb == 1 -> query for clusters finished 00078 //if exit_cb == 2 -> query for colors finished 00079 int exit_cb = 0; 00080 00081 void callback(vector<answer_st> *prob, const boost::shared_ptr<const cop_answer> &msg) 00082 { 00083 answer_st answer_tmp; 00084 prob->clear(); 00085 ROS_DEBUG("got answer from cop! (Errors: %s)", msg->error.c_str()); 00086 ROS_DEBUG("msg size %ld", msg->found_poses.size()); 00087 for(unsigned int i = 0; i < msg->found_poses.size(); i++) 00088 { 00089 const aposteriori_position &pos = msg->found_poses[i]; 00090 ROS_DEBUG("Found Obj (callback) nr %d with prob %f at pos %d, exit: %d", (int)pos.objectId, pos.probability, (int)pos.position, exit_cb); 00091 answer_tmp.objId = (int)pos.objectId; 00092 answer_tmp.prob = pos.probability; 00093 answer_tmp.loId = (int)pos.position; 00094 prob->push_back(answer_tmp); 00095 } 00096 exit_cb++; 00097 ROS_DEBUG("End!"); 00098 } 00099 00100 foreign_t 00101 pl_getCopClustersColorROS(term_t l) 00102 { 00103 int argc; 00104 argc=1; 00105 char **argv; 00106 argv = (char **)malloc(sizeof(char*)); 00107 argv[0] = (char *)malloc(sizeof(char) *2); 00108 argv[0][0] = 'c'; 00109 argv[0][1] = '\0'; 00110 00111 term_t tmp = PL_new_term_ref(); 00112 00113 ros::init(argc, argv, "testclient") ; 00114 cop_call call; 00115 cop_answer answer; 00116 ros::NodeHandle n; 00117 00119 std::string stTopicName = "/tracking/out"; 00121 call.request.outputtopic = stTopicName; 00122 call.request.object_classes.push_back("Cluster"); 00123 call.request.action_type = 0; 00124 call.request.number_of_objects = 5; 00125 apriori_position pos; 00126 pos.probability = 1.0; 00127 00128 ros::Subscriber read = n.subscribe<cop_answer>(stTopicName, 1000, boost::bind(&callback, &answer_vec, _1)); 00129 00131 ros::service::waitForService("/tracking/in"); 00132 ros::ServiceClient client = n.serviceClient<cop_call>("/tracking/in", true); 00133 //call for clusters 00134 if(!client.call(call)) 00135 { 00136 ROS_DEBUG("Error calling cop\n"); 00137 } 00138 else 00139 { 00140 ROS_DEBUG("Called cop \n"); 00141 } 00142 00143 ros::Rate r(100); 00144 while(n.ok() && exit_cb < 1) 00145 { 00146 ros::spinOnce(); 00147 r.sleep(); 00148 } 00149 //call for color 00150 call.request.object_classes.pop_back(); 00151 //call.request.object_classes.push_back("red"); 00152 call.request.object_classes.push_back("green"); 00153 //call.request.object_classes.push_back("blue"); 00154 //call.request.object_classes.push_back("white"); 00155 //call.request.object_classes.push_back("black"); 00156 ROS_DEBUG("vector size %ld", answer_vec.size()); 00157 00158 for (unsigned int i =0; i < answer_vec.size(); i++) 00159 { 00160 pos.positionId = answer_vec[i].loId; 00161 call.request.list_of_poses.push_back(pos); 00162 } 00163 if(!client.call(call)) 00164 { 00165 ROS_DEBUG("Error calling cop\n"); 00166 } 00167 else 00168 { 00169 ROS_DEBUG("Called cop \n"); 00170 } 00171 00172 while(n.ok() && exit_cb < 2) 00173 { 00174 ros::spinOnce(); 00175 r.sleep(); 00176 } 00177 00178 for (unsigned int i =0; i < answer_vec.size(); i++) 00179 { 00180 if(answer_vec[i].prob > 0.5) 00181 { 00182 ROS_DEBUG("Unifying object %d with prob %f at pos %d", answer_vec[i].objId, answer_vec[i].prob, answer_vec[i].loId); 00183 if (!PL_unify_list(l, tmp, l) || 00184 !PL_unify_float(tmp, answer_vec[i].prob)) 00185 PL_fail; 00186 } 00187 } 00188 free(argv[0]); 00189 free(argv); 00190 return PL_unify_nil(l); 00191 } 00192 00194 // register foreign functions 00196 install_t 00197 install() 00198 { 00199 PL_register_foreign("getCopClustersColorROS", 1, (void *)pl_getCopClustersColorROS, 0); 00200 } 00201 }