i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57: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_util/src/nodelets/obstacles_detection.cpp
-2
View File
@@ -113,8 +113,6 @@ private:
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+20 -7
View File
@@ -99,11 +99,24 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 5;
int queueSize = 1;
int syncQueueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize, syncQueueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
else
{
pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
}
pnh.param("frame_id", frameId_, frameId_);
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("approx_sync", approx, approx);
@@ -112,14 +125,14 @@ private:
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("xyz_output", xyzOutput_, xyzOutput_);
cloudSub_1_.subscribe(nh, "cloud1", 1);
cloudSub_2_.subscribe(nh, "cloud2", 1);
cloudSub_1_.subscribe(nh, "cloud1", queueSize);
cloudSub_2_.subscribe(nh, "cloud2", queueSize);
std::string subscribedTopicsMsg;
if(count == 4)
{
cloudSub_3_.subscribe(nh, "cloud3", 1);
cloudSub_4_.subscribe(nh, "cloud4", 1);
cloudSub_3_.subscribe(nh, "cloud3", queueSize);
cloudSub_4_.subscribe(nh, "cloud4", queueSize);
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
@@ -143,7 +156,7 @@ private:
}
else if(count == 3)
{
cloudSub_3_.subscribe(nh, "cloud3", 1);
cloudSub_3_.subscribe(nh, "cloud3", queueSize);
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+24 -10
View File
@@ -109,10 +109,23 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 5;
int queueSize = 1;
int syncQueueSize = 5;
bool subscribeOdomInfo = false;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize, syncQueueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
else
{
pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
}
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("frame_id", frameId_, frameId_);
pnh.param("max_clouds", maxClouds_, maxClouds_);
@@ -131,7 +144,8 @@ private:
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
ROS_INFO("%s: queue_size=%d", getName().c_str(), queueSize);
ROS_INFO("%s: topic_queue_size=%d", getName().c_str(), queueSize);
ROS_INFO("%s: sync_queue_size=%d", getName().c_str(), syncQueueSize);
ROS_INFO("%s: fixed_frame_id=%s", getName().c_str(), fixedFrameId_.c_str());
ROS_INFO("%s: frame_id=%s", getName().c_str(), frameId_.c_str());
ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_);
@@ -166,10 +180,10 @@ private:
}
else if(subscribeOdomInfo)
{
syncCloudSub_.subscribe(nh, "cloud", 1);
syncOdomSub_.subscribe(nh, "odom", 1);
syncOdomInfoSub_.subscribe(nh, "odom_info", 1);
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
syncCloudSub_.subscribe(nh, "cloud", queueSize);
syncOdomSub_.subscribe(nh, "odom", queueSize);
syncOdomInfoSub_.subscribe(nh, "odom_info", queueSize);
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
exactInfoSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdomInfo, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
getName().c_str(),
@@ -181,9 +195,9 @@ private:
}
else
{
syncCloudSub_.subscribe(nh, "cloud", 1);
syncOdomSub_.subscribe(nh, "odom", 1);
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_);
syncCloudSub_.subscribe(nh, "cloud", queueSize);
syncOdomSub_.subscribe(nh, "odom", queueSize);
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(syncQueueSize), syncCloudSub_, syncOdomSub_);
exactSync_->registerCallback(boost::bind(&rtabmap_util::PointCloudAssembler::callbackCloudOdom, this, boost::placeholders::_1, boost::placeholders::_2));
subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
getName().c_str(),
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+23 -10
View File
@@ -100,13 +100,26 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize, syncQueueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
else
{
pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
}
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
@@ -171,22 +184,22 @@ private:
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), disparitySub_, disparityCameraInfoSub_);
exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2));
}
@@ -195,11 +208,11 @@ private:
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth);
cameraInfoSub_.subscribe(depth_nh, "camera_info", queueSize);
disparitySub_.subscribe(nh, "disparity/image", 1);
disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
disparitySub_.subscribe(nh, "disparity/image", queueSize);
disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", queueSize);
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
}
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+29 -16
View File
@@ -108,13 +108,26 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize, syncQueueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
else
{
pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
}
pnh.param("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_);
@@ -199,30 +212,30 @@ 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(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_ = new message_filters::Synchronizer<MyExactSyncDisparityPolicy>(MyExactSyncDisparityPolicy(syncQueueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_ = new message_filters::Synchronizer<MyExactSyncStereoPolicy>(MyExactSyncStereoPolicy(syncQueueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
exactSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
@@ -235,9 +248,9 @@ 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);
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
@@ -248,12 +261,12 @@ private:
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageDisparitySub_.subscribe(nh, "disparity", 1);
imageDisparitySub_.subscribe(nh, "disparity", queueSize);
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), 1, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
imageLeft_.subscribe(left_it, left_nh.resolveName("image"), queueSize, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), queueSize, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize);
cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize);
}
void depthCallback(
rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+21 -7
View File
@@ -92,9 +92,22 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
int queueSize = 1;
int syncQueueSize = 10;
bool approx = true;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize, syncQueueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is copied to "
"\"sync_queue_size\".", syncQueueSize);
}
else
{
pnh.param("sync_queue_size", syncQueueSize, syncQueueSize);
}
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
@@ -115,7 +128,8 @@ private:
ROS_INFO("Params:");
ROS_INFO(" approx=%s", approx?"true":"false");
ROS_INFO(" queue_size=%d", queueSize);
ROS_INFO(" topic_queue_size=%d", queueSize);
ROS_INFO(" sync_queue_size=%d", syncQueueSize);
ROS_INFO(" fixed_frame_id=%s", fixedFrameId_.c_str());
ROS_INFO(" wait_for_transform=%fs", waitForTransform_);
ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
@@ -133,18 +147,18 @@ private:
if(approx)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
}
else
{
fixedFrameId_.clear();
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), pointCloudSub_, cameraInfoSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), pointCloudSub_, cameraInfoSub_);
exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
}
pointCloudSub_.subscribe(nh, "cloud", 1);
cameraInfoSub_.subscribe(nh, "camera_info", 1);
pointCloudSub_.subscribe(nh, "cloud", queueSize);
cameraInfoSub_.subscribe(nh, "camera_info", queueSize);
}
void callback(
rtabmap_util/src/nodelets/rgbd_relay.cpp
-5
View File
@@ -73,14 +73,9 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compress", compress_, compress_);
pnh.param("uncompress", uncompress_, uncompress_);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this);
rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>(nh.resolveName("rgbd_image") + "_relay", 1);
}
rtabmap_util/src/nodelets/rgbd_split.cpp
-5
View File
@@ -71,11 +71,6 @@ private:
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
ros::NodeHandle rgb_nh(nh, nh.resolveName("rgbd_image") + "/rgb");
ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth");
image_transport::ImageTransport rgb_it(rgb_nh);