From ae44e1a2157b84e67ade530f2b8e713eac9650e8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 11 Jun 2024 22:35:54 -0700 Subject: [PATCH] Split queue_size param into sync_queue_size and topic_queue_size parameters for more fine tuning of topic synchronization (#1054) --- rtabmap_launch/launch/rtabmap.launch | 25 +++- rtabmap_odom/src/nodelets/rgbd_odometry.cpp | 93 +++++++----- .../src/nodelets/rgbdicp_odometry.cpp | 47 ++++-- rtabmap_odom/src/nodelets/stereo_odometry.cpp | 95 +++++++----- .../rtabmap_sync/CommonDataSubscriber.h | 58 +++----- .../CommonDataSubscriberDefines.h | 2 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 78 +++++----- .../src/impl/CommonDataSubscriberDepth.cpp | 138 +++++++++--------- .../src/impl/CommonDataSubscriberOdom.cpp | 20 ++- .../src/impl/CommonDataSubscriberRGB.cpp | 136 +++++++++-------- .../src/impl/CommonDataSubscriberRGBD.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD2.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD3.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD4.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberRGBD5.cpp | 44 +++--- .../src/impl/CommonDataSubscriberRGBD6.cpp | 44 +++--- .../src/impl/CommonDataSubscriberRGBDX.cpp | 86 ++++++----- .../src/impl/CommonDataSubscriberScan.cpp | 86 ++++++----- .../impl/CommonDataSubscriberSensorData.cpp | 20 ++- .../src/impl/CommonDataSubscriberStereo.cpp | 26 ++-- rtabmap_sync/src/nodelets/rgb_sync.cpp | 28 +++- rtabmap_sync/src/nodelets/rgbd_sync.cpp | 30 +++- rtabmap_sync/src/nodelets/rgbdx_sync.cpp | 34 +++-- rtabmap_sync/src/nodelets/stereo_sync.cpp | 32 ++-- .../src/nodelets/obstacles_detection.cpp | 2 - .../src/nodelets/point_cloud_aggregator.cpp | 27 +++- .../src/nodelets/point_cloud_assembler.cpp | 34 +++-- rtabmap_util/src/nodelets/point_cloud_xyz.cpp | 33 +++-- .../src/nodelets/point_cloud_xyzrgb.cpp | 45 ++++-- .../src/nodelets/pointcloud_to_depthimage.cpp | 28 +++- rtabmap_util/src/nodelets/rgbd_relay.cpp | 5 - rtabmap_util/src/nodelets/rgbd_split.cpp | 5 - rtabmap_viz/src/GuiWrapper.cpp | 16 +- 33 files changed, 882 insertions(+), 779 deletions(-) diff --git a/rtabmap_launch/launch/rtabmap.launch b/rtabmap_launch/launch/rtabmap.launch index 3cd5a90de..19ea0b14a 100644 --- a/rtabmap_launch/launch/rtabmap.launch +++ b/rtabmap_launch/launch/rtabmap.launch @@ -47,7 +47,9 @@ - + + + @@ -181,7 +183,8 @@ - + + @@ -203,7 +206,8 @@ - + + @@ -253,7 +257,8 @@ - + + @@ -283,7 +288,8 @@ - + + @@ -310,7 +316,8 @@ - + + @@ -377,7 +384,8 @@ - + + @@ -434,7 +442,8 @@ - + + diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index f1932c7dc..bda1e1a96 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -74,7 +74,8 @@ class RGBDOdometry : public OdometryROS exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(1), + syncQueueSize_(5), keepColor_(false) { } @@ -110,7 +111,19 @@ class RGBDOdometry : public OdometryROS double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); if(pnh.hasParam("depth_cameras")) { @@ -126,7 +139,8 @@ class RGBDOdometry : public OdometryROS NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_); + NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras); NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); @@ -137,23 +151,23 @@ class RGBDOdometry : public OdometryROS { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); - rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_); } if(rgbdCameras == 2) @@ -161,7 +175,7 @@ class RGBDOdometry : public OdometryROS if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -171,7 +185,7 @@ class RGBDOdometry : public OdometryROS else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -188,7 +202,7 @@ class RGBDOdometry : public OdometryROS if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -199,7 +213,7 @@ class RGBDOdometry : public OdometryROS else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -218,7 +232,7 @@ class RGBDOdometry : public OdometryROS if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -230,7 +244,7 @@ class RGBDOdometry : public OdometryROS else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -251,7 +265,7 @@ class RGBDOdometry : public OdometryROS if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -264,7 +278,7 @@ class RGBDOdometry : public OdometryROS else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -287,7 +301,7 @@ class RGBDOdometry : public OdometryROS if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -301,7 +315,7 @@ class RGBDOdometry : public OdometryROS else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -333,7 +347,7 @@ class RGBDOdometry : public OdometryROS } else if(rgbdCameras == 0) { - rgbdxSub_ = nh.subscribe("rgbd_images", 1, &RGBDOdometry::callbackRGBDX, this); + rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &RGBDOdometry::callbackRGBDX, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -342,7 +356,7 @@ class RGBDOdometry : public OdometryROS } else { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this); + rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &RGBDOdometry::callbackRGBD, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -361,20 +375,20 @@ class RGBDOdometry : public OdometryROS image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); if(approxSyncMaxInterval > 0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } @@ -758,20 +772,20 @@ class RGBDOdometry : public OdometryROS if(approxSync_) { delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } if(exactSync_) { delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -780,7 +794,7 @@ class RGBDOdometry : public OdometryROS { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -789,7 +803,7 @@ class RGBDOdometry : public OdometryROS { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -799,7 +813,7 @@ class RGBDOdometry : public OdometryROS { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -809,7 +823,7 @@ class RGBDOdometry : public OdometryROS { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -820,7 +834,7 @@ class RGBDOdometry : public OdometryROS { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -831,7 +845,7 @@ class RGBDOdometry : public OdometryROS { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -843,7 +857,7 @@ class RGBDOdometry : public OdometryROS { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -855,7 +869,7 @@ class RGBDOdometry : public OdometryROS { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -868,7 +882,7 @@ class RGBDOdometry : public OdometryROS { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -917,7 +931,8 @@ class RGBDOdometry : public OdometryROS message_filters::Synchronizer * approxSync6_; typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; message_filters::Synchronizer * exactSync6_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; bool keepColor_; }; diff --git a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp index 65e9b8cc7..99b5ddb11 100644 --- a/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp @@ -72,7 +72,8 @@ class RGBDICPOdometry : public OdometryROS exactScanSync_(0), approxCloudSync_(0), exactCloudSync_(0), - queueSize_(5), + queueSize_(1), + syncQueueSize_(5), keepColor_(false), scanCloudMaxPoints_(0), scanVoxelSize_(0.0), @@ -113,7 +114,19 @@ class RGBDICPOdometry : public OdometryROS double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", queueSize_, queueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); @@ -130,7 +143,8 @@ class RGBDICPOdometry : public OdometryROS NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_); + NODELET_INFO("RGBDIcpOdometry: topic_queue_size = %d", queueSize_); + NODELET_INFO("RGBDIcpOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_); @@ -147,24 +161,24 @@ class RGBDICPOdometry : public OdometryROS image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - info_sub_.subscribe(rgb_nh, "camera_info", 1); + image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize_, hintsRgb); + image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize_, hintsDepth); + info_sub_.subscribe(rgb_nh, "camera_info", queueSize_); std::string subscribedTopicsMsg; if(subscribeScanCloud) { - cloud_sub_.subscribe(nh, "scan_cloud", 1); + cloud_sub_.subscribe(nh, "scan_cloud", queueSize_); if(approxSync) { - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); if(approxSyncMaxInterval > 0.0) approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -179,17 +193,17 @@ class RGBDICPOdometry : public OdometryROS } else { - scan_sub_.subscribe(nh, "scan", 1); + scan_sub_.subscribe(nh, "scan", queueSize_); if(approxSync) { - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); if(approxSyncMaxInterval > 0.0) approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -460,25 +474,25 @@ class RGBDICPOdometry : public OdometryROS if(approxScanSync_) { delete approxScanSync_; - approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + approxScanSync_ = new message_filters::Synchronizer(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactScanSync_) { delete exactScanSync_; - exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); + exactScanSync_ = new message_filters::Synchronizer(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(approxCloudSync_) { delete approxCloudSync_; - approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + approxCloudSync_ = new message_filters::Synchronizer(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactCloudSync_) { delete exactCloudSync_; - exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); + exactCloudSync_ = new message_filters::Synchronizer(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } } @@ -498,6 +512,7 @@ class RGBDICPOdometry : public OdometryROS typedef message_filters::sync_policies::ApproximateTime MyExactCloudSyncPolicy; message_filters::Synchronizer * exactCloudSync_; int queueSize_; + int syncQueueSize_; bool keepColor_; int scanCloudMaxPoints_; double scanVoxelSize_; diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index 889de3656..e698c38d6 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -74,7 +74,8 @@ class StereoOdometry : public OdometryROS exactSync5_(0), approxSync6_(0), exactSync6_(0), - queueSize_(5), + topicQueueSize_(1), + syncQueueSize_(5), keepColor_(false) { } @@ -109,7 +110,19 @@ class StereoOdometry : public OdometryROS int rgbdCameras = 1; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); pnh.param("keep_color", keepColor_, keepColor_); @@ -117,7 +130,8 @@ class StereoOdometry : public OdometryROS NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); if(approxSync) NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); - NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_); + NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_); + NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_); NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false"); @@ -127,23 +141,23 @@ class StereoOdometry : public OdometryROS { if(rgbdCameras >= 2) { - rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); - rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); + rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_); + rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_); if(rgbdCameras >= 3) { - rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); + rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_); } if(rgbdCameras >= 4) { - rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); + rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_); } if(rgbdCameras >= 5) { - rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); + rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_); } if(rgbdCameras >= 6) { - rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); + rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_); } if(rgbdCameras == 2) @@ -151,7 +165,7 @@ class StereoOdometry : public OdometryROS if(approxSync) { approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); if(approxSyncMaxInterval > 0.0) @@ -161,7 +175,7 @@ class StereoOdometry : public OdometryROS else { exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -178,7 +192,7 @@ class StereoOdometry : public OdometryROS if(approxSync) { approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -189,7 +203,7 @@ class StereoOdometry : public OdometryROS else { exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -208,7 +222,7 @@ class StereoOdometry : public OdometryROS if(approxSync) { approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -220,7 +234,7 @@ class StereoOdometry : public OdometryROS else { exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -241,7 +255,7 @@ class StereoOdometry : public OdometryROS if(approxSync) { approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -254,7 +268,7 @@ class StereoOdometry : public OdometryROS else { exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -277,7 +291,7 @@ class StereoOdometry : public OdometryROS if(approxSync) { approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -291,7 +305,7 @@ class StereoOdometry : public OdometryROS else { exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -323,7 +337,7 @@ class StereoOdometry : public OdometryROS } else if(rgbdCameras == 0) { - rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this); + rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &StereoOdometry::callbackRGBDX, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -332,7 +346,7 @@ class StereoOdometry : public OdometryROS } else { - rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); + rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &StereoOdometry::callbackRGBD, this); subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s", @@ -351,21 +365,21 @@ class StereoOdometry : public OdometryROS image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), topicQueueSize_, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), topicQueueSize_, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_); + cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_); if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); if(approxSyncMaxInterval>0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -897,20 +911,20 @@ class StereoOdometry : public OdometryROS if(approxSync_) { delete approxSync_; - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(exactSync_) { delete exactSync_; - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } if(approxSync2_) { delete approxSync2_; approxSync2_ = new message_filters::Synchronizer( - MyApproxSync2Policy(queueSize_), + MyApproxSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -919,7 +933,7 @@ class StereoOdometry : public OdometryROS { delete exactSync2_; exactSync2_ = new message_filters::Synchronizer( - MyExactSync2Policy(queueSize_), + MyExactSync2Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); @@ -928,7 +942,7 @@ class StereoOdometry : public OdometryROS { delete approxSync3_; approxSync3_ = new message_filters::Synchronizer( - MyApproxSync3Policy(queueSize_), + MyApproxSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -938,7 +952,7 @@ class StereoOdometry : public OdometryROS { delete exactSync3_; exactSync3_ = new message_filters::Synchronizer( - MyExactSync3Policy(queueSize_), + MyExactSync3Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_); @@ -948,7 +962,7 @@ class StereoOdometry : public OdometryROS { delete approxSync4_; approxSync4_ = new message_filters::Synchronizer( - MyApproxSync4Policy(queueSize_), + MyApproxSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -959,7 +973,7 @@ class StereoOdometry : public OdometryROS { delete exactSync4_; exactSync4_ = new message_filters::Synchronizer( - MyExactSync4Policy(queueSize_), + MyExactSync4Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -970,7 +984,7 @@ class StereoOdometry : public OdometryROS { delete approxSync5_; approxSync5_ = new message_filters::Synchronizer( - MyApproxSync5Policy(queueSize_), + MyApproxSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -982,7 +996,7 @@ class StereoOdometry : public OdometryROS { delete exactSync5_; exactSync5_ = new message_filters::Synchronizer( - MyExactSync5Policy(queueSize_), + MyExactSync5Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -994,7 +1008,7 @@ class StereoOdometry : public OdometryROS { delete approxSync6_; approxSync6_ = new message_filters::Synchronizer( - MyApproxSync6Policy(queueSize_), + MyApproxSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1007,7 +1021,7 @@ class StereoOdometry : public OdometryROS { delete exactSync6_; exactSync6_ = new message_filters::Synchronizer( - MyExactSync6Policy(queueSize_), + MyExactSync6Policy(syncQueueSize_), rgbd_image1_sub_, rgbd_image2_sub_, rgbd_image3_sub_, @@ -1058,7 +1072,8 @@ class StereoOdometry : public OdometryROS typedef message_filters::sync_policies::ExactTime MyExactSync6Policy; message_filters::Synchronizer * exactSync6_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; bool keepColor_; }; diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index 6c486fcb4..79b6127a4 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -74,7 +74,8 @@ class CommonDataSubscriber { bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;} bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();} int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;} - int getQueueSize() const {return queueSize_;} + int getTopicQueueSize() const {return topicQueueSize_;} + int getSyncQueueSize() const {return syncQueueSize_;} bool isApproxSync() const {return approxSync_;} const std::string & name() const {return name_;} @@ -140,16 +141,12 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupStereoCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -158,9 +155,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -169,9 +164,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBDXCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -180,9 +173,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #ifdef RTABMAP_SYNC_MULTI_RGBD void setupRGBD2Callbacks( ros::NodeHandle & nh, @@ -192,9 +183,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD3Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -203,9 +192,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD4Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -214,9 +201,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD5Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -225,9 +210,7 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupRGBD6Callbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -236,17 +219,13 @@ class CommonDataSubscriber { bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); #endif void setupSensorDataCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupScanCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, @@ -254,20 +233,17 @@ class CommonDataSubscriber { bool subscribeScanDesc, bool subscribeOdom, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); void setupOdomCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync); + bool subscribeOdomInfo); protected: std::string subscribedTopicsMsg_; - int queueSize_; + int topicQueueSize_; + int syncQueueSize_; private: bool approxSync_; diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h index e614c5c25..a8938ba1a 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h @@ -180,7 +180,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. } \ subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \ name_.c_str(), \ - approxSync?"approx":"exact", \ + APPROX?"approx":"exact", \ SUB0.getTopic().c_str(), \ SUB1.getTopic().c_str(), \ SUB2.getTopic().c_str(), \ diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 0960064d5..e95597ed7 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -30,7 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace rtabmap_sync { CommonDataSubscriber::CommonDataSubscriber(bool gui) : - queueSize_(10), + topicQueueSize_(10), + syncQueueSize_(10), approxSync_(true), subscribedToDepth_(!gui), subscribedToStereo_(false), @@ -504,11 +505,23 @@ void CommonDataSubscriber::setupCallbacks( { ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" used when \"subscribe_rgbd\" is true."); } - pnh.param("queue_size", queueSize_, queueSize_); + pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize_, syncQueueSize_); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize_); + } + else + { + pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_); + } if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync")) { ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " - "to \"approx_sync\"! Your value is still copied to " + "to \"approx_sync\"! Your value is copied to " "corresponding parameter."); pnh.param("stereo_approx_sync", approxSync_, approxSync_); } @@ -527,8 +540,9 @@ void CommonDataSubscriber::setupCallbacks( ROS_INFO("%s: subscribe_scan = %s", name.c_str(), subscribeScan2d?"true":"false"); ROS_INFO("%s: subscribe_scan_cloud = %s", name.c_str(), subscribeScan3d?"true":"false"); ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"true":"false"); - ROS_INFO("%s: queue_size = %d", name.c_str(), queueSize_); - ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); + ROS_INFO("%s: topic_queue_size = %d", name.c_str(), topicQueueSize_); + ROS_INFO("%s: sync_queue_size = %d", name.c_str(), syncQueueSize_); + ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); subscribedToOdom_ = odomFrameId.empty() && subscribeOdom; if(subscribedToDepth_) @@ -541,9 +555,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToStereo_) { @@ -551,9 +563,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribedToOdom_, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToRGB_) { @@ -565,9 +575,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToRGBD_) { @@ -589,9 +597,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 5) { @@ -603,9 +609,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 4) { @@ -617,9 +621,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 3) { @@ -631,9 +633,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(rgbdCameras == 2) { @@ -645,9 +645,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } #else if(rgbdCameras>1) @@ -668,9 +666,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else { @@ -682,9 +678,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScan2d, subscribeScan3d, subscribeScanDesc, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } } else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc) @@ -696,9 +690,7 @@ void CommonDataSubscriber::setupCallbacks( subscribeScanDesc, subscribedToOdom_, subscribeUserData, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToSensorData_) { @@ -706,9 +698,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribedToOdom_, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } else if(subscribedToOdom_) { @@ -716,9 +706,7 @@ void CommonDataSubscriber::setupCallbacks( nh, pnh, subscribeUserData, - subscribeOdomInfo, - queueSize_, - approxSync_); + subscribeOdomInfo); } if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_) @@ -732,7 +720,7 @@ void CommonDataSubscriber::setupCallbacks( "the clocks of the computers are synchronized (\"ntpdate\"). %s%s", name_.c_str(), approxSync_? - uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str(): + uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str(): "Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.", subscribedTopicsMsg_.c_str()), otherTasks); diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 0b2dc8973..b2494c236 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -463,9 +463,7 @@ void CommonDataSubscriber::setupDepthCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup depth callback"); @@ -480,195 +478,195 @@ void CommonDataSubscriber::setupDepthCallbacks( image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); } } #endif @@ -677,57 +675,57 @@ void CommonDataSubscriber::setupDepthCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp index d4cab009e..786235179 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -67,29 +67,27 @@ void CommonDataSubscriber::setupOdomCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup scan callback"); if(subscribeUserData || subscribeOdomInfo) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_); + SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_); } } else @@ -97,13 +95,13 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_); } } else { - odomSubOnly_ = nh.subscribe("odom", queueSize, &CommonDataSubscriber::odomCallback, this); + odomSubOnly_ = nh.subscribe("odom", syncQueueSize_, &CommonDataSubscriber::odomCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index 5e6867b7e..30b2dcecd 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -463,9 +463,7 @@ void CommonDataSubscriber::setupRGBCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgb-only callback"); @@ -475,194 +473,194 @@ void CommonDataSubscriber::setupRGBCallbacks( image_transport::ImageTransport rgb_it(rgb_nh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), syncQueueSize_, hintsRgb); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_); + SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_); } } #endif @@ -671,57 +669,57 @@ void CommonDataSubscriber::setupRGBCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_); } } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_); } } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_); } } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_); + SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index e620b4482..582809b61 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -539,9 +539,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd callback"); @@ -556,152 +554,152 @@ void CommonDataSubscriber::setupRGBDCallbacks( { rgbdSubs_.resize(1); rgbdSubs_[0] = new message_filters::Subscriber; - rgbdSubs_[0]->subscribe(nh, "rgbd_image", queueSize); + rgbdSubs_[0]->subscribe(nh, "rgbd_image", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0])); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0])); + SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); + SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0])); } } #endif @@ -710,41 +708,41 @@ void CommonDataSubscriber::setupRGBDCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_); } else { @@ -754,7 +752,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( } else { - rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this); + rgbdSub_ = nh.subscribe("rgbd_image", syncQueueSize_, &CommonDataSubscriber::rgbdCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index adbaabcba..0fb4df15d 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -350,9 +350,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd2 callback"); @@ -360,152 +358,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks( for(int i=0; i<2; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } #endif @@ -514,45 +512,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index 77c045e12..91f73d01c 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -438,9 +438,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDescriptor, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd3 callback"); @@ -448,151 +446,151 @@ void CommonDataSubscriber::setupRGBD3Callbacks( for(int i=0; i<3; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } #endif @@ -601,45 +599,45 @@ void CommonDataSubscriber::setupRGBD3Callbacks( if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index b4289e228..637645ca5 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -407,9 +407,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd4 callback"); @@ -417,152 +415,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks( for(int i=0; i<4; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } #endif @@ -571,45 +569,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index 1e6ceac9d..c98221546 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -263,9 +263,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd5 callback"); @@ -273,53 +271,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks( for(int i=0; i<5; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); } } else @@ -327,45 +325,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index faf21b0ba..7dbfcd15d 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -281,9 +281,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbd6 callback"); @@ -291,53 +289,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks( for(int i=0; i<6; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), queueSize); + rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), topicQueueSize_); } if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); } } else @@ -345,45 +343,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else { - SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); } } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index 7f28041a4..5e9d425bc 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -326,157 +326,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks( bool subscribeScan2d, bool subscribeScan3d, bool subscribeScanDesc, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup rgbdX callback"); - rgbdXSub_.subscribe(nh, "rgbd_images", queueSize); + rgbdXSub_.subscribe(nh, "rgbd_images", topicQueueSize_); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_); } } else #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_); } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_); } } #endif @@ -485,46 +483,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync_, syncQueueSize_, rgbdXSub_, scanDescSub_); } else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync_, syncQueueSize_, rgbdXSub_, scanSub_); } else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; ROS_WARN("subscribe_odom_info ignored..."); } - SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync_, syncQueueSize_, rgbdXSub_, scan3dSub_); } else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_); } else { rgbdXSub_.unsubscribe(); - rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this); + rgbdXSubOnly_ = nh.subscribe("rgbd_images", syncQueueSize_, &CommonDataSubscriber::rgbdXCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp index 549500216..992b5b5e9 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -250,9 +250,7 @@ void CommonDataSubscriber::setupScanCallbacks( bool scanDescTopic, bool subscribeOdom, bool subscribeUserData, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup scan callback"); @@ -261,36 +259,36 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); + scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_); } else if(scan2dTopic) { subscribedToScan2d_ = true; - scanSub_.subscribe(nh, "scan", queueSize); + scanSub_.subscribe(nh, "scan", topicQueueSize_); } else { subscribedToScan3d_ = true; - scan3dSub_.subscribe(nh, "scan_cloud", queueSize); + scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(nh, "odom", queueSize); - userDataSub_.subscribe(nh, "user_data", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -298,12 +296,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_); } } else @@ -311,12 +309,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_); + SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_); } } } @@ -324,19 +322,19 @@ void CommonDataSubscriber::setupScanCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_); } } else if(scan2dTopic) @@ -344,12 +342,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_); } } else @@ -357,31 +355,31 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_); } } } #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(nh, "user_data", queueSize); + userDataSub_.subscribe(nh, "user_data", topicQueueSize_); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_); + SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_); } } else if(scan2dTopic) @@ -389,12 +387,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_); } } else @@ -402,12 +400,12 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_); + SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_); } } } @@ -415,18 +413,18 @@ void CommonDataSubscriber::setupScanCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); if(scanDescTopic) { - SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_); } else if(scan2dTopic) { - SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_); + SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync_, syncQueueSize_, scan3dSub_, odomInfoSub_); } } } @@ -435,7 +433,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSubOnly_ = nh.subscribe("scan_descriptor", queueSize, &CommonDataSubscriber::scanDescCallback, this); + scanDescSubOnly_ = nh.subscribe("scan_descriptor", syncQueueSize_, &CommonDataSubscriber::scanDescCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), @@ -444,7 +442,7 @@ void CommonDataSubscriber::setupScanCallbacks( else if(scan2dTopic) { subscribedToScan2d_ = true; - scan2dSubOnly_ = nh.subscribe("scan", queueSize, &CommonDataSubscriber::scan2dCallback, this); + scan2dSubOnly_ = nh.subscribe("scan", syncQueueSize_, &CommonDataSubscriber::scan2dCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), @@ -453,7 +451,7 @@ void CommonDataSubscriber::setupScanCallbacks( else { subscribedToScan3d_ = true; - scan3dSubOnly_ = nh.subscribe("scan_cloud", queueSize, &CommonDataSubscriber::scan3dCallback, this); + scan3dSubOnly_ = nh.subscribe("scan_cloud", syncQueueSize_, &CommonDataSubscriber::scan3dCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", ros::this_node::getName().c_str(), diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index b54a7c333..5e6a7a390 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -68,25 +68,23 @@ void CommonDataSubscriber::setupSensorDataCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup SensorData callback"); - sensorDataSub_.subscribe(nh, "sensor_data", queueSize); + sensorDataSub_.subscribe(nh, "sensor_data", topicQueueSize_); if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_); } else { - SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_); + SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_); } } else @@ -94,13 +92,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_); } else { sensorDataSub_.unsubscribe(); - sensorDataSubOnly_ = nh.subscribe("sensor_data", queueSize, &CommonDataSubscriber::sensorDataCallback, this); + sensorDataSubOnly_ = nh.subscribe("sensor_data", syncQueueSize_, &CommonDataSubscriber::sensorDataCallback, this); subscribedTopicsMsg_ = uFormat("\n%s subscribed to:\n %s", diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 4c67845a2..e09dbab44 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -89,9 +89,7 @@ void CommonDataSubscriber::setupStereoCallbacks( ros::NodeHandle & nh, ros::NodeHandle & pnh, bool subscribeOdom, - bool subscribeOdomInfo, - int queueSize, - bool approxSync) + bool subscribeOdomInfo) { ROS_INFO("Setup stereo callback"); @@ -104,24 +102,24 @@ void CommonDataSubscriber::setupStereoCallbacks( image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), queueSize, hintsLeft); - imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), queueSize, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize); - cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize); + imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), syncQueueSize_, hintsLeft); + imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), syncQueueSize_, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_); + cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_); if(subscribeOdom) { - odomSub_.subscribe(nh, "odom", queueSize); + odomSub_.subscribe(nh, "odom", topicQueueSize_); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); } } else @@ -129,12 +127,12 @@ void CommonDataSubscriber::setupStereoCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(nh, "odom_info", queueSize); - SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); + odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_); + SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else { - SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); + SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); } } } diff --git a/rtabmap_sync/src/nodelets/rgb_sync.cpp b/rtabmap_sync/src/nodelets/rgb_sync.cpp index 688c3ffd4..35ffc3827 100644 --- a/rtabmap_sync/src/nodelets/rgb_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgb_sync.cpp @@ -77,18 +77,32 @@ class RgbSync : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("compressed_rate", compressedRate_, compressedRate_); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); rgbdImagePub_ = nh.advertise("rgbd_image", 1); @@ -96,14 +110,14 @@ class RgbSync : public nodelet::Nodelet if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_); exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } @@ -112,8 +126,8 @@ class RgbSync : public nodelet::Nodelet image_transport::ImageTransport rgb_it(rgb_nh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), queueSize, hintsRgb); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_sync/src/nodelets/rgbd_sync.cpp b/rtabmap_sync/src/nodelets/rgbd_sync.cpp index eb2665ec7..70d7607d4 100644 --- a/rtabmap_sync/src/nodelets/rgbd_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbd_sync.cpp @@ -81,12 +81,25 @@ class RGBDSync : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("depth_scale", depthScale_, depthScale_); pnh.param("decimation", decimation_, decimation_); pnh.param("compressed_rate", compressedRate_, compressedRate_); @@ -99,7 +112,8 @@ class RGBDSync : public nodelet::Nodelet NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: depth_scale = %f", getName().c_str(), depthScale_); NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); @@ -109,14 +123,14 @@ class RGBDSync : public nodelet::Nodelet if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } @@ -129,9 +143,9 @@ class RGBDSync : public nodelet::Nodelet image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp index f68c56b3e..6fb187ae7 100644 --- a/rtabmap_sync/src/nodelets/rgbdx_sync.cpp +++ b/rtabmap_sync/src/nodelets/rgbdx_sync.cpp @@ -77,19 +77,33 @@ class RGBDXSync : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; int rgbdCameras = 2; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); if(approxSync) NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras); rgbdImagesPub_ = nh.advertise("rgbd_images", 1); @@ -107,7 +121,7 @@ class RGBDXSync : public nodelet::Nodelet std::string subscribedTopicsMsg_; if(rgbdCameras==2) { - SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); + SYNC_DECL2(RGBDXSync, rgbd2, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -115,7 +129,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==3) { - SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); + SYNC_DECL3(RGBDXSync, rgbd3, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -123,7 +137,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==4) { - SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); + SYNC_DECL4(RGBDXSync, rgbd4, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -131,7 +145,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==5) { - SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); + SYNC_DECL5(RGBDXSync, rgbd5, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -139,7 +153,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==6) { - SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); + SYNC_DECL6(RGBDXSync, rgbd6, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -147,7 +161,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==7) { - SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6])); + SYNC_DECL7(RGBDXSync, rgbd7, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); @@ -155,7 +169,7 @@ class RGBDXSync : public nodelet::Nodelet } else if(rgbdCameras==8) { - SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7])); + SYNC_DECL8(RGBDXSync, rgbd8, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7])); if(approxSync && approxSyncMaxInterval>0.0) { rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); diff --git a/rtabmap_sync/src/nodelets/stereo_sync.cpp b/rtabmap_sync/src/nodelets/stereo_sync.cpp index 82be9b0d7..ebb6ad2c2 100644 --- a/rtabmap_sync/src/nodelets/stereo_sync.cpp +++ b/rtabmap_sync/src/nodelets/stereo_sync.cpp @@ -77,18 +77,32 @@ class StereoSync : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = false; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); if(approxSync) pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is still copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("compressed_rate", compressedRate_, compressedRate_); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: topic_queue_size = %d", getName().c_str(), queueSize); + NODELET_INFO("%s: sync_queue_size = %d", getName().c_str(), syncQueueSize); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); rgbdImagePub_ = nh.advertise("rgbd_image", 1); @@ -96,14 +110,14 @@ class StereoSync : public nodelet::Nodelet if(approxSync) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); if(approxSyncMaxInterval>0.0) approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_); exactSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -116,10 +130,10 @@ class StereoSync : public nodelet::Nodelet image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh); - imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb); - imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth); - cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1); - cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1); + imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), queueSize, hintsRgb); + imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), queueSize, hintsDepth); + cameraInfoLeftSub_.subscribe(left_nh, "camera_info", queueSize); + cameraInfoRightSub_.subscribe(right_nh, "camera_info", queueSize); std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s", getName().c_str(), diff --git a/rtabmap_util/src/nodelets/obstacles_detection.cpp b/rtabmap_util/src/nodelets/obstacles_detection.cpp index 1fd0038b8..b5213276d 100644 --- a/rtabmap_util/src/nodelets/obstacles_detection.cpp +++ b/rtabmap_util/src/nodelets/obstacles_detection.cpp @@ -113,8 +113,6 @@ class ObstaclesDetection : public nodelet::Nodelet ULogger::setType(ULogger::kTypeConsole); ULogger::setLevel(ULogger::kWarning); - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); pnh.param("frame_id", frameId_, frameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 65f4a47f5..80493365a 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -99,11 +99,24 @@ class PointCloudAggregator : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 5; + int queueSize = 1; + int syncQueueSize = 5; int count = 2; bool approx=true; double approxSyncMaxInterval = 0.0; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("frame_id", frameId_, frameId_); pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("approx_sync", approx, approx); @@ -112,14 +125,14 @@ class PointCloudAggregator : public nodelet::Nodelet pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("xyz_output", xyzOutput_, xyzOutput_); - cloudSub_1_.subscribe(nh, "cloud1", 1); - cloudSub_2_.subscribe(nh, "cloud2", 1); + cloudSub_1_.subscribe(nh, "cloud1", queueSize); + cloudSub_2_.subscribe(nh, "cloud2", queueSize); std::string subscribedTopicsMsg; if(count == 4) { - cloudSub_3_.subscribe(nh, "cloud3", 1); - cloudSub_4_.subscribe(nh, "cloud4", 1); + cloudSub_3_.subscribe(nh, "cloud3", queueSize); + cloudSub_4_.subscribe(nh, "cloud4", queueSize); if(approx) { approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); @@ -143,7 +156,7 @@ class PointCloudAggregator : public nodelet::Nodelet } else if(count == 3) { - cloudSub_3_.subscribe(nh, "cloud3", 1); + cloudSub_3_.subscribe(nh, "cloud3", queueSize); if(approx) { approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index 33126a126..ead6cb0e0 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -109,10 +109,23 @@ class PointCloudAssembler : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 5; + int queueSize = 1; + int syncQueueSize = 5; bool subscribeOdomInfo = false; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("frame_id", frameId_, frameId_); pnh.param("max_clouds", maxClouds_, maxClouds_); @@ -131,7 +144,8 @@ class PointCloudAssembler : public nodelet::Nodelet pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0); - ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize); + ROS_INFO("%s: topic_queue_size=%d", getName().c_str(), queueSize); + ROS_INFO("%s: sync_queue_size=%d", getName().c_str(), syncQueueSize); ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str()); ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str()); ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_); @@ -166,10 +180,10 @@ class PointCloudAssembler : public nodelet::Nodelet } else if(subscribeOdomInfo) { - syncCloudSub_.subscribe(nh, "cloud", 1); - syncOdomSub_.subscribe(nh, "odom", 1); - syncOdomInfoSub_.subscribe(nh, "odom_info", 1); - exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); + syncCloudSub_.subscribe(nh, "cloud", queueSize); + syncOdomSub_.subscribe(nh, "odom", queueSize); + syncOdomInfoSub_.subscribe(nh, "odom_info", queueSize); + exactInfoSync_ = new message_filters::Synchronizer(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); exactInfoSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", getName().c_str(), @@ -181,9 +195,9 @@ class PointCloudAssembler : public nodelet::Nodelet } else { - syncCloudSub_.subscribe(nh, "cloud", 1); - syncOdomSub_.subscribe(nh, "odom", 1); - exactSync_ = new message_filters::Synchronizer(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_); + syncCloudSub_.subscribe(nh, "cloud", queueSize); + syncOdomSub_.subscribe(nh, "odom", queueSize); + exactSync_ = new message_filters::Synchronizer(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_); exactSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, boost::placeholders::_1, boost::placeholders::_2)); subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s", getName().c_str(), diff --git a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp index 1e8d12e9b..7ff99925f 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyz.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyz.cpp @@ -100,13 +100,26 @@ class PointCloudXYZ : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); @@ -171,22 +184,22 @@ class PointCloudXYZ : public nodelet::Nodelet if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2)); - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2)); } @@ -195,11 +208,11 @@ class PointCloudXYZ : public nodelet::Nodelet image_transport::ImageTransport depth_it(depth_nh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(depth_nh, "camera_info", queueSize); - disparitySub_.subscribe(nh, "disparity/image", 1); - disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); + disparitySub_.subscribe(nh, "disparity/image", queueSize); + disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", queueSize); cloudPub_ = nh.advertise("cloud", 1); } diff --git a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp index b57075ca5..6ff125f1d 100644 --- a/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp @@ -108,13 +108,26 @@ class PointCloudXYZRGB : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approxSync = true; std::string roiStr; double approxSyncMaxInterval = 0.0; pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("max_depth", maxDepth_, maxDepth_); pnh.param("min_depth", minDepth_, minDepth_); pnh.param("voxel_size", voxelSize_, voxelSize_); @@ -199,30 +212,30 @@ class PointCloudXYZRGB : public nodelet::Nodelet if(approxSync) { - approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + approxSyncDepth_ = new message_filters::Synchronizer(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); if(approxSyncMaxInterval > 0.0) approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + approxSyncDisparity_ = new message_filters::Synchronizer(MyApproxSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); if(approxSyncMaxInterval > 0.0) approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + approxSyncStereo_ = new message_filters::Synchronizer(MyApproxSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); if(approxSyncMaxInterval > 0.0) approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); + exactSyncDepth_ = new message_filters::Synchronizer(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_); exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); + exactSyncDisparity_ = new message_filters::Synchronizer(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_); exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); - exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); + exactSyncStereo_ = new message_filters::Synchronizer(MyExactSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_); exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } @@ -235,9 +248,9 @@ class PointCloudXYZRGB : public nodelet::Nodelet image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); - imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); - imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); - cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); + imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); + imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); + cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); ros::NodeHandle left_nh(nh, "left"); ros::NodeHandle right_nh(nh, "right"); @@ -248,12 +261,12 @@ class PointCloudXYZRGB : public nodelet::Nodelet image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); - imageDisparitySub_.subscribe(nh, "disparity", 1); + imageDisparitySub_.subscribe(nh, "disparity", queueSize); - imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft); - imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); - cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); - cameraInfoRight_.subscribe(right_nh, "camera_info", 1); + imageLeft_.subscribe(left_it, left_nh.resolveName("image"), queueSize, hintsLeft); + imageRight_.subscribe(right_it, right_nh.resolveName("image"), queueSize, hintsRight); + cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize); + cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize); } void depthCallback( diff --git a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp index 249015e46..87cd7fc7a 100644 --- a/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp +++ b/rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp @@ -92,9 +92,22 @@ class PointCloudToDepthImage : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; + int queueSize = 1; + int syncQueueSize = 10; bool approx = true; - pnh.param("queue_size", queueSize, queueSize); + pnh.param("topic_queue_size", queueSize, queueSize); + if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size")) + { + pnh.param("queue_size", syncQueueSize, syncQueueSize); + ROS_WARN("Parameter \"queue_size\" has been renamed " + "to \"sync_queue_size\" and will be removed " + "in future versions! The value (%d) is copied to " + "\"sync_queue_size\".", syncQueueSize); + } + else + { + pnh.param("sync_queue_size", syncQueueSize, syncQueueSize); + } pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_); @@ -115,7 +128,8 @@ class PointCloudToDepthImage : public nodelet::Nodelet ROS_INFO("Params:"); ROS_INFO(" approx=%s", approx?"true":"false"); - ROS_INFO(" queue_size=%d", queueSize); + ROS_INFO(" topic_queue_size=%d", queueSize); + ROS_INFO(" sync_queue_size=%d", syncQueueSize); ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str()); ROS_INFO(" wait_for_transform=%fs", waitForTransform_); ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); @@ -133,18 +147,18 @@ class PointCloudToDepthImage : public nodelet::Nodelet if(approx) { - approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + approxSync_ = new message_filters::Synchronizer(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_); approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { fixedFrameId_.clear(); - exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_); + exactSync_ = new message_filters::Synchronizer(MyExactSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_); exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2)); } - pointCloudSub_.subscribe(nh, "cloud", 1); - cameraInfoSub_.subscribe(nh, "camera_info", 1); + pointCloudSub_.subscribe(nh, "cloud", queueSize); + cameraInfoSub_.subscribe(nh, "camera_info", queueSize); } void callback( diff --git a/rtabmap_util/src/nodelets/rgbd_relay.cpp b/rtabmap_util/src/nodelets/rgbd_relay.cpp index fd17015f4..6f6a3e17d 100644 --- a/rtabmap_util/src/nodelets/rgbd_relay.cpp +++ b/rtabmap_util/src/nodelets/rgbd_relay.cpp @@ -73,14 +73,9 @@ class RGBDRelay : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; - bool approxSync = true; - pnh.param("queue_size", queueSize, queueSize); pnh.param("compress", compress_, compress_); pnh.param("uncompress", uncompress_, uncompress_); - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); - rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this); rgbdImagePub_ = nh.advertise(nh.resolveName("rgbd_image") + "_relay", 1); } diff --git a/rtabmap_util/src/nodelets/rgbd_split.cpp b/rtabmap_util/src/nodelets/rgbd_split.cpp index 4680a3d8d..ff26c3d4a 100644 --- a/rtabmap_util/src/nodelets/rgbd_split.cpp +++ b/rtabmap_util/src/nodelets/rgbd_split.cpp @@ -71,11 +71,6 @@ class RGBDSplit : public nodelet::Nodelet ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle(); - int queueSize = 10; - pnh.param("queue_size", queueSize, queueSize); - - NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize); - ros::NodeHandle rgb_nh(nh, nh.resolveName("rgbd_image") + "/rgb"); ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth"); image_transport::ImageTransport rgb_it(rgb_nh); diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index 9e1a75de1..d5746164c 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -162,27 +162,27 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : if(subscribeInfoOnly) { ROS_INFO("subscribe_info_only=true"); - infoOnlyTopic_ = nh.subscribe("info", 1, &GuiWrapper::infoCallback, this); + infoOnlyTopic_ = nh.subscribe("info", this->getTopicQueueSize(), &GuiWrapper::infoCallback, this); } else { - infoTopic_.subscribe(nh, "info", 1); - mapDataTopic_.subscribe(nh, "mapData", 1); + infoTopic_.subscribe(nh, "info", this->getTopicQueueSize()); + mapDataTopic_.subscribe(nh, "mapData", this->getTopicQueueSize()); infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(this->getQueueSize()), + MyInfoMapSyncPolicy(this->getSyncQueueSize()), infoTopic_, mapDataTopic_); infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2)); } - goalTopic_.subscribe(nh, "goal_node", 1); - pathTopic_.subscribe(nh, "global_path", 1); + goalTopic_.subscribe(nh, "goal_node", this->getTopicQueueSize()); + pathTopic_.subscribe(nh, "global_path", this->getTopicQueueSize()); goalPathSync_ = new message_filters::Synchronizer( - MyGoalPathSyncPolicy(this->getQueueSize()), + MyGoalPathSyncPolicy(this->getSyncQueueSize()), goalTopic_, pathTopic_); goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, boost::placeholders::_1, boost::placeholders::_2)); - goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this); + goalReachedTopic_ = nh.subscribe("goal_reached", this->getTopicQueueSize(), &GuiWrapper::goalReachedCallback, this); setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end }