Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00048 #include <ros/ros.h>
00049 #include <vision_srvs/cop_call.h>
00050 #include <vision_msgs/cop_answer.h>
00051
00052
00053 #include <cstdlib>
00054 #include <ctype.h>
00055 #include <SWI-Prolog.h>
00056 #include <vector>
00057
00058
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
00068 struct answer_st
00069 {
00070 int objId;
00071 double prob;
00072 int loId;
00073 };
00074
00075 vector<answer_st> answer_vec;
00076
00077
00078
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
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
00150 call.request.object_classes.pop_back();
00151
00152 call.request.object_classes.push_back("green");
00153
00154
00155
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
00196 install_t
00197 install()
00198 {
00199 PL_register_foreign("getCopClustersColorROS", 1, (void *)pl_getCopClustersColorROS, 0);
00200 }
00201 }