i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 18:27:46 +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/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(),