query_lo_cop.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_lo_cop.cpp 17089 2009-06-15 18:52:12Z pangercic $
00028  *
00029  */
00030 
00045 #include <ros/ros.h>
00046 #include <vision_srvs/srvjlo.h>
00047 #include <vision_srvs/cop_call.h>
00048 #include <vision_msgs/partial_lo.h>
00049 #include <vision_msgs/cop_answer.h>
00050 #include <geometry_msgs/Point32.h>
00051 #include <sensor_msgs/PointCloud.h>
00052 #include <point_cloud_mapping/geometry/statistics.h> 
00053 using namespace vision_srvs; 
00054 using namespace vision_msgs;
00055 using namespace geometry_msgs;
00056 using namespace std;
00057 using namespace sensor_msgs;
00058 
00059 #define JLO_IDQUERY "idquery"
00060 #define JLO_FRAMEQUERY "framequery"
00061 #define JLO_DELETE "del"
00062 #define JLO_UPDATE "update"
00063 #define ID_WORLD 1
00064 
00065 bool exit_cb = false;
00066 vector<Point32> cluster_centers_g, cluster_covs_g;
00067 void cop_cb(const boost::shared_ptr<const cop_answer> &msg)
00068 {
00069   ROS_DEBUG("got answer from cop! (Errors: %s)\n", msg->error.c_str());
00070   for(unsigned int i = 0; i < msg->found_poses.size(); i++)
00071     {
00072       const aposteriori_position &pos =  msg->found_poses[i];
00073       ROS_DEBUG("Foub Obj nr %d with prob %f at pos %d\n", (int)pos.objectId, pos.probability, (int)pos.position);
00074     }
00075   ROS_DEBUG("End!\n");
00076   exit_cb = true;
00077 }
00078 
00079 //Assuming to get clusters one by one on the topic
00080 //TODO: Implementation for the case when we get an annotated point cloud with 
00081 //multiple clusters
00082 void clusters_cb(const PointCloudConstPtr& cloud)
00083 {
00084   Point32 minP;
00085   Point32 maxP;
00086   Point32 center, extents;
00087   cloud_geometry::statistics::getMinMax (*cloud, minP, maxP);
00088   center.x = minP.x + (maxP.x - minP.x) * 0.5;                                                                                         
00089   center.y = minP.y + (maxP.y - minP.y) * 0.5;                                                                                         
00090   center.z = minP.z + (maxP.z - minP.z) * 0.5;   
00091   cluster_centers_g.push_back(center);
00092   extents.x = center.x - minP.x;                                                                                                                 
00093   extents.y = center.y - minP.y;                                                                                                                 
00094   extents.z = center.z - minP.z; 
00095   cluster_covs_g.push_back(extents);
00096 }
00097 
00098 vector<unsigned int> update_jlo(vector<Point32> cluster_centers, vector<Point32> cluster_covs, ros::ServiceClient client)
00099 {
00100   vector<unsigned int> lo_ids;
00101   for (unsigned int cluster = 0; cluster < cluster_centers.size();  cluster++)
00102     {
00103       srvjlo call;
00104       call.request.command = JLO_UPDATE;
00105       //world frame
00106       call.request.query.parent_id = 1;
00107       call.request.query.id = 0;
00108       
00109       //fill in pose
00110       int width = 4;
00111       for(int r = 0; r < width; r++)
00112         {
00113           for(int c = 0; c < width; c++)
00114             {
00115               if(r == c)
00116                 call.request.query.pose[r * width + c] = 1;
00117               else
00118                 call.request.query.pose[r * width + c] = 0;
00119             }
00120         }
00121       call.request.query.pose[3] = cluster_centers[cluster].x;
00122       call.request.query.pose[7] =  cluster_centers[cluster].y;
00123       call.request.query.pose[11] =  cluster_centers[cluster].z;
00124       
00125       //fill in covariance matrix: 0.9 * 1/2*max(cluster)-min(cluster) [m]
00126       width = 6;
00127       for(int r = 0; r < width; r++)
00128         {
00129           for(int c = 0; c < width; c++)
00130             {
00131               if(r == c)
00132                 call.request.query.cov[r * width + c] = 0.0;
00133               else
00134                 call.request.query.cov[r * width + c] = 0;
00135             }
00136         }
00137       call.request.query.cov[0] = cluster_covs[cluster].x;
00138       call.request.query.cov[7] = cluster_covs[cluster].y;
00139       call.request.query.cov[13] = cluster_covs[cluster].z;
00140       if (!client.call(call))
00141         {
00142           ROS_ERROR("Error in ROSjloComm: Update of pose information not possible!\n");
00143         }
00144       else if (call.response.error.length() > 0)
00145         {
00146           ROS_ERROR("Error from jlo: %s!\n", call.response.error.c_str());
00147         } 
00148       ROS_DEBUG("New Id: %ld (parent %ld)\n", call.response.answer.id, call.response.answer.parent_id);
00149       width = 4;
00150       for(int r = 0; r < width; r++)
00151         {
00152           for(int c = 0; c < width; c++)
00153             {
00154               ROS_DEBUG("%f", call.response.answer.pose[r * width + c]);
00155             }
00156         }
00157       lo_ids.push_back(call.response.answer.id);
00158     }
00159   return lo_ids;
00160 }
00161 
00162 bool call_cop(vector<unsigned int> pos_ids, std::string stTopicName, ros::ServiceClient client)
00163 { 
00164   vector<string> colors;
00165   colors.push_back("black"), colors.push_back("white");
00166   colors.push_back("red"), colors.push_back("green");
00167   colors.push_back("blue");
00168   for (unsigned int col = 0; col < colors.size(); col ++)
00169     {
00170       cop_call call;
00171       call.request.outputtopic = stTopicName;
00172       call.request.object_classes.push_back(colors[col]);
00173       call.request.action_type = 0;
00174       call.request.number_of_objects = 1;
00175       for (unsigned int obj = 0; pos_ids.size(); obj++)
00176         {
00177           apriori_position pos;
00178           pos.probability = 1.0;
00179           pos.positionId = pos_ids[obj];
00180           call.request.list_of_poses.push_back(pos);
00181         }
00182       if(!client.call(call))
00183         {
00184           ROS_DEBUG("Error calling cop\n");
00185           return false;
00186         }
00187       else
00188         {
00189           ROS_DEBUG("Called cop \n");
00190         }
00191     }
00192   return true;
00193 }
00194 
00195 // Main
00196 int main(int argc, char *argv[])
00197 {
00198   ros::init(argc, argv, "query_lo_cop") ;
00199   ros::NodeHandle n;
00200   //check if service servers are there
00201   if(!ros::service::waitForService("/located_object") || !ros::service::waitForService("/tracking/in"))
00202     return 0;
00203   ros::ServiceClient client_lo = n.serviceClient<srvjlo>("/located_object", true);
00204   ros::ServiceClient client_cop = n.serviceClient<cop_call>("/tracking/in", true);
00206   std::string cop_topic_name = "/tracking/out";
00207   ros::Subscriber read_cop_answer = n.subscribe<cop_answer>(cop_topic_name, 1000, &cop_cb);
00208   //subscribe to get clusters
00209   std::string clusters_topic_name = "/clusters";
00210   ros::Subscriber read_clusters = n.subscribe(clusters_topic_name, 1000, &clusters_cb);
00211   vector<unsigned int> lo_ids;
00212 
00213   lo_ids = update_jlo(cluster_centers_g, cluster_covs_g, client_lo);
00214   call_cop(lo_ids, cop_topic_name, client_cop);
00215   ros::Rate r(100);
00216   while(n.ok() && !exit_cb)
00217     {
00218       ros::spinOnce();
00219       r.sleep();
00220     }
00221   return 0;
00222 }
00223 
00224 
00225 


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