service_server.cpp
Go to the documentation of this file.
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 


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:07:23