00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap_ros/CommonDataSubscriber.h>
00029
00030 namespace rtabmap_ros {
00031
00032 CommonDataSubscriber::CommonDataSubscriber(bool gui) :
00033 queueSize_(10),
00034 approxSync_(true),
00035 warningThread_(0),
00036 callbackCalled_(false),
00037 subscribedToDepth_(!gui),
00038 subscribedToStereo_(false),
00039 subscribedToRGBD_(false),
00040 subscribedToScan2d_(false),
00041 subscribedToScan3d_(false),
00042 subscribedToOdomInfo_(false),
00043
00044
00045 SYNC_INIT(depth),
00046 SYNC_INIT(depthScan2d),
00047 SYNC_INIT(depthScan3d),
00048 SYNC_INIT(depthInfo),
00049 SYNC_INIT(depthScan2dInfo),
00050 SYNC_INIT(depthScan3dInfo),
00051
00052
00053 SYNC_INIT(depthOdom),
00054 SYNC_INIT(depthOdomScan2d),
00055 SYNC_INIT(depthOdomScan3d),
00056 SYNC_INIT(depthOdomInfo),
00057 SYNC_INIT(depthOdomScan2dInfo),
00058 SYNC_INIT(depthOdomScan3dInfo),
00059
00060
00061 SYNC_INIT(depthData),
00062 SYNC_INIT(depthDataScan2d),
00063 SYNC_INIT(depthDataScan3d),
00064 SYNC_INIT(depthDataInfo),
00065 SYNC_INIT(depthDataScan2dInfo),
00066 SYNC_INIT(depthDataScan3dInfo),
00067
00068
00069 SYNC_INIT(depthOdomData),
00070 SYNC_INIT(depthOdomDataScan2d),
00071 SYNC_INIT(depthOdomDataScan3d),
00072 SYNC_INIT(depthOdomDataInfo),
00073 SYNC_INIT(depthOdomDataScan2dInfo),
00074 SYNC_INIT(depthOdomDataScan3dInfo),
00075
00076
00077 SYNC_INIT(stereo),
00078 SYNC_INIT(stereoInfo),
00079
00080
00081 SYNC_INIT(stereoOdom),
00082 SYNC_INIT(stereoOdomInfo),
00083
00084
00085 SYNC_INIT(rgbdScan2d),
00086 SYNC_INIT(rgbdScan3d),
00087 SYNC_INIT(rgbdInfo),
00088 SYNC_INIT(rgbdScan2dInfo),
00089 SYNC_INIT(rgbdScan3dInfo),
00090
00091
00092 SYNC_INIT(rgbdOdom),
00093 SYNC_INIT(rgbdOdomScan2d),
00094 SYNC_INIT(rgbdOdomScan3d),
00095 SYNC_INIT(rgbdOdomInfo),
00096 SYNC_INIT(rgbdOdomScan2dInfo),
00097 SYNC_INIT(rgbdOdomScan3dInfo),
00098
00099
00100 SYNC_INIT(rgbdData),
00101 SYNC_INIT(rgbdDataScan2d),
00102 SYNC_INIT(rgbdDataScan3d),
00103 SYNC_INIT(rgbdDataInfo),
00104 SYNC_INIT(rgbdDataScan2dInfo),
00105 SYNC_INIT(rgbdDataScan3dInfo),
00106
00107
00108 SYNC_INIT(rgbdOdomData),
00109 SYNC_INIT(rgbdOdomDataScan2d),
00110 SYNC_INIT(rgbdOdomDataScan3d),
00111 SYNC_INIT(rgbdOdomDataInfo),
00112 SYNC_INIT(rgbdOdomDataScan2dInfo),
00113 SYNC_INIT(rgbdOdomDataScan3dInfo),
00114
00115
00116 SYNC_INIT(rgbd2),
00117 SYNC_INIT(rgbd2Scan2d),
00118 SYNC_INIT(rgbd2Scan3d),
00119 SYNC_INIT(rgbd2Info),
00120 SYNC_INIT(rgbd2Scan2dInfo),
00121 SYNC_INIT(rgbd2Scan3dInfo),
00122
00123
00124 SYNC_INIT(rgbd2Odom),
00125 SYNC_INIT(rgbd2OdomScan2d),
00126 SYNC_INIT(rgbd2OdomScan3d),
00127 SYNC_INIT(rgbd2OdomInfo),
00128 SYNC_INIT(rgbd2OdomScan2dInfo),
00129 SYNC_INIT(rgbd2OdomScan3dInfo),
00130
00131
00132 SYNC_INIT(rgbd2Data),
00133 SYNC_INIT(rgbd2DataScan2d),
00134 SYNC_INIT(rgbd2DataScan3d),
00135 SYNC_INIT(rgbd2DataInfo),
00136 SYNC_INIT(rgbd2DataScan2dInfo),
00137 SYNC_INIT(rgbd2DataScan3dInfo),
00138
00139
00140 SYNC_INIT(rgbd2OdomData),
00141 SYNC_INIT(rgbd2OdomDataScan2d),
00142 SYNC_INIT(rgbd2OdomDataScan3d),
00143 SYNC_INIT(rgbd2OdomDataInfo),
00144 SYNC_INIT(rgbd2OdomDataScan2dInfo),
00145 SYNC_INIT(rgbd2OdomDataScan3dInfo),
00146
00147
00148 SYNC_INIT(rgbd3),
00149 SYNC_INIT(rgbd3Scan2d),
00150 SYNC_INIT(rgbd3Scan3d),
00151 SYNC_INIT(rgbd3Info),
00152 SYNC_INIT(rgbd3Scan2dInfo),
00153 SYNC_INIT(rgbd3Scan3dInfo),
00154
00155
00156 SYNC_INIT(rgbd3Odom),
00157 SYNC_INIT(rgbd3OdomScan2d),
00158 SYNC_INIT(rgbd3OdomScan3d),
00159 SYNC_INIT(rgbd3OdomInfo),
00160 SYNC_INIT(rgbd3OdomScan2dInfo),
00161 SYNC_INIT(rgbd3OdomScan3dInfo),
00162
00163
00164 SYNC_INIT(rgbd3Data),
00165 SYNC_INIT(rgbd3DataScan2d),
00166 SYNC_INIT(rgbd3DataScan3d),
00167 SYNC_INIT(rgbd3DataInfo),
00168 SYNC_INIT(rgbd3DataScan2dInfo),
00169 SYNC_INIT(rgbd3DataScan3dInfo),
00170
00171
00172 SYNC_INIT(rgbd3OdomData),
00173 SYNC_INIT(rgbd3OdomDataScan2d),
00174 SYNC_INIT(rgbd3OdomDataScan3d),
00175 SYNC_INIT(rgbd3OdomDataInfo),
00176 SYNC_INIT(rgbd3OdomDataScan2dInfo),
00177 SYNC_INIT(rgbd3OdomDataScan3dInfo),
00178
00179
00180 SYNC_INIT(rgbd4),
00181 SYNC_INIT(rgbd4Scan2d),
00182 SYNC_INIT(rgbd4Scan3d),
00183 SYNC_INIT(rgbd4Info),
00184 SYNC_INIT(rgbd4Scan2dInfo),
00185 SYNC_INIT(rgbd4Scan3dInfo),
00186
00187
00188 SYNC_INIT(rgbd4Odom),
00189 SYNC_INIT(rgbd4OdomScan2d),
00190 SYNC_INIT(rgbd4OdomScan3d),
00191 SYNC_INIT(rgbd4OdomInfo),
00192 SYNC_INIT(rgbd4OdomScan2dInfo),
00193 SYNC_INIT(rgbd4OdomScan3dInfo),
00194
00195
00196 SYNC_INIT(rgbd4Data),
00197 SYNC_INIT(rgbd4DataScan2d),
00198 SYNC_INIT(rgbd4DataScan3d),
00199 SYNC_INIT(rgbd4DataInfo),
00200 SYNC_INIT(rgbd4DataScan2dInfo),
00201 SYNC_INIT(rgbd4DataScan3dInfo),
00202
00203
00204 SYNC_INIT(rgbd4OdomData),
00205 SYNC_INIT(rgbd4OdomDataScan2d),
00206 SYNC_INIT(rgbd4OdomDataScan3d),
00207 SYNC_INIT(rgbd4OdomDataInfo),
00208 SYNC_INIT(rgbd4OdomDataScan2dInfo),
00209 SYNC_INIT(rgbd4OdomDataScan3dInfo)
00210
00211 {
00212 }
00213
00214 void CommonDataSubscriber::setupCallbacks(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name)
00215 {
00216 bool subscribeScan2d = false;
00217 bool subscribeScan3d = false;
00218 bool subscribeOdomInfo = false;
00219 bool subscribeUserData = false;
00220 int rgbdCameras = 1;
00221 name_ = name;
00222
00223
00224 pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_);
00225 if(pnh.getParam("subscribe_laserScan", subscribeScan2d) && subscribeScan2d)
00226 {
00227 ROS_WARN("rtabmap: \"subscribe_laserScan\" parameter is deprecated, use \"subscribe_scan\" instead. The scan topic is still subscribed.");
00228 }
00229 pnh.param("subscribe_scan", subscribeScan2d, subscribeScan2d);
00230 pnh.param("subscribe_scan_cloud", subscribeScan3d, subscribeScan3d);
00231 pnh.param("subscribe_stereo", subscribedToStereo_, subscribedToStereo_);
00232 pnh.param("subscribe_rgbd", subscribedToRGBD_, subscribedToRGBD_);
00233 pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
00234 pnh.param("subscribe_user_data", subscribeUserData, subscribeUserData);
00235 if(subscribedToDepth_ && subscribedToStereo_)
00236 {
00237 ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
00238 subscribedToDepth_ = false;
00239 }
00240 if(subscribedToDepth_ && subscribedToRGBD_)
00241 {
00242 ROS_WARN("rtabmap: Parameters subscribe_depth and subscribe_rgbd cannot be true at the same time. Parameter subscribe_depth is set to false.");
00243 subscribedToDepth_ = false;
00244 }
00245 if(subscribedToStereo_ && subscribedToRGBD_)
00246 {
00247 ROS_WARN("rtabmap: Parameters subscribe_stereo and subscribe_rgbd cannot be true at the same time. Parameter subscribe_stereo is set to false.");
00248 subscribedToStereo_ = false;
00249 }
00250 if(subscribeScan2d && subscribeScan3d)
00251 {
00252 ROS_WARN("rtabmap: Parameters subscribe_scan and subscribe_scan_cloud cannot be true at the same time. Parameter subscribe_scan_cloud is set to false.");
00253 subscribeScan3d = false;
00254 }
00255 if(subscribeScan2d || subscribeScan3d)
00256 {
00257 if(!subscribedToDepth_ && !subscribedToStereo_ && !subscribedToRGBD_)
00258 {
00259 ROS_WARN("When subscribing to laser scan, you should subscribe to depth, stereo or rgbd too. Subscribing to depth by default...");
00260 subscribedToDepth_ = true;
00261 }
00262 }
00263 if(subscribedToStereo_)
00264 {
00265 approxSync_ = false;
00266 }
00267
00268 std::string odomFrameId;
00269 pnh.getParam("odom_frame_id", odomFrameId);
00270 pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
00271 if(pnh.hasParam("depth_cameras"))
00272 {
00273 ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true.");
00274 }
00275 pnh.param("queue_size", queueSize_, queueSize_);
00276 if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
00277 {
00278 ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
00279 "to \"approx_sync\"! Your value is still copied to "
00280 "corresponding parameter.");
00281 pnh.param("stereo_approx_sync", approxSync_, approxSync_);
00282 }
00283 else
00284 {
00285 pnh.param("approx_sync", approxSync_, approxSync_);
00286 }
00287
00288 if(rgbdCameras <= 0 && subscribedToRGBD_)
00289 {
00290 rgbdCameras = 1;
00291 }
00292
00293 ROS_INFO("%s: queue_size = %d", name.c_str(), queueSize_);
00294 ROS_INFO("%s: rgbd_cameras = %d", name.c_str(), rgbdCameras);
00295 ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false");
00296
00297 bool subscribeOdom = odomFrameId.empty();
00298 if(subscribedToDepth_)
00299 {
00300 setupDepthCallbacks(
00301 nh,
00302 pnh,
00303 subscribeOdom,
00304 subscribeUserData,
00305 subscribeScan2d,
00306 subscribeScan3d,
00307 subscribeOdomInfo,
00308 queueSize_,
00309 approxSync_);
00310 }
00311 else if(subscribedToStereo_)
00312 {
00313 setupStereoCallbacks(
00314 nh,
00315 pnh,
00316 subscribeOdom,
00317 subscribeOdomInfo,
00318 queueSize_,
00319 approxSync_);
00320 }
00321 else if(subscribedToRGBD_)
00322 {
00323 if(rgbdCameras == 4)
00324 {
00325 setupRGBD4Callbacks(
00326 nh,
00327 pnh,
00328 subscribeOdom,
00329 subscribeUserData,
00330 subscribeScan2d,
00331 subscribeScan3d,
00332 subscribeOdomInfo,
00333 queueSize_,
00334 approxSync_);
00335 }
00336 else if(rgbdCameras == 3)
00337 {
00338 setupRGBD3Callbacks(
00339 nh,
00340 pnh,
00341 subscribeOdom,
00342 subscribeUserData,
00343 subscribeScan2d,
00344 subscribeScan3d,
00345 subscribeOdomInfo,
00346 queueSize_,
00347 approxSync_);
00348 }
00349 else if(rgbdCameras == 2)
00350 {
00351 setupRGBD2Callbacks(
00352 nh,
00353 pnh,
00354 subscribeOdom,
00355 subscribeUserData,
00356 subscribeScan2d,
00357 subscribeScan3d,
00358 subscribeOdomInfo,
00359 queueSize_,
00360 approxSync_);
00361 }
00362 else
00363 {
00364 setupRGBDCallbacks(
00365 nh,
00366 pnh,
00367 subscribeOdom,
00368 subscribeUserData,
00369 subscribeScan2d,
00370 subscribeScan3d,
00371 subscribeOdomInfo,
00372 queueSize_,
00373 approxSync_);
00374 }
00375 }
00376 if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_)
00377 {
00378 warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
00379 ROS_INFO("%s", subscribedTopicsMsg_.c_str());
00380 }
00381 }
00382
00383 CommonDataSubscriber::~CommonDataSubscriber()
00384 {
00385 if(warningThread_)
00386 {
00387 callbackCalled();
00388 warningThread_->join();
00389 delete warningThread_;
00390 }
00391
00392
00393 SYNC_DEL(depth);
00394 SYNC_DEL(depthScan2d);
00395 SYNC_DEL(depthScan3d);
00396 SYNC_DEL(depthInfo);
00397 SYNC_DEL(depthScan2dInfo);
00398 SYNC_DEL(depthScan3dInfo);
00399
00400
00401 SYNC_DEL(depthOdom);
00402 SYNC_DEL(depthOdomScan2d);
00403 SYNC_DEL(depthOdomScan3d);
00404 SYNC_DEL(depthOdomInfo);
00405 SYNC_DEL(depthOdomScan2dInfo);
00406 SYNC_DEL(depthOdomScan3dInfo);
00407
00408
00409 SYNC_DEL(depthData);
00410 SYNC_DEL(depthDataScan2d);
00411 SYNC_DEL(depthDataScan3d);
00412 SYNC_DEL(depthDataInfo);
00413 SYNC_DEL(depthDataScan2dInfo);
00414 SYNC_DEL(depthDataScan3dInfo);
00415
00416
00417 SYNC_DEL(depthOdomData);
00418 SYNC_DEL(depthOdomDataScan2d);
00419 SYNC_DEL(depthOdomDataScan3d);
00420 SYNC_DEL(depthOdomDataInfo);
00421 SYNC_DEL(depthOdomDataScan2dInfo);
00422 SYNC_DEL(depthOdomDataScan3dInfo);
00423
00424
00425 SYNC_DEL(stereo);
00426 SYNC_DEL(stereoInfo);
00427
00428
00429 SYNC_DEL(stereoOdom);
00430 SYNC_DEL(stereoOdomInfo);
00431
00432
00433 SYNC_DEL(rgbdScan2d);
00434 SYNC_DEL(rgbdScan3d);
00435 SYNC_DEL(rgbdInfo);
00436 SYNC_DEL(rgbdScan2dInfo);
00437 SYNC_DEL(rgbdScan3dInfo);
00438
00439
00440 SYNC_DEL(rgbdOdom);
00441 SYNC_DEL(rgbdOdomScan2d);
00442 SYNC_DEL(rgbdOdomScan3d);
00443 SYNC_DEL(rgbdOdomInfo);
00444 SYNC_DEL(rgbdOdomScan2dInfo);
00445 SYNC_DEL(rgbdOdomScan3dInfo);
00446
00447
00448 SYNC_DEL(rgbdData);
00449 SYNC_DEL(rgbdDataScan2d);
00450 SYNC_DEL(rgbdDataScan3d);
00451 SYNC_DEL(rgbdDataInfo);
00452 SYNC_DEL(rgbdDataScan2dInfo);
00453 SYNC_DEL(rgbdDataScan3dInfo);
00454
00455
00456 SYNC_DEL(rgbdOdomData);
00457 SYNC_DEL(rgbdOdomDataScan2d);
00458 SYNC_DEL(rgbdOdomDataScan3d);
00459 SYNC_DEL(rgbdOdomDataInfo);
00460 SYNC_DEL(rgbdOdomDataScan2dInfo);
00461 SYNC_DEL(rgbdOdomDataScan3dInfo);
00462
00463
00464 SYNC_DEL(rgbd2);
00465 SYNC_DEL(rgbd2Scan2d);
00466 SYNC_DEL(rgbd2Scan3d);
00467 SYNC_DEL(rgbd2Info);
00468 SYNC_DEL(rgbd2Scan2dInfo);
00469 SYNC_DEL(rgbd2Scan3dInfo);
00470
00471
00472 SYNC_DEL(rgbd2Odom);
00473 SYNC_DEL(rgbd2OdomScan2d);
00474 SYNC_DEL(rgbd2OdomScan3d);
00475 SYNC_DEL(rgbd2OdomInfo);
00476 SYNC_DEL(rgbd2OdomScan2dInfo);
00477 SYNC_DEL(rgbd2OdomScan3dInfo);
00478
00479
00480 SYNC_DEL(rgbd2Data);
00481 SYNC_DEL(rgbd2DataScan2d);
00482 SYNC_DEL(rgbd2DataScan3d);
00483 SYNC_DEL(rgbd2DataInfo);
00484 SYNC_DEL(rgbd2DataScan2dInfo);
00485 SYNC_DEL(rgbd2DataScan3dInfo);
00486
00487
00488 SYNC_DEL(rgbd2OdomData);
00489 SYNC_DEL(rgbd2OdomDataScan2d);
00490 SYNC_DEL(rgbd2OdomDataScan3d);
00491 SYNC_DEL(rgbd2OdomDataInfo);
00492 SYNC_DEL(rgbd2OdomDataScan2dInfo);
00493 SYNC_DEL(rgbd2OdomDataScan3dInfo);
00494
00495
00496 SYNC_DEL(rgbd3);
00497 SYNC_DEL(rgbd3Scan2d);
00498 SYNC_DEL(rgbd3Scan3d);
00499 SYNC_DEL(rgbd3Info);
00500 SYNC_DEL(rgbd3Scan2dInfo);
00501 SYNC_DEL(rgbd3Scan3dInfo);
00502
00503
00504 SYNC_DEL(rgbd3Odom);
00505 SYNC_DEL(rgbd3OdomScan2d);
00506 SYNC_DEL(rgbd3OdomScan3d);
00507 SYNC_DEL(rgbd3OdomInfo);
00508 SYNC_DEL(rgbd3OdomScan2dInfo);
00509 SYNC_DEL(rgbd3OdomScan3dInfo);
00510
00511
00512 SYNC_DEL(rgbd3Data);
00513 SYNC_DEL(rgbd3DataScan2d);
00514 SYNC_DEL(rgbd3DataScan3d);
00515 SYNC_DEL(rgbd3DataInfo);
00516 SYNC_DEL(rgbd3DataScan2dInfo);
00517 SYNC_DEL(rgbd3DataScan3dInfo);
00518
00519
00520 SYNC_DEL(rgbd3OdomData);
00521 SYNC_DEL(rgbd3OdomDataScan2d);
00522 SYNC_DEL(rgbd3OdomDataScan3d);
00523 SYNC_DEL(rgbd3OdomDataInfo);
00524 SYNC_DEL(rgbd3OdomDataScan2dInfo);
00525 SYNC_DEL(rgbd3OdomDataScan3dInfo);
00526
00527
00528 SYNC_DEL(rgbd4);
00529 SYNC_DEL(rgbd4Scan2d);
00530 SYNC_DEL(rgbd4Scan3d);
00531 SYNC_DEL(rgbd4Info);
00532 SYNC_DEL(rgbd4Scan2dInfo);
00533 SYNC_DEL(rgbd4Scan3dInfo);
00534
00535
00536 SYNC_DEL(rgbd4Odom);
00537 SYNC_DEL(rgbd4OdomScan2d);
00538 SYNC_DEL(rgbd4OdomScan3d);
00539 SYNC_DEL(rgbd4OdomInfo);
00540 SYNC_DEL(rgbd4OdomScan2dInfo);
00541 SYNC_DEL(rgbd4OdomScan3dInfo);
00542
00543
00544 SYNC_DEL(rgbd4Data);
00545 SYNC_DEL(rgbd4DataScan2d);
00546 SYNC_DEL(rgbd4DataScan3d);
00547 SYNC_DEL(rgbd4DataInfo);
00548 SYNC_DEL(rgbd4DataScan2dInfo);
00549 SYNC_DEL(rgbd4DataScan3dInfo);
00550
00551
00552 SYNC_DEL(rgbd4OdomData);
00553 SYNC_DEL(rgbd4OdomDataScan2d);
00554 SYNC_DEL(rgbd4OdomDataScan3d);
00555 SYNC_DEL(rgbd4OdomDataInfo);
00556 SYNC_DEL(rgbd4OdomDataScan2dInfo);
00557 SYNC_DEL(rgbd4OdomDataScan3dInfo);
00558
00559 for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
00560 {
00561 delete rgbdSubs_[i];
00562 }
00563 rgbdSubs_.clear();
00564 }
00565
00566 void CommonDataSubscriber::warningLoop()
00567 {
00568 ros::Duration r(5.0);
00569 while(!callbackCalled_)
00570 {
00571 r.sleep();
00572 if(!callbackCalled_)
00573 {
00574 ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
00575 "published (\"$ rostopic hz my_topic\") and the timestamps in their "
00576 "header are set. If topics are coming from different computers, make sure "
00577 "the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
00578 name_.c_str(),
00579 approxSync_?
00580 uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
00581 "Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
00582 subscribedTopicsMsg_.c_str());
00583 }
00584 }
00585 }
00586
00587 void CommonDataSubscriber::commonSingleDepthCallback(
00588 const nav_msgs::OdometryConstPtr & odomMsg,
00589 const rtabmap_ros::UserDataConstPtr & userDataMsg,
00590 const cv_bridge::CvImageConstPtr & imageMsg,
00591 const cv_bridge::CvImageConstPtr & depthMsg,
00592 const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
00593 const sensor_msgs::CameraInfo & depthCameraInfoMsg,
00594 const sensor_msgs::LaserScanConstPtr& scanMsg,
00595 const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
00596 const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
00597 {
00598 callbackCalled();
00599
00600 if(depthMsg.get() == 0 ||
00601 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
00602 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
00603 depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
00604 {
00605 std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
00606 std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
00607 std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
00608 if(imageMsg.get())
00609 {
00610 imageMsgs.push_back(imageMsg);
00611 }
00612 if(depthMsg.get())
00613 {
00614 depthMsgs.push_back(depthMsg);
00615 }
00616 cameraInfoMsgs.push_back(rgbCameraInfoMsg);
00617 commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
00618 }
00619 else
00620 {
00621 commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
00622 }
00623
00624
00625 }
00626
00627 }