$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: bb_estimator_server.cpp 742 2012-04-25 15:18:28Z spanel $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Tomas Hodan (xhodan04@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 25.4.2012 (version 6.0) 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00053 #include <srs_env_model_percp/bb_estimator/funcs.h> 00054 #include <srs_env_model_percp/services_list.h> 00055 #include <srs_env_model_percp/topics_list.h> 00056 #include <srs_env_model_percp/parameters_list.h> 00057 00058 // Definition of the service for BB estimation 00059 #include "srs_env_model_percp/EstimateBB.h" 00060 #include "srs_env_model_percp/EstimateBBAlt.h" 00061 #include "srs_env_model_percp/EstimateRect.h" 00062 #include "srs_env_model_percp/EstimateRectAlt.h" 00063 #include "srs_env_model_percp/Estimate2DHullMesh.h" 00064 #include "srs_env_model_percp/Estimate2DHullPointCloud.h" 00065 00066 #include <algorithm> 00067 00068 #include <message_filters/subscriber.h> 00069 #include <message_filters/synchronizer.h> 00070 #include <message_filters/sync_policies/approximate_time.h> 00071 #include <message_filters/cache.h> 00072 00073 #include <sensor_msgs/Image.h> 00074 #include <sensor_msgs/CameraInfo.h> 00075 #include <sensor_msgs/image_encodings.h> 00076 #include <sensor_msgs/PointCloud2.h> 00077 00078 #include <arm_navigation_msgs/Shape.h> 00079 00080 #include <cv_bridge/cv_bridge.h> 00081 #include <opencv2/imgproc/imgproc.hpp> 00082 00083 #include <pcl/io/pcd_io.h> 00084 #include <pcl/point_types.h> 00085 #include <pcl/surface/convex_hull.h> 00086 00087 #include <tf/transform_listener.h> 00088 00089 using namespace cv; 00090 using namespace sensor_msgs; 00091 using namespace message_filters; 00092 00093 00094 namespace srs_env_model_percp 00095 { 00096 00097 // Cache 00098 message_filters::Cache<Image> depthCache; // Cache of Image messages 00099 message_filters::Cache<PointCloud2> pointCloudCache; // Cache of PointCloud2 messages 00100 message_filters::Cache<CameraInfo> camInfoCache; // Cache of CameraInfo messages 00101 00102 // Frame IDs 00103 std::string camFrameId; // Camera frame id (will be obtained automatically) 00104 std::string sceneFrameId; // Scene (world) frame id 00105 00106 // Subscription variants 00107 int subVariant = SV_NONE; 00108 00109 // TF listener 00110 tf::TransformListener *tfListener; 00111 00112 // Percentage of farthest points from mean considered as outliers when 00113 // calculating statistics of ROI 00114 int outliersPercent = OUTLIERS_PERCENT_DEFAULT; 00115 00116 // Percentage of rows and columns considered for sampling (for calculation of statistics) 00117 int samplingPercent = SAMPLING_PERCENT_DEFAULT; 00118 00119 // The required maximum ratio of sides length (the longer side is at maximum 00120 // sidesRatio times longer than the shorter one) 00121 double sidesRatio = SIDES_RATIO_DEFAULT; 00122 00123 // Modes of bounding box estimation 00124 int estimationMode = 1; 00125 00126 00127 /****************************************************************************** 00128 * Bounding box estimation service. 00129 * 00130 * @param req Request of type EstimateBB. 00131 * @param res Response of type EstimateBB. 00132 */ 00133 bool estimateBB_callback(srs_env_model_percp::EstimateBB::Request &req, 00134 srs_env_model_percp::EstimateBB::Response &res 00135 ) 00136 { 00137 if( DEBUG ) 00138 { 00139 std::cout << "EstimateBB service called:" 00140 << " p1.x = " << req.p1[0] 00141 << ", p1.y = " << req.p1[1] 00142 << ", p2.x = " << req.p2[0] 00143 << ", p2.y = " << req.p2[1] 00144 << std::endl; 00145 } 00146 00147 // Estimate the bounding box 00148 //-------------------------------------------------------------------------- 00149 00150 // Vertices (corners) defining the bounding box. 00151 // Each is noted using this template: 00152 // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back} 00153 // (This notation provides more readable code, in comparison with 00154 // e.g. an array with 8 items.) 00155 Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB; 00156 00157 if( !estimateBB(req.header.stamp, 00158 req.p1, req.p2, req.mode, 00159 bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB) ) 00160 { 00161 return false; 00162 } 00163 00164 // Calculate also Pose of the bounding box 00165 //-------------------------------------------------------------------------- 00166 tf::Quaternion q; 00167 Point3f p, s; 00168 00169 if( !estimateBBPose(bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB, 00170 p, q, s) ) 00171 { 00172 return false; 00173 } 00174 00175 // Set response 00176 //-------------------------------------------------------------------------- 00177 res.p1[0] = bbLBF.x; res.p1[1] = bbLBF.y; res.p1[2] = bbLBF.z; 00178 res.p2[0] = bbRBF.x; res.p2[1] = bbRBF.y; res.p2[2] = bbRBF.z; 00179 res.p3[0] = bbRTF.x; res.p3[1] = bbRTF.y; res.p3[2] = bbRTF.z; 00180 res.p4[0] = bbLTF.x; res.p4[1] = bbLTF.y; res.p4[2] = bbLTF.z; 00181 res.p5[0] = bbLBB.x; res.p5[1] = bbLBB.y; res.p5[2] = bbLBB.z; 00182 res.p6[0] = bbRBB.x; res.p6[1] = bbRBB.y; res.p6[2] = bbRBB.z; 00183 res.p7[0] = bbRTB.x; res.p7[1] = bbRTB.y; res.p7[2] = bbRTB.z; 00184 res.p8[0] = bbLTB.x; res.p8[1] = bbLTB.y; res.p8[2] = bbLTB.z; 00185 00186 res.pose.position.x = p.x; 00187 res.pose.position.y = p.y; 00188 res.pose.position.z = p.z; 00189 00190 // Keep orientation only for the first estimation mode. For the others, 00191 // the orientation is ignored (=> BB will be axis-aligned) and only position 00192 // and scale is preserved. 00193 // (The computed orientation represents the smallest rotation to achieve 00194 // an axis alignment.) 00195 if(estimationMode == 1) { 00196 res.pose.orientation.x = q.x(); 00197 res.pose.orientation.y = q.y(); 00198 res.pose.orientation.z = q.z(); 00199 res.pose.orientation.w = q.w(); 00200 } 00201 else { 00202 res.pose.orientation.x = 0; 00203 res.pose.orientation.y = 0; 00204 res.pose.orientation.z = 0; 00205 res.pose.orientation.w = 1; 00206 } 00207 00208 res.scale.x = s.x; 00209 res.scale.y = s.y; 00210 res.scale.z = s.z; 00211 00212 00213 00214 // Log request timestamp 00215 //-------------------------------------------------------------------------- 00216 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00217 00218 return true; 00219 } 00220 00221 00222 /****************************************************************************** 00223 * Bounding box estimation alternative service. 00224 * 00225 * @param req Request of type EstimateBBAlt. 00226 * @param res Response of type EstimateBBAlt. 00227 */ 00228 bool estimateBBAlt_callback(srs_env_model_percp::EstimateBBAlt::Request &req, 00229 srs_env_model_percp::EstimateBBAlt::Response &res 00230 ) 00231 { 00232 // Estimate the bounding box 00233 //-------------------------------------------------------------------------- 00234 00235 // Vertices (corners) defining the bounding box. 00236 // Each is noted using this template: 00237 // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back} 00238 // (This notation provides more readable code, in comparison with 00239 // e.g. an array with 8 items.) 00240 Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB; 00241 00242 if( !estimateBB(req.header.stamp, 00243 req.p1, req.p2, req.mode, 00244 bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB) ) 00245 { 00246 return false; 00247 } 00248 00249 // Calculate also Pose of the bounding box 00250 //-------------------------------------------------------------------------- 00251 tf::Quaternion q; 00252 Point3f p, s; 00253 00254 if( !estimateBBPose(bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB, 00255 p, q, s) ) 00256 { 00257 return false; 00258 } 00259 00260 // Set response 00261 //-------------------------------------------------------------------------- 00262 res.pose.position.x = p.x; 00263 res.pose.position.y = p.y; 00264 res.pose.position.z = p.z - 0.5f * s.z; 00265 00266 // Keep orientation only for the first estimation mode. For the others, 00267 // the orientation is ignored (=> BB will be axis-aligned) and only position 00268 // and scale is preserved. 00269 // (The computed orientation represents the smallest rotation to achieve 00270 // an axis alignment.) 00271 if(estimationMode == 1) { 00272 res.pose.orientation.x = q.x(); 00273 res.pose.orientation.y = q.y(); 00274 res.pose.orientation.z = q.z(); 00275 res.pose.orientation.w = q.w(); 00276 } 00277 else { 00278 res.pose.orientation.x = 0; 00279 res.pose.orientation.y = 0; 00280 res.pose.orientation.z = 0; 00281 res.pose.orientation.w = 1; 00282 } 00283 00284 res.bounding_box_lwh.x = 0.5f * s.x; 00285 res.bounding_box_lwh.y = 0.5f * s.y; 00286 res.bounding_box_lwh.z = s.z; 00287 00288 // Log request timestamp 00289 //-------------------------------------------------------------------------- 00290 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00291 00292 return true; 00293 } 00294 00295 00296 /****************************************************************************** 00297 * Rectangle estimation service. 00298 * 00299 * @param req Request of type EstimateRect. 00300 * @param res Response of type EstimateRect. 00301 */ 00302 bool estimateRect_callback(srs_env_model_percp::EstimateRect::Request &req, 00303 srs_env_model_percp::EstimateRect::Response &res 00304 ) 00305 { 00306 // Vertices (corners) defining the bounding box. 00307 // Each is noted using this template: 00308 // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back} 00309 // (This notation provides more readable code, in comparison with 00310 // e.g. an array with 8 items.) 00311 Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB; 00312 00313 // Vector with pointers to the BB vertices (to be able to iterate over them) 00314 vector<Point3f *> bbVertices(8); 00315 bbVertices[0] = &bbLBF; 00316 bbVertices[1] = &bbRBF; 00317 bbVertices[2] = &bbRTF; 00318 bbVertices[3] = &bbLTF; 00319 bbVertices[4] = &bbLBB; 00320 bbVertices[5] = &bbRBB; 00321 bbVertices[6] = &bbRTB; 00322 bbVertices[7] = &bbLTB; 00323 00324 // Calculate coordinates of corners of the bounding box relative to its center 00325 bbLBF.x = -0.5f * req.scale.x; 00326 bbLBF.y = -0.5f * req.scale.y; 00327 bbLBF.z = -0.5f * req.scale.z; 00328 00329 bbLTF.x = -0.5f * req.scale.x; 00330 bbLTF.y = -0.5f * req.scale.y; 00331 bbLTF.z = 0.5f * req.scale.z; 00332 00333 bbRBF.x = 0.5f * req.scale.x; 00334 bbRBF.y = -0.5f * req.scale.y; 00335 bbRBF.z = -0.5f * req.scale.z; 00336 00337 bbRTF.x = 0.5f * req.scale.x; 00338 bbRTF.y = -0.5f * req.scale.y; 00339 bbRTF.z = 0.5f * req.scale.z; 00340 00341 bbRBB.x = 0.5f * req.scale.x; 00342 bbRBB.y = 0.5f * req.scale.y; 00343 bbRBF.z = -0.5f * req.scale.z; 00344 00345 bbRTB.x = 0.5f * req.scale.x; 00346 bbRTB.y = 0.5f * req.scale.y; 00347 bbRTB.z = 0.5f * req.scale.z; 00348 00349 bbLBB.x = -0.5f * req.scale.x; 00350 bbLBB.y = 0.5f * req.scale.y; 00351 bbRBF.z = -0.5f * req.scale.z; 00352 00353 bbLTB.x = -0.5f * req.scale.x; 00354 bbLTB.y = 0.5f * req.scale.y; 00355 bbLTB.z = 0.5f * req.scale.z; 00356 00357 // Apply the rotation and translation stored in the Pose parameter 00358 tf::Quaternion q(req.pose.orientation.x, 00359 req.pose.orientation.y, 00360 req.pose.orientation.z, 00361 req.pose.orientation.w); 00362 btTransform trMat(q); 00363 for( int i = 0; i < (int)bbVertices.size(); i++ ) 00364 { 00365 btVector3 res = trMat * btVector3(bbVertices[i]->x, bbVertices[i]->y, bbVertices[i]->z); 00366 bbVertices[i]->x = res.x() + req.pose.position.x; 00367 bbVertices[i]->y = res.y() + req.pose.position.y; 00368 bbVertices[i]->z = res.z() + req.pose.position.z; 00369 } 00370 00371 // Estimate the rectangle 00372 point2_t p1, p2; 00373 if( !estimateRect(req.header.stamp, 00374 bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB, 00375 p1, p2) ) 00376 { 00377 return false; 00378 } 00379 00380 // Set response 00381 //-------------------------------------------------------------------------- 00382 res.p1[0] = p1[0]; 00383 res.p1[1] = p1[1]; 00384 res.p2[0] = p2[0]; 00385 res.p2[1] = p2[1]; 00386 00387 // Log request timestamp 00388 //-------------------------------------------------------------------------- 00389 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00390 00391 return true; 00392 } 00393 00394 00395 /****************************************************************************** 00396 * Rectangle estimation service. 00397 * 00398 * @param req Request of type EstimateRectAlt. 00399 * @param res Response of type EstimateRectAlt. 00400 */ 00401 bool estimateRectAlt_callback(srs_env_model_percp::EstimateRectAlt::Request &req, 00402 srs_env_model_percp::EstimateRectAlt::Response &res 00403 ) 00404 { 00405 // Vertices (corners) defining the bounding box. 00406 // Each is noted using this template: 00407 // bb{L=left,R=right}{B=bottom,T=top}{F=front,B=back} 00408 // (This notation provides more readable code, in comparison with 00409 // e.g. an array with 8 items.) 00410 Point3f bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB; 00411 00412 // Vector with pointers to the BB vertices (to be able to iterate over them) 00413 vector<Point3f *> bbVertices(8); 00414 bbVertices[0] = &bbLBF; 00415 bbVertices[1] = &bbRBF; 00416 bbVertices[2] = &bbRTF; 00417 bbVertices[3] = &bbLTF; 00418 bbVertices[4] = &bbLBB; 00419 bbVertices[5] = &bbRBB; 00420 bbVertices[6] = &bbRTB; 00421 bbVertices[7] = &bbLTB; 00422 00423 // Calculate coordinates of corners of the bounding box relative to its center 00424 bbLBF.x = -req.bounding_box_lwh.x; 00425 bbLBF.y = -req.bounding_box_lwh.y; 00426 bbLBF.z = 0.0f; 00427 00428 bbLTF.x = -req.bounding_box_lwh.x; 00429 bbLTF.y = -req.bounding_box_lwh.y; 00430 bbLTF.z = req.bounding_box_lwh.z; 00431 00432 bbRBF.x = req.bounding_box_lwh.x; 00433 bbRBF.y = -req.bounding_box_lwh.y; 00434 bbRBF.z = 0.0f; 00435 00436 bbRTF.x = req.bounding_box_lwh.x; 00437 bbRTF.y = -req.bounding_box_lwh.y; 00438 bbRTF.z = req.bounding_box_lwh.z; 00439 00440 bbRBB.x = req.bounding_box_lwh.x; 00441 bbRBB.y = req.bounding_box_lwh.y; 00442 bbRBF.z = 0.0f; 00443 00444 bbRTB.x = req.bounding_box_lwh.x; 00445 bbRTB.y = req.bounding_box_lwh.y; 00446 bbRTB.z = req.bounding_box_lwh.z; 00447 00448 bbLBB.x = -req.bounding_box_lwh.x; 00449 bbLBB.y = req.bounding_box_lwh.y; 00450 bbRBF.z = 0.0f; 00451 00452 bbLTB.x = -req.bounding_box_lwh.x; 00453 bbLTB.y = req.bounding_box_lwh.y; 00454 bbLTB.z = req.bounding_box_lwh.z; 00455 00456 // Apply the rotation and translation stored in the Pose parameter 00457 tf::Quaternion q(req.pose.orientation.x, 00458 req.pose.orientation.y, 00459 req.pose.orientation.z, 00460 req.pose.orientation.w); 00461 btTransform trMat(q); 00462 for( int i = 0; i < (int)bbVertices.size(); i++ ) 00463 { 00464 btVector3 res = trMat * btVector3(bbVertices[i]->x, bbVertices[i]->y, bbVertices[i]->z); 00465 bbVertices[i]->x = res.x() + req.pose.position.x; 00466 bbVertices[i]->y = res.y() + req.pose.position.y; 00467 bbVertices[i]->z = res.z() + req.pose.position.z; 00468 } 00469 00470 // Estimate the rectangle 00471 point2_t p1, p2; 00472 if( !estimateRect(req.header.stamp, 00473 bbLBF, bbRBF, bbRTF, bbLTF, bbLBB, bbRBB, bbRTB, bbLTB, 00474 p1, p2) ) 00475 { 00476 return false; 00477 } 00478 00479 // Set response 00480 //-------------------------------------------------------------------------- 00481 res.p1[0] = p1[0]; 00482 res.p1[1] = p1[1]; 00483 res.p2[0] = p2[0]; 00484 res.p2[1] = p2[1]; 00485 00486 // Log request timestamp 00487 //-------------------------------------------------------------------------- 00488 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00489 00490 return true; 00491 } 00492 00493 00494 /****************************************************************************** 00495 * Estimation service for 2D convex hull (in image plane coordinates) of a mesh 00496 * (in world coordinates). 00497 * 00498 * @param req Request of type Estimate2DHullMesh. 00499 * @param res Response of type Estimate2DHullMesh. 00500 */ 00501 bool estimate2DHullMesh_callback(srs_env_model_percp::Estimate2DHullMesh::Request &req, 00502 srs_env_model_percp::Estimate2DHullMesh::Response &res 00503 ) 00504 { 00505 vector<cv::Point3f> points; 00506 vector<cv::Point2i> convexHull; 00507 00508 if(req.mesh.type != req.mesh.MESH) { 00509 ROS_ERROR("The given shape is not a mesh."); 00510 return false; 00511 } 00512 else { 00513 for( int i = 0; i < (int)req.mesh.vertices.size(); i++ ) { 00514 points.push_back(cv::Point3f(req.mesh.vertices[i].x, 00515 req.mesh.vertices[i].y, 00516 req.mesh.vertices[i].z)); 00517 } 00518 } 00519 00520 // Calculate the convex hull 00521 estimate2DConvexHull(req.header.stamp, points, convexHull); 00522 00523 // Set response 00524 //-------------------------------------------------------------------------- 00525 res.convexHull.points.resize(convexHull.size()); 00526 for( int i = 0; i < (int)convexHull.size(); i++ ) { 00527 res.convexHull.points[i].x = convexHull[i].x; 00528 res.convexHull.points[i].y = convexHull[i].y; 00529 res.convexHull.points[i].z = 0; 00530 } 00531 00532 // Log request timestamp 00533 //-------------------------------------------------------------------------- 00534 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00535 00536 return true; 00537 } 00538 00539 00540 /****************************************************************************** 00541 * Estimation service for 2D convex hull (in image plane coordinates) of a point cloud 00542 * (in world coordinates). 00543 * 00544 * @param req Request of type Estimate2DHullPointCloud. 00545 * @param res Response of type Estimate2DHullPointCloud. 00546 */ 00547 bool estimate2DHullPointCloud_callback(srs_env_model_percp::Estimate2DHullPointCloud::Request &req, 00548 srs_env_model_percp::Estimate2DHullPointCloud::Response &res 00549 ) 00550 { 00551 vector<cv::Point3f> points; 00552 vector<cv::Point2i> convexHull; 00553 00554 // Convert to pcl/PointCloud 00555 pcl::PointCloud<pcl::PointXYZ> cloud; 00556 pcl::fromROSMsg(req.pointCloud, cloud); 00557 00558 for(int y = 0; y < (int)cloud.height; y++) { 00559 for(int x = 0; x < (int)cloud.width; x++) { 00560 pcl::PointXYZ p = cloud.points[y * cloud.width + x]; 00561 points.push_back(cv::Point3f(p.x, p.y, p.z)); 00562 } 00563 } 00564 00565 // Calculate the convex hull 00566 estimate2DConvexHull(req.header.stamp, points, convexHull); 00567 00568 // Set response 00569 //-------------------------------------------------------------------------- 00570 res.convexHull.points.resize(convexHull.size()); 00571 for( int i = 0; i < (int)convexHull.size(); i++ ) { 00572 res.convexHull.points[i].x = convexHull[i].x; 00573 res.convexHull.points[i].y = convexHull[i].y; 00574 res.convexHull.points[i].z = 0; 00575 } 00576 00577 // Log request timestamp 00578 //-------------------------------------------------------------------------- 00579 ROS_INFO("Request timestamp: %d.%d", req.header.stamp.sec, req.header.stamp.nsec); 00580 00581 return true; 00582 } 00583 00584 00585 /****************************************************************************** 00586 * Adds messages to cache (for synchronized subscription variant #1) and obtains 00587 * the corresponding transformation. 00588 * 00589 * @param depth Message with depth image. 00590 * @param camInfo Message with camera information. 00591 */ 00592 void sv1_callback(const sensor_msgs::ImageConstPtr &depth, 00593 const sensor_msgs::CameraInfoConstPtr &camInfo) 00594 { 00595 if(subVariant == SV_NONE) { 00596 subVariant = SV_1; 00597 00598 // Set size of cache 00599 depthCache.setCacheSize(CACHE_SIZE); 00600 camInfoCache.setCacheSize(CACHE_SIZE); 00601 } 00602 else if(subVariant != SV_1) { 00603 return; 00604 } 00605 00606 depthCache.add(depth); 00607 camInfoCache.add(camInfo); 00608 00609 if(DEBUG) { 00610 // Prints the timestamp of the received frame 00611 ROS_INFO("Received frame timestamp: %d.%d", 00612 depth->header.stamp.sec, depth->header.stamp.nsec); 00613 } 00614 } 00615 00616 00617 /****************************************************************************** 00618 * Adds messages to cache (for synchronized subscription variant #2) and obtains 00619 * the corresponding transformation. 00620 * 00621 * @param pointCloud Message with point cloud. 00622 * @param camInfo Message with camera information. 00623 */ 00624 void sv2_callback(const sensor_msgs::PointCloud2ConstPtr &pointCloud, 00625 const sensor_msgs::CameraInfoConstPtr &camInfo) 00626 { 00627 if(subVariant == SV_NONE) { 00628 subVariant = SV_2; 00629 00630 // Set size of cache 00631 pointCloudCache.setCacheSize(CACHE_SIZE); 00632 camInfoCache.setCacheSize(CACHE_SIZE); 00633 } 00634 else if(subVariant != SV_2) { 00635 return; 00636 } 00637 00638 pointCloudCache.add(pointCloud); 00639 camInfoCache.add(camInfo); 00640 00641 if(DEBUG) { 00642 // Prints the timestamp of the received frame 00643 ROS_INFO("Received frame timestamp: %d.%d", 00644 pointCloud->header.stamp.sec, pointCloud->header.stamp.nsec); 00645 } 00646 } 00647 00648 } 00649 00650 00651 /*============================================================================== 00652 * Main function. 00653 */ 00654 int main(int argc, char **argv) 00655 { 00656 using namespace srs_env_model_percp; 00657 00658 // ROS initialization (the last argument is the name of the node) 00659 ros::init(argc, argv, "bb_estimator_server"); 00660 00661 // NodeHandle is the main access point to communications with the ROS system 00662 ros::NodeHandle n; 00663 00664 // Create a TF listener 00665 tfListener = new tf::TransformListener(); 00666 00667 // Get private parameters from the parameter server 00668 // (the third parameter of function param is the default value) 00669 //-------------------------------------------------------------------------- 00670 ros::NodeHandle private_nh("~"); 00671 private_nh.param(OUTLIERS_PERCENT_PARAM, outliersPercent, OUTLIERS_PERCENT_DEFAULT); 00672 private_nh.param(SAMPLING_PERCENT_PARAM, samplingPercent, SAMPLING_PERCENT_DEFAULT); 00673 private_nh.param(GLOBAL_FRAME_PARAM, sceneFrameId, GLOBAL_FRAME_DEFAULT); 00674 private_nh.param(SIDES_RATIO_PARAM, sidesRatio, SIDES_RATIO_DEFAULT); 00675 00676 ROS_INFO_STREAM(OUTLIERS_PERCENT_PARAM << " = " << outliersPercent); 00677 ROS_INFO_STREAM(SAMPLING_PERCENT_PARAM << " = " << samplingPercent); 00678 ROS_INFO_STREAM(GLOBAL_FRAME_PARAM << " = " << sceneFrameId); 00679 ROS_INFO_STREAM(SIDES_RATIO_PARAM << " = " << sidesRatio); 00680 00681 // Subscription and synchronization of messages 00682 // TODO: Create the subscribers dynamically and unsubscribe the unused 00683 // subscription variant. 00684 //-------------------------------------------------------------------------- 00685 // Subscription variant #1 00686 message_filters::Subscriber<Image> sv1_depth_sub(n, DEPTH_IMAGE_TOPIC_IN, 1); 00687 message_filters::Subscriber<CameraInfo> sv1_camInfo_sub(n, CAMERA_INFO_TOPIC_IN, 1); 00688 00689 typedef sync_policies::ApproximateTime<Image, CameraInfo> sv1_MySyncPolicy; 00690 Synchronizer<sv1_MySyncPolicy> sv1_sync(sv1_MySyncPolicy(QUEUE_SIZE), sv1_depth_sub, sv1_camInfo_sub); 00691 sv1_sync.registerCallback(boost::bind(&sv1_callback, _1, _2)); 00692 00693 // Subscription variant #2 00694 message_filters::Subscriber<PointCloud2> sv2_pointCloud_sub(n, POINT_CLOUD_TOPIC_IN, 1); 00695 message_filters::Subscriber<CameraInfo> sv2_camInfo_sub(n, CAMERA_INFO_TOPIC_IN, 1); 00696 00697 typedef sync_policies::ApproximateTime<PointCloud2, CameraInfo> sv2_MySyncPolicy; 00698 Synchronizer<sv2_MySyncPolicy> sv2_sync(sv2_MySyncPolicy(QUEUE_SIZE), sv2_pointCloud_sub, sv2_camInfo_sub); 00699 sv2_sync.registerCallback(boost::bind(&sv2_callback, _1, _2)); 00700 00701 // Create and advertise this service over ROS 00702 //-------------------------------------------------------------------------- 00703 ros::ServiceServer service = n.advertiseService(EstimateBB_SRV, estimateBB_callback); 00704 ros::ServiceServer serviceAlt = n.advertiseService(EstimateBBAlt_SRV, estimateBBAlt_callback); 00705 ros::ServiceServer serviceRect = n.advertiseService(EstimateRect_SRV, estimateRect_callback); 00706 ros::ServiceServer serviceRectAlt = n.advertiseService(EstimateRectAlt_SRV, estimateRectAlt_callback); 00707 ros::ServiceServer service2DHullMesh = n.advertiseService(Estimate2DHullMesh_SRV, estimate2DHullMesh_callback); 00708 ros::ServiceServer service2DHullPointCloud = n.advertiseService(Estimate2DHullPointCloud_SRV, estimate2DHullPointCloud_callback); 00709 ROS_INFO("Ready."); 00710 00711 // Enters a loop, calling message callbacks 00712 ros::spin(); 00713 00714 return 0; 00715 } 00716