mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-08 02:37:45 +08:00
Split queue_size param into sync_queue_size and topic_queue_size parameters for more fine tuning of topic synchronization (#1054)
This commit is contained in:
matlabbe
@@ -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);
|
||||
|
||||
@@ -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_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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(),
|
||||
|
||||
@@ -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_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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",
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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<rtabmap_msgs::RGBDImage>;
|
||||
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]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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(),
|
||||
|
||||
@@ -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",
|
||||
|
||||
@@ -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_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -77,18 +77,32 @@ private:
|
||||
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<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
|
||||
@@ -96,14 +110,14 @@ private:
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(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>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
|
||||
exactSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
|
||||
}
|
||||
|
||||
@@ -112,8 +126,8 @@ private:
|
||||
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(),
|
||||
|
||||
@@ -81,12 +81,25 @@ private:
|
||||
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 @@ private:
|
||||
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 @@ private:
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(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>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
|
||||
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(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 @@ private:
|
||||
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(),
|
||||
|
||||
@@ -77,19 +77,33 @@ private:
|
||||
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<rtabmap_msgs::RGBDImages>("rgbd_images", 1);
|
||||
@@ -107,7 +121,7 @@ private:
|
||||
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 @@ private:
|
||||
}
|
||||
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 @@ private:
|
||||
}
|
||||
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 @@ private:
|
||||
}
|
||||
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 @@ private:
|
||||
}
|
||||
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 @@ private:
|
||||
}
|
||||
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 @@ private:
|
||||
}
|
||||
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));
|
||||
|
||||
@@ -77,18 +77,32 @@ private:
|
||||
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<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
|
||||
@@ -96,14 +110,14 @@ private:
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
|
||||
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(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>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
|
||||
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(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 @@ private:
|
||||
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(),
|
||||
|
||||
Reference in New Issue
Block a user