CommonDataSubscriberDepth.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <rtabmap_ros/CommonDataSubscriber.h>
00029 
00030 namespace rtabmap_ros {
00031 
00032 // RGB + Depth
00033 void CommonDataSubscriber::depthCallback(
00034                 const sensor_msgs::ImageConstPtr& imageMsg,
00035                 const sensor_msgs::ImageConstPtr& depthMsg,
00036                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00037 {
00038         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00039         nav_msgs::OdometryConstPtr odomMsg; // Null
00040         sensor_msgs::LaserScanConstPtr scanMsg; // Null
00041         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00042         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00043         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00044 }
00045 void CommonDataSubscriber::depthScan2dCallback(
00046                 const sensor_msgs::ImageConstPtr& imageMsg,
00047                 const sensor_msgs::ImageConstPtr& depthMsg,
00048                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00049                 const sensor_msgs::LaserScanConstPtr& scanMsg)
00050 {
00051         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00052         nav_msgs::OdometryConstPtr odomMsg; // Null
00053         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00054         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00055         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00056 }
00057 void CommonDataSubscriber::depthScan3dCallback(
00058                 const sensor_msgs::ImageConstPtr& imageMsg,
00059                 const sensor_msgs::ImageConstPtr& depthMsg,
00060                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00061                 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
00062 {
00063         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00064         nav_msgs::OdometryConstPtr odomMsg; // Null
00065         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00066         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00067         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00068 }
00069 void CommonDataSubscriber::depthInfoCallback(
00070                 const sensor_msgs::ImageConstPtr& imageMsg,
00071                 const sensor_msgs::ImageConstPtr& depthMsg,
00072                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00073                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00074 {
00075         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00076         nav_msgs::OdometryConstPtr odomMsg; // Null
00077         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00078         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00079         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
00080 }
00081 void CommonDataSubscriber::depthScan2dInfoCallback(
00082                 const sensor_msgs::ImageConstPtr& imageMsg,
00083                 const sensor_msgs::ImageConstPtr& depthMsg,
00084                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00085                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00086                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00087 {
00088         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00089         nav_msgs::OdometryConstPtr odomMsg; // Null
00090         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00091         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00092 }
00093 void CommonDataSubscriber::depthScan3dInfoCallback(
00094                 const sensor_msgs::ImageConstPtr& imageMsg,
00095                 const sensor_msgs::ImageConstPtr& depthMsg,
00096                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00097                 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
00098                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00099 {
00100         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00101         nav_msgs::OdometryConstPtr odomMsg; // Null
00102         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00103         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00104 }
00105 
00106 // RGB + Depth + Odom
00107 void CommonDataSubscriber::depthOdomCallback(
00108                 const nav_msgs::OdometryConstPtr & odomMsg,
00109                 const sensor_msgs::ImageConstPtr& imageMsg,
00110                 const sensor_msgs::ImageConstPtr& depthMsg,
00111                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00112 {
00113         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00114         sensor_msgs::LaserScanConstPtr scanMsg; // Null
00115         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
00116         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00117         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00118 }
00119 void CommonDataSubscriber::depthOdomScan2dCallback(
00120                 const nav_msgs::OdometryConstPtr & odomMsg,
00121                 const sensor_msgs::ImageConstPtr& imageMsg,
00122                 const sensor_msgs::ImageConstPtr& depthMsg,
00123                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00124                 const sensor_msgs::LaserScanConstPtr& scanMsg)
00125 {
00126         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00127         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00128         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00129         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00130 }
00131 void CommonDataSubscriber::depthOdomScan3dCallback(
00132                 const nav_msgs::OdometryConstPtr & odomMsg,
00133                 const sensor_msgs::ImageConstPtr& imageMsg,
00134                 const sensor_msgs::ImageConstPtr& depthMsg,
00135                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00136                 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
00137 {
00138         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00139         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00140         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00141         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00142 }
00143 void CommonDataSubscriber::depthOdomInfoCallback(
00144                 const nav_msgs::OdometryConstPtr & odomMsg,
00145                 const sensor_msgs::ImageConstPtr& imageMsg,
00146                 const sensor_msgs::ImageConstPtr& depthMsg,
00147                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00148                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00149 {
00150         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00151         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00152         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00153         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
00154 }
00155 void CommonDataSubscriber::depthOdomScan2dInfoCallback(
00156                 const nav_msgs::OdometryConstPtr & odomMsg,
00157                 const sensor_msgs::ImageConstPtr& imageMsg,
00158                 const sensor_msgs::ImageConstPtr& depthMsg,
00159                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00160                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00161                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00162 {
00163         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00164         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00165         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00166 }
00167 void CommonDataSubscriber::depthOdomScan3dInfoCallback(
00168                 const nav_msgs::OdometryConstPtr & odomMsg,
00169                 const sensor_msgs::ImageConstPtr& imageMsg,
00170                 const sensor_msgs::ImageConstPtr& depthMsg,
00171                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00172                 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
00173                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00174 {
00175         rtabmap_ros::UserDataConstPtr userDataMsg; // Null
00176         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00177         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00178 }
00179 
00180 // RGB + Depth + User Data
00181 void CommonDataSubscriber::depthDataCallback(
00182                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00183                 const sensor_msgs::ImageConstPtr& imageMsg,
00184                 const sensor_msgs::ImageConstPtr& depthMsg,
00185                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00186 {
00187         nav_msgs::OdometryConstPtr odomMsg; // Null
00188         sensor_msgs::LaserScanConstPtr scanMsg; // Null
00189         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
00190         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00191         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00192 }
00193 void CommonDataSubscriber::depthDataScan2dCallback(
00194                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00195                 const sensor_msgs::ImageConstPtr& imageMsg,
00196                 const sensor_msgs::ImageConstPtr& depthMsg,
00197                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00198                 const sensor_msgs::LaserScanConstPtr& scanMsg)
00199 {
00200         nav_msgs::OdometryConstPtr odomMsg; // null
00201         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00202         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00203         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00204 }
00205 void CommonDataSubscriber::depthDataScan3dCallback(
00206                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00207                 const sensor_msgs::ImageConstPtr& imageMsg,
00208                 const sensor_msgs::ImageConstPtr& depthMsg,
00209                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00210                 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
00211 {
00212         nav_msgs::OdometryConstPtr odomMsg; // null
00213         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00214         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00215         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00216 }
00217 void CommonDataSubscriber::depthDataInfoCallback(
00218                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00219                 const sensor_msgs::ImageConstPtr& imageMsg,
00220                 const sensor_msgs::ImageConstPtr& depthMsg,
00221                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00222                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00223 {
00224         nav_msgs::OdometryConstPtr odomMsg; // null
00225         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00226         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00227         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
00228 }
00229 void CommonDataSubscriber::depthDataScan2dInfoCallback(
00230                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00231                 const sensor_msgs::ImageConstPtr& imageMsg,
00232                 const sensor_msgs::ImageConstPtr& depthMsg,
00233                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00234                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00235                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00236 {
00237         nav_msgs::OdometryConstPtr odomMsg; // null
00238         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00239         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00240 }
00241 void CommonDataSubscriber::depthDataScan3dInfoCallback(
00242                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00243                 const sensor_msgs::ImageConstPtr& imageMsg,
00244                 const sensor_msgs::ImageConstPtr& depthMsg,
00245                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00246                 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
00247                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00248 {
00249         nav_msgs::OdometryConstPtr odomMsg; // null
00250         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00251         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00252 }
00253 
00254 // RGB + Depth + Odom + User Data
00255 void CommonDataSubscriber::depthOdomDataCallback(
00256                 const nav_msgs::OdometryConstPtr & odomMsg,
00257                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00258                 const sensor_msgs::ImageConstPtr& imageMsg,
00259                 const sensor_msgs::ImageConstPtr& depthMsg,
00260                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
00261 {
00262         sensor_msgs::LaserScanConstPtr scanMsg; // Null
00263         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
00264         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00265         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00266 }
00267 void CommonDataSubscriber::depthOdomDataScan2dCallback(
00268                 const nav_msgs::OdometryConstPtr & odomMsg,
00269                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00270                 const sensor_msgs::ImageConstPtr& imageMsg,
00271                 const sensor_msgs::ImageConstPtr& depthMsg,
00272                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00273                 const sensor_msgs::LaserScanConstPtr& scanMsg)
00274 {
00275         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00276         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00277         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00278 }
00279 void CommonDataSubscriber::depthOdomDataScan3dCallback(
00280                 const nav_msgs::OdometryConstPtr & odomMsg,
00281                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00282                 const sensor_msgs::ImageConstPtr& imageMsg,
00283                 const sensor_msgs::ImageConstPtr& depthMsg,
00284                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00285                 const sensor_msgs::PointCloud2ConstPtr& scanMsg)
00286 {
00287         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00288         rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
00289         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00290 }
00291 void CommonDataSubscriber::depthOdomDataInfoCallback(
00292                 const nav_msgs::OdometryConstPtr & odomMsg,
00293                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00294                 const sensor_msgs::ImageConstPtr& imageMsg,
00295                 const sensor_msgs::ImageConstPtr& depthMsg,
00296                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00297                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00298 {
00299         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00300         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00301         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
00302 }
00303 void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
00304                 const nav_msgs::OdometryConstPtr & odomMsg,
00305                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00306                 const sensor_msgs::ImageConstPtr& imageMsg,
00307                 const sensor_msgs::ImageConstPtr& depthMsg,
00308                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00309                 const sensor_msgs::LaserScanConstPtr& scanMsg,
00310                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00311 {
00312         sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
00313         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00314 }
00315 void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
00316                 const nav_msgs::OdometryConstPtr & odomMsg,
00317                 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00318                 const sensor_msgs::ImageConstPtr& imageMsg,
00319                 const sensor_msgs::ImageConstPtr& depthMsg,
00320                 const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
00321                 const sensor_msgs::PointCloud2ConstPtr& scanMsg,
00322                 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00323 {
00324         sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
00325         commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
00326 }
00327 
00328 void CommonDataSubscriber::setupDepthCallbacks(
00329                 ros::NodeHandle & nh,
00330                 ros::NodeHandle & pnh,
00331                 bool subscribeOdom,
00332                 bool subscribeUserData,
00333                 bool subscribeScan2d,
00334                 bool subscribeScan3d,
00335                 bool subscribeOdomInfo,
00336                 int queueSize,
00337                 bool approxSync)
00338 {
00339         ROS_INFO("Setup depth callback");
00340 
00341         std::string rgbPrefix = "rgb";
00342         std::string depthPrefix = "depth";
00343         ros::NodeHandle rgb_nh(nh, rgbPrefix);
00344         ros::NodeHandle depth_nh(nh, depthPrefix);
00345         ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
00346         ros::NodeHandle depth_pnh(pnh, depthPrefix);
00347         image_transport::ImageTransport rgb_it(rgb_nh);
00348         image_transport::ImageTransport depth_it(depth_nh);
00349         image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00350         image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00351 
00352         imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00353         imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00354         cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
00355 
00356         if(subscribeOdom && subscribeUserData)
00357         {
00358                 odomSub_.subscribe(nh, "odom", 1);
00359                 userDataSub_.subscribe(nh, "user_data", 1);
00360 
00361                 if(subscribeScan2d)
00362                 {
00363                         subscribedToScan2d_ = true;
00364                         scanSub_.subscribe(nh, "scan", 1);
00365                         if(subscribeOdomInfo)
00366                         {
00367                                 subscribedToOdomInfo_ = true;
00368                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00369                                 SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
00370                         }
00371                         else
00372                         {
00373                                 SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
00374                         }
00375                 }
00376                 else if(subscribeScan3d)
00377                 {
00378                         subscribedToScan3d_ = true;
00379                         scan3dSub_.subscribe(nh, "scan_cloud", 1);
00380                         if(subscribeOdomInfo)
00381                         {
00382                                 subscribedToOdomInfo_ = true;
00383                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00384                                 SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
00385                         }
00386                         else
00387                         {
00388                                 SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
00389                         }
00390                 }
00391                 else if(subscribeOdomInfo)
00392                 {
00393                         subscribedToOdomInfo_ = true;
00394                         odomInfoSub_.subscribe(nh, "odom_info", 1);
00395                         SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
00396                 }
00397                 else
00398                 {
00399                         SYNC_DECL5(depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
00400                 }
00401         }
00402         else if(subscribeOdom)
00403         {
00404                 odomSub_.subscribe(nh, "odom", 1);
00405 
00406                 if(subscribeScan2d)
00407                 {
00408                         subscribedToScan2d_ = true;
00409                         scanSub_.subscribe(nh, "scan", 1);
00410                         if(subscribeOdomInfo)
00411                         {
00412                                 subscribedToOdomInfo_ = true;
00413                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00414                                 SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
00415                         }
00416                         else
00417                         {
00418                                 SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
00419                         }
00420                 }
00421                 else if(subscribeScan3d)
00422                 {
00423                         subscribedToScan3d_ = true;
00424                         scan3dSub_.subscribe(nh, "scan_cloud", 1);
00425                         if(subscribeOdomInfo)
00426                         {
00427                                 subscribedToOdomInfo_ = true;
00428                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00429                                 SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
00430                         }
00431                         else
00432                         {
00433                                 SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
00434                         }
00435                 }
00436                 else if(subscribeOdomInfo)
00437                 {
00438                         subscribedToOdomInfo_ = true;
00439                         odomInfoSub_.subscribe(nh, "odom_info", 1);
00440                         SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
00441                 }
00442                 else
00443                 {
00444                         SYNC_DECL4(depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
00445                 }
00446         }
00447         else if(subscribeUserData)
00448         {
00449                 userDataSub_.subscribe(nh, "user_data", 1);
00450 
00451                 if(subscribeScan2d)
00452                 {
00453                         subscribedToScan2d_ = true;
00454                         scanSub_.subscribe(nh, "scan", 1);
00455 
00456                         if(subscribeOdomInfo)
00457                         {
00458                                 subscribedToOdomInfo_ = true;
00459                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00460                                 SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
00461                         }
00462                         else
00463                         {
00464                                 SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
00465                         }
00466                 }
00467                 else if(subscribeScan3d)
00468                 {
00469                         subscribedToScan3d_ = true;
00470                         scan3dSub_.subscribe(nh, "scan_cloud", 1);
00471                         if(subscribeOdomInfo)
00472                         {
00473                                 subscribedToOdomInfo_ = true;
00474                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00475                                 SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
00476                         }
00477                         else
00478                         {
00479                                 SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
00480                         }
00481                 }
00482                 else if(subscribeOdomInfo)
00483                 {
00484                         subscribedToOdomInfo_ = true;
00485                         odomInfoSub_.subscribe(nh, "odom_info", 1);
00486                         SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
00487                 }
00488                 else
00489                 {
00490                         SYNC_DECL4(depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
00491                 }
00492         }
00493         else
00494         {
00495                 if(subscribeScan2d)
00496                 {
00497                         subscribedToScan2d_ = true;
00498                         scanSub_.subscribe(nh, "scan", 1);
00499                         if(subscribeOdomInfo)
00500                         {
00501                                 subscribedToOdomInfo_ = true;
00502                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00503                                 SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
00504                         }
00505                         else
00506                         {
00507                                 SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
00508                         }
00509                 }
00510                 else if(subscribeScan3d)
00511                 {
00512                         subscribedToScan3d_ = true;
00513                         scan3dSub_.subscribe(nh, "scan_cloud", 1);
00514                         if(subscribeOdomInfo)
00515                         {
00516                                 subscribedToOdomInfo_ = true;
00517                                 odomInfoSub_.subscribe(nh, "odom_info", 1);
00518                                 SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
00519                         }
00520                         else
00521                         {
00522                                 SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
00523                         }
00524                 }
00525                 else if(subscribeOdomInfo)
00526                 {
00527                         subscribedToOdomInfo_ = true;
00528                         odomInfoSub_.subscribe(nh, "odom_info", 1);
00529                         SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
00530                 }
00531                 else
00532                 {
00533                         SYNC_DECL3(depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
00534                 }
00535         }
00536 }
00537 
00538 } /* namespace rtabmap_ros */

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:30:49