i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 02:37:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Split queue_size param into sync_queue_size and topic_queue_size parameters for more fine tuning of topic synchronization (#1054)

Browse Source
This commit is contained in:
matlabbe
2024-06-11 22:35:54 -07:00
parent 9ced85d5f8
commit ae44e1a215
33 changed files with 882 additions and 779 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/src/nodelets/rgb_sync.cpp
+21 -7
View File
@@ -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(),
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+22 -8
View File
@@ -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(),
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+24 -10
View File
@@ -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));
rtabmap_sync/src/nodelets/stereo_sync.cpp
+23 -9
View File
@@ -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(),