$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_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