i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-16 08:10:19 +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_launch/launch/rtabmap.launch
+17 -8
View File
@@ -47,7 +47,9 @@
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) --> <arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/> <arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/> <arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/> <arg name="topic_queue_size" default="1"/>
<arg name="queue_size" default="10"/> <!-- for backward compatibility, use sync_queue_size directly-->
<arg name="sync_queue_size" default="$(arg queue_size)"/>
<arg name="wait_for_transform" default="0.2"/> <arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug --> <arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument --> <arg name="rtabmap_args" default="$(arg args)"/> <!-- deprecated, use "args" argument -->
@@ -181,7 +183,8 @@
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/> <remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/> <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param name="depth_scale" type="double" value="$(arg rgbd_depth_scale)"/> <param name="depth_scale" type="double" value="$(arg rgbd_depth_scale)"/>
<param name="decimation" type="double" value="$(arg rgbd_decimation)"/> <param name="decimation" type="double" value="$(arg rgbd_decimation)"/>
</node> </node>
@@ -203,7 +206,8 @@
<remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/> <remap from="rgbd_image" to="$(arg rgbd_topic_relay)"/>
<param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_rgbd_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/> <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
</node> </node>
</group> </group>
</group> </group>
@@ -253,7 +257,8 @@
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/> <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/> <param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
@@ -283,7 +288,8 @@
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/> <param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/> <param name="approx_sync_max_interval" type="double" value="$(arg approx_sync_max_interval)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/> <param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
@@ -310,7 +316,8 @@
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/> <param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/> <param name="guess_frame_id" type="string" value="$(arg odom_guess_frame_id)"/>
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/> <param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/> <param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
@@ -377,7 +384,8 @@
<param name="database_path" type="string" value="$(arg database_path)"/> <param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/> <param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<param name="config_path" type="string" value="$(arg cfg)"/> <param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/> <param if="$(eval not scan_cloud_filtered and not scan_cloud_assembling)" name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/> <param name="landmark_linear_variance" type="double" value="$(arg tag_linear_variance)"/>
<param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/> <param name="landmark_angular_variance" type="double" value="$(arg tag_angular_variance)"/>
@@ -434,7 +442,8 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/> <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/> <param name="topic_queue_size" type="int" value="$(arg topic_queue_size)"/>
<param name="sync_queue_size" type="int" value="$(arg sync_queue_size)"/>
<param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/> <param name="approx_sync" type="bool" value="$(eval approx_sync and not use_odom_features)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/> <remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+54 -39
View File
@@ -74,7 +74,8 @@ public:
exactSync5_(0), exactSync5_(0),
approxSync6_(0), approxSync6_(0),
exactSync6_(0), exactSync6_(0),
queueSize_(5), topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false) keepColor_(false)
{ {
} }
@@ -110,7 +111,19 @@ private:
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_); pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is still copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
else
{
pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
}
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
if(pnh.hasParam("depth_cameras")) if(pnh.hasParam("depth_cameras"))
{ {
@@ -126,7 +139,8 @@ private:
NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); NODELET_INFO("RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_); NODELET_INFO("RGBDOdometry: topic_queue_size = %d", topicQueueSize_);
NODELET_INFO("RGBDOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras); NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false"); NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -137,23 +151,23 @@ private:
{ {
if(rgbdCameras >= 2) if(rgbdCameras >= 2)
{ {
rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_);
rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_);
if(rgbdCameras >= 3) if(rgbdCameras >= 3)
{ {
rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_);
} }
if(rgbdCameras >= 4) if(rgbdCameras >= 4)
{ {
rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_);
} }
if(rgbdCameras >= 5) if(rgbdCameras >= 5)
{ {
rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_);
} }
if(rgbdCameras >= 6) if(rgbdCameras >= 6)
{ {
rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_);
} }
if(rgbdCameras == 2) if(rgbdCameras == 2)
@@ -161,7 +175,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>( approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_), MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0) if(approxSyncMaxInterval > 0.0)
@@ -171,7 +185,7 @@ private:
else else
{ {
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>( exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_), MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -188,7 +202,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>( approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_), MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -199,7 +213,7 @@ private:
else else
{ {
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>( exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_), MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -218,7 +232,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>( approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_), MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -230,7 +244,7 @@ private:
else else
{ {
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>( exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_), MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -251,7 +265,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>( approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_), MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -264,7 +278,7 @@ private:
else else
{ {
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>( exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_), MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -287,7 +301,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>( approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_), MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -301,7 +315,7 @@ private:
else else
{ {
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>( exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_), MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -333,7 +347,7 @@ private:
} }
else if(rgbdCameras == 0) else if(rgbdCameras == 0)
{ {
rgbdxSub_ = nh.subscribe("rgbd_images", 1, &RGBDOdometry::callbackRGBDX, this); rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &RGBDOdometry::callbackRGBDX, this);
subscribedTopicsMsg = subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
@@ -342,7 +356,7 @@ private:
} }
else else
{ {
rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this); rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &RGBDOdometry::callbackRGBD, this);
subscribedTopicsMsg = subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
@@ -361,20 +375,20 @@ private:
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1); info_sub_.subscribe(rgb_nh, "camera_info", topicQueueSize_);
if(approxSync) if(approxSync)
{ {
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0) if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
} }
else else
{ {
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
} }
@@ -758,20 +772,20 @@ protected:
if(approxSync_) if(approxSync_)
{ {
delete approxSync_; delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
} }
if(exactSync_) if(exactSync_)
{ {
delete exactSync_; delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
} }
if(approxSync2_) if(approxSync2_)
{ {
delete approxSync2_; delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>( approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_), MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -780,7 +794,7 @@ protected:
{ {
delete exactSync2_; delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>( exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_), MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -789,7 +803,7 @@ protected:
{ {
delete approxSync3_; delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>( approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_), MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -799,7 +813,7 @@ protected:
{ {
delete exactSync3_; delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>( exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_), MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -809,7 +823,7 @@ protected:
{ {
delete approxSync4_; delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>( approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_), MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -820,7 +834,7 @@ protected:
{ {
delete exactSync4_; delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>( exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_), MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -831,7 +845,7 @@ protected:
{ {
delete approxSync5_; delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>( approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_), MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -843,7 +857,7 @@ protected:
{ {
delete exactSync5_; delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>( exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_), MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -855,7 +869,7 @@ protected:
{ {
delete approxSync6_; delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>( approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_), MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -868,7 +882,7 @@ protected:
{ {
delete exactSync6_; delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>( exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_), MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -917,7 +931,8 @@ private:
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_; message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy; typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_; message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_; int topicQueueSize_;
int syncQueueSize_;
bool keepColor_; bool keepColor_;
}; };
rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
+31 -16
View File
@@ -72,7 +72,8 @@ public:
exactScanSync_(0), exactScanSync_(0),
approxCloudSync_(0), approxCloudSync_(0),
exactCloudSync_(0), exactCloudSync_(0),
queueSize_(5), queueSize_(1),
syncQueueSize_(5),
keepColor_(false), keepColor_(false),
scanCloudMaxPoints_(0), scanCloudMaxPoints_(0),
scanVoxelSize_(0.0), scanVoxelSize_(0.0),
@@ -113,7 +114,19 @@ private:
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_); pnh.param("topic_queue_size", queueSize_, queueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is still copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
else
{
pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
}
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud); pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_); pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_); pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
@@ -130,7 +143,8 @@ private:
NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false"); NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_); NODELET_INFO("RGBDIcpOdometry: topic_queue_size = %d", queueSize_);
NODELET_INFO("RGBDIcpOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false"); NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_); NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_); NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
@@ -147,24 +161,24 @@ private:
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize_, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize_, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1); info_sub_.subscribe(rgb_nh, "camera_info", queueSize_);
std::string subscribedTopicsMsg; std::string subscribedTopicsMsg;
if(subscribeScanCloud) if(subscribeScanCloud)
{ {
cloud_sub_.subscribe(nh, "scan_cloud", 1); cloud_sub_.subscribe(nh, "scan_cloud", queueSize_);
if(approxSync) if(approxSync)
{ {
approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
if(approxSyncMaxInterval > 0.0) if(approxSyncMaxInterval > 0.0)
approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
else else
{ {
exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
@@ -179,17 +193,17 @@ private:
} }
else else
{ {
scan_sub_.subscribe(nh, "scan", 1); scan_sub_.subscribe(nh, "scan", queueSize_);
if(approxSync) if(approxSync)
{ {
approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
if(approxSyncMaxInterval > 0.0) if(approxSyncMaxInterval > 0.0)
approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
else else
{ {
exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
@@ -460,25 +474,25 @@ protected:
if(approxScanSync_) if(approxScanSync_)
{ {
delete approxScanSync_; delete approxScanSync_;
approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(exactScanSync_) if(exactScanSync_)
{ {
delete exactScanSync_; delete exactScanSync_;
exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_); exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(approxCloudSync_) if(approxCloudSync_)
{ {
delete approxCloudSync_; delete approxCloudSync_;
approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(exactCloudSync_) if(exactCloudSync_)
{ {
delete exactCloudSync_; delete exactCloudSync_;
exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_); exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(syncQueueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
} }
@@ -498,6 +512,7 @@ private:
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_; message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
int queueSize_; int queueSize_;
int syncQueueSize_;
bool keepColor_; bool keepColor_;
int scanCloudMaxPoints_; int scanCloudMaxPoints_;
double scanVoxelSize_; double scanVoxelSize_;
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+55 -40
View File
@@ -74,7 +74,8 @@ public:
exactSync5_(0), exactSync5_(0),
approxSync6_(0), approxSync6_(0),
exactSync6_(0), exactSync6_(0),
queueSize_(5), topicQueueSize_(1),
syncQueueSize_(5),
keepColor_(false) keepColor_(false)
{ {
} }
@@ -109,7 +110,19 @@ private:
int rgbdCameras = 1; int rgbdCameras = 1;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_); pnh.param("topic_queue_size", topicQueueSize_, topicQueueSize_);
if(pnh.hasParam("queue_size") && !pnh.hasParam("sync_queue_size"))
{
pnh.param("queue_size", syncQueueSize_, syncQueueSize_);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"sync_queue_size\" and will be removed "
"in future versions! The value (%d) is still copied to "
"\"sync_queue_size\".", syncQueueSize_);
}
else
{
pnh.param("sync_queue_size", syncQueueSize_, syncQueueSize_);
}
pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD); pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras); pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
pnh.param("keep_color", keepColor_, keepColor_); pnh.param("keep_color", keepColor_, keepColor_);
@@ -117,7 +130,8 @@ private:
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false"); NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval); NODELET_INFO("StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_); NODELET_INFO("StereoOdometry: topic_queue_size = %d", topicQueueSize_);
NODELET_INFO("StereoOdometry: sync_queue_size = %d", syncQueueSize_);
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false"); NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false"); NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
@@ -127,23 +141,23 @@ private:
{ {
if(rgbdCameras >= 2) if(rgbdCameras >= 2)
{ {
rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1); rgbd_image1_sub_.subscribe(nh, "rgbd_image0", topicQueueSize_);
rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1); rgbd_image2_sub_.subscribe(nh, "rgbd_image1", topicQueueSize_);
if(rgbdCameras >= 3) if(rgbdCameras >= 3)
{ {
rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1); rgbd_image3_sub_.subscribe(nh, "rgbd_image2", topicQueueSize_);
} }
if(rgbdCameras >= 4) if(rgbdCameras >= 4)
{ {
rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1); rgbd_image4_sub_.subscribe(nh, "rgbd_image3", topicQueueSize_);
} }
if(rgbdCameras >= 5) if(rgbdCameras >= 5)
{ {
rgbd_image5_sub_.subscribe(nh, "rgbd_image4", 1); rgbd_image5_sub_.subscribe(nh, "rgbd_image4", topicQueueSize_);
} }
if(rgbdCameras >= 6) if(rgbdCameras >= 6)
{ {
rgbd_image6_sub_.subscribe(nh, "rgbd_image5", 1); rgbd_image6_sub_.subscribe(nh, "rgbd_image5", topicQueueSize_);
} }
if(rgbdCameras == 2) if(rgbdCameras == 2)
@@ -151,7 +165,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>( approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_), MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0) if(approxSyncMaxInterval > 0.0)
@@ -161,7 +175,7 @@ private:
else else
{ {
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>( exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_), MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -178,7 +192,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>( approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_), MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -189,7 +203,7 @@ private:
else else
{ {
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>( exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_), MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -208,7 +222,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>( approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_), MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -220,7 +234,7 @@ private:
else else
{ {
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>( exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_), MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -241,7 +255,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>( approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_), MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -254,7 +268,7 @@ private:
else else
{ {
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>( exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_), MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -277,7 +291,7 @@ private:
if(approxSync) if(approxSync)
{ {
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>( approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_), MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -291,7 +305,7 @@ private:
else else
{ {
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>( exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_), MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -323,7 +337,7 @@ private:
} }
else if(rgbdCameras == 0) else if(rgbdCameras == 0)
{ {
rgbdxSub_ = nh.subscribe("rgbd_images", 1, &StereoOdometry::callbackRGBDX, this); rgbdxSub_ = nh.subscribe("rgbd_images", topicQueueSize_, &StereoOdometry::callbackRGBDX, this);
subscribedTopicsMsg = subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
@@ -332,7 +346,7 @@ private:
} }
else else
{ {
rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this); rgbdSub_ = nh.subscribe("rgbd_image", topicQueueSize_, &StereoOdometry::callbackRGBD, this);
subscribedTopicsMsg = subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
@@ -351,21 +365,21 @@ private:
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft); imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), topicQueueSize_, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight); imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), topicQueueSize_, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1); cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_);
if(approxSync) if(approxSync)
{ {
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0) if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
else else
{ {
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
@@ -897,20 +911,20 @@ protected:
if(approxSync_) if(approxSync_)
{ {
delete approxSync_; delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(exactSync_) if(exactSync_)
{ {
delete exactSync_; delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
if(approxSync2_) if(approxSync2_)
{ {
delete approxSync2_; delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>( approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_), MyApproxSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); approxSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -919,7 +933,7 @@ protected:
{ {
delete exactSync2_; delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>( exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_), MyExactSync2Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_); rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2)); exactSync2_->registerCallback(boost::bind(&StereoOdometry::callbackRGBD2, this, boost::placeholders::_1, boost::placeholders::_2));
@@ -928,7 +942,7 @@ protected:
{ {
delete approxSync3_; delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>( approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_), MyApproxSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -938,7 +952,7 @@ protected:
{ {
delete exactSync3_; delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>( exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_), MyExactSync3Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_); rgbd_image3_sub_);
@@ -948,7 +962,7 @@ protected:
{ {
delete approxSync4_; delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>( approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_), MyApproxSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -959,7 +973,7 @@ protected:
{ {
delete exactSync4_; delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>( exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_), MyExactSync4Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -970,7 +984,7 @@ protected:
{ {
delete approxSync5_; delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>( approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_), MyApproxSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -982,7 +996,7 @@ protected:
{ {
delete exactSync5_; delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>( exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_), MyExactSync5Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -994,7 +1008,7 @@ protected:
{ {
delete approxSync6_; delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>( approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_), MyApproxSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -1007,7 +1021,7 @@ protected:
{ {
delete exactSync6_; delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>( exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_), MyExactSync6Policy(syncQueueSize_),
rgbd_image1_sub_, rgbd_image1_sub_,
rgbd_image2_sub_, rgbd_image2_sub_,
rgbd_image3_sub_, rgbd_image3_sub_,
@@ -1058,7 +1072,8 @@ private:
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy; typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage, rtabmap_msgs::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_; message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_; int topicQueueSize_;
int syncQueueSize_;
bool keepColor_; bool keepColor_;
}; };
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+17 -41
View File
@@ -74,7 +74,8 @@ public:
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;} bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();} bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;} int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;} int getTopicQueueSize() const {return topicQueueSize_;}
int getSyncQueueSize() const {return syncQueueSize_;}
bool isApproxSync() const {return approxSync_;} bool isApproxSync() const {return approxSync_;}
const std::string & name() const {return name_;} const std::string & name() const {return name_;}
@@ -140,16 +141,12 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupStereoCallbacks( void setupStereoCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeOdom, bool subscribeOdom,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBCallbacks( void setupRGBCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -158,9 +155,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBDCallbacks( void setupRGBDCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -169,9 +164,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBDXCallbacks( void setupRGBDXCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -180,9 +173,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
#ifdef RTABMAP_SYNC_MULTI_RGBD #ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks( void setupRGBD2Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
@@ -192,9 +183,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBD3Callbacks( void setupRGBD3Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -203,9 +192,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBD4Callbacks( void setupRGBD4Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -214,9 +201,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBD5Callbacks( void setupRGBD5Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -225,9 +210,7 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupRGBD6Callbacks( void setupRGBD6Callbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -236,17 +219,13 @@ private:
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
#endif #endif
void setupSensorDataCallbacks( void setupSensorDataCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeOdom, bool subscribeOdom,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupScanCallbacks( void setupScanCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
@@ -254,20 +233,17 @@ private:
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdom, bool subscribeOdom,
bool subscribeUserData, bool subscribeUserData,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
void setupOdomCallbacks( void setupOdomCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeUserData, bool subscribeUserData,
bool subscribeOdomInfo, bool subscribeOdomInfo);
int queueSize,
bool approxSync);
protected: protected:
std::string subscribedTopicsMsg_; std::string subscribedTopicsMsg_;
int queueSize_; int topicQueueSize_;
int syncQueueSize_;
private: private:
bool approxSync_; bool approxSync_;
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriberDefines.h
+1 -1
View File
@@ -180,7 +180,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
} \ } \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \ subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s", \
name_.c_str(), \ name_.c_str(), \
approxSync?"approx":"exact", \ APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \ SUB0.getTopic().c_str(), \
SUB1.getTopic().c_str(), \ SUB1.getTopic().c_str(), \
SUB2.getTopic().c_str(), \ SUB2.getTopic().c_str(), \
rtabmap_sync/src/CommonDataSubscriber.cpp
+33 -45
View File
@@ -30,7 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync { namespace rtabmap_sync {
CommonDataSubscriber::CommonDataSubscriber(bool gui) : CommonDataSubscriber::CommonDataSubscriber(bool gui) :
queueSize_(10), topicQueueSize_(10),
syncQueueSize_(10),
approxSync_(true), approxSync_(true),
subscribedToDepth_(!gui), subscribedToDepth_(!gui),
subscribedToStereo_(false), 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."); 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")) if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
{ {
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed " 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."); "corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync_, approxSync_); 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 = %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_cloud = %s", name.c_str(), subscribeScan3d?"true":"false");
ROS_INFO("%s: subscribe_scan_descriptor = %s", name.c_str(), subscribeScanDesc?"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: topic_queue_size = %d", name.c_str(), topicQueueSize_);
ROS_INFO("%s: approx_sync = %s", name.c_str(), approxSync_?"true":"false"); 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; subscribedToOdom_ = odomFrameId.empty() && subscribeOdom;
if(subscribedToDepth_) if(subscribedToDepth_)
@@ -541,9 +555,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(subscribedToStereo_) else if(subscribedToStereo_)
{ {
@@ -551,9 +563,7 @@ void CommonDataSubscriber::setupCallbacks(
nh, nh,
pnh, pnh,
subscribedToOdom_, subscribedToOdom_,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(subscribedToRGB_) else if(subscribedToRGB_)
{ {
@@ -565,9 +575,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(subscribedToRGBD_) else if(subscribedToRGBD_)
{ {
@@ -589,9 +597,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(rgbdCameras == 5) else if(rgbdCameras == 5)
{ {
@@ -603,9 +609,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(rgbdCameras == 4) else if(rgbdCameras == 4)
{ {
@@ -617,9 +621,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(rgbdCameras == 3) else if(rgbdCameras == 3)
{ {
@@ -631,9 +633,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(rgbdCameras == 2) else if(rgbdCameras == 2)
{ {
@@ -645,9 +645,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
#else #else
if(rgbdCameras>1) if(rgbdCameras>1)
@@ -668,9 +666,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else else
{ {
@@ -682,9 +678,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScan2d, subscribeScan2d,
subscribeScan3d, subscribeScan3d,
subscribeScanDesc, subscribeScanDesc,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
} }
else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc) else if(subscribeScan2d || subscribeScan3d || subscribeScanDesc)
@@ -696,9 +690,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribeScanDesc, subscribeScanDesc,
subscribedToOdom_, subscribedToOdom_,
subscribeUserData, subscribeUserData,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(subscribedToSensorData_) else if(subscribedToSensorData_)
{ {
@@ -706,9 +698,7 @@ void CommonDataSubscriber::setupCallbacks(
nh, nh,
pnh, pnh,
subscribedToOdom_, subscribedToOdom_,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
else if(subscribedToOdom_) else if(subscribedToOdom_)
{ {
@@ -716,9 +706,7 @@ void CommonDataSubscriber::setupCallbacks(
nh, nh,
pnh, pnh,
subscribeUserData, subscribeUserData,
subscribeOdomInfo, subscribeOdomInfo);
queueSize_,
approxSync_);
} }
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_) 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", "the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
name_.c_str(), name_.c_str(),
approxSync_? 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.", "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()), subscribedTopicsMsg_.c_str()),
otherTasks); otherTasks);
rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp
+68 -70
View File
@@ -463,9 +463,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup depth callback"); ROS_INFO("Setup depth callback");
@@ -480,195 +478,195 @@ void CommonDataSubscriber::setupDepthCallbacks(
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), topicQueueSize_, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), topicQueueSize_, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
else else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
} }
else 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 #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
#endif #endif
@@ -677,57 +675,57 @@ void CommonDataSubscriber::setupDepthCallbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_); SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
} }
} }
else if(subscribeScan2d) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_); SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
} }
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_); SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
} }
} }
else if(subscribeOdomInfo) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_); SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_);
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp
+9 -11
View File
@@ -67,29 +67,27 @@ void CommonDataSubscriber::setupOdomCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeUserData, bool subscribeUserData,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup scan callback"); ROS_INFO("Setup scan callback");
if(subscribeUserData || subscribeOdomInfo) if(subscribeUserData || subscribeOdomInfo)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData) if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_); SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_);
} }
} }
else else
@@ -97,13 +95,13 @@ void CommonDataSubscriber::setupOdomCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_);
} }
} }
else else
{ {
odomSubOnly_ = nh.subscribe("odom", queueSize, &CommonDataSubscriber::odomCallback, this); odomSubOnly_ = nh.subscribe("odom", syncQueueSize_, &CommonDataSubscriber::odomCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp
+67 -69
View File
@@ -463,9 +463,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgb-only callback"); ROS_INFO("Setup rgb-only callback");
@@ -475,194 +473,194 @@ void CommonDataSubscriber::setupRGBCallbacks(
image_transport::ImageTransport rgb_it(rgb_nh); image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), syncQueueSize_, hintsRgb);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize); cameraInfoSub_.subscribe(rgb_nh, "camera_info", topicQueueSize_);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
} }
} }
else else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_); SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
} }
} }
else if(subscribeScan2d) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_); SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
} }
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_); SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
} }
} }
else if(subscribeOdomInfo) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_);
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_); SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
} }
} }
else if(subscribeScan2d) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_); SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
} }
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_); SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
} }
} }
else if(subscribeOdomInfo) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_);
} }
} }
#endif #endif
@@ -671,57 +669,57 @@ void CommonDataSubscriber::setupRGBCallbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_); SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_);
} }
} }
else if(subscribeScan2d) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_); SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_);
} }
} }
else if(subscribeScan3d) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_); SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_);
} }
} }
else if(subscribeOdomInfo) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_); SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_);
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp
+42 -44
View File
@@ -539,9 +539,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd callback"); ROS_INFO("Setup rgbd callback");
@@ -556,152 +554,152 @@ void CommonDataSubscriber::setupRGBDCallbacks(
{ {
rgbdSubs_.resize(1); rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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 #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0])); SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]));
} }
} }
else else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0])); SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]));
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0])); SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]));
} }
} }
#endif #endif
@@ -710,41 +708,41 @@ void CommonDataSubscriber::setupRGBDCallbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
} }
else else
{ {
@@ -754,7 +752,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
} }
else else
{ {
rgbdSub_ = nh.subscribe("rgbd_image", queueSize, &CommonDataSubscriber::rgbdCallback, this); rgbdSub_ = nh.subscribe("rgbd_image", syncQueueSize_, &CommonDataSubscriber::rgbdCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp
+42 -44
View File
@@ -350,9 +350,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd2 callback"); ROS_INFO("Setup rgbd2 callback");
@@ -360,152 +358,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
for(int i=0; i<2; ++i) for(int i=0; i<2; ++i)
{ {
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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 #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
} }
else 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 else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
} }
else 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 #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1])); SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
} }
} }
#endif #endif
@@ -514,45 +512,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1])); SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp
+42 -44
View File
@@ -438,9 +438,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDescriptor, bool subscribeScanDescriptor,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd3 callback"); ROS_INFO("Setup rgbd3 callback");
@@ -448,151 +446,151 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
for(int i=0; i<3; ++i) for(int i=0; i<3; ++i)
{ {
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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 #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDescriptor) if(subscribeScanDescriptor)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
} }
else 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 else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDescriptor) if(subscribeScanDescriptor)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
} }
else 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 #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDescriptor) if(subscribeScanDescriptor)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
} }
else 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 #endif
@@ -601,45 +599,45 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
if(subscribeScanDescriptor) if(subscribeScanDescriptor)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
} }
else 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]));
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp
+42 -44
View File
@@ -407,9 +407,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd4 callback"); ROS_INFO("Setup rgbd4 callback");
@@ -417,152 +415,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
for(int i=0; i<4; ++i) for(int i=0; i<4; ++i)
{ {
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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 #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
} }
else 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 else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
} }
else 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 #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
} }
else 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 #endif
@@ -571,45 +569,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
} }
else 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]));
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp
+21 -23
View File
@@ -263,9 +263,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd5 callback"); ROS_INFO("Setup rgbd5 callback");
@@ -273,53 +271,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
for(int i=0; i<5; ++i) for(int i=0; i<5; ++i)
{ {
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
} }
else 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 else
@@ -327,45 +325,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
} }
else 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]));
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp
+21 -23
View File
@@ -281,9 +281,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbd6 callback"); ROS_INFO("Setup rgbd6 callback");
@@ -291,53 +289,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
for(int i=0; i<6; ++i) for(int i=0; i<6; ++i)
{ {
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::RGBDImage>; 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) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
} }
else 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 else
@@ -345,45 +343,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
} }
else 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]));
} }
} }
} }
rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp
+42 -44
View File
@@ -326,157 +326,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
bool subscribeScan2d, bool subscribeScan2d,
bool subscribeScan3d, bool subscribeScan3d,
bool subscribeScanDesc, bool subscribeScanDesc,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup rgbdX callback"); ROS_INFO("Setup rgbdX callback");
rgbdXSub_.subscribe(nh, "rgbd_images", queueSize); rgbdXSub_.subscribe(nh, "rgbd_images", topicQueueSize_);
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_); SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_);
} }
} }
else else
#endif #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_); SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_);
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_); SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_);
} }
} }
#endif #endif
@@ -485,46 +483,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
if(subscribeScanDesc) if(subscribeScanDesc)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan2d)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeScan3d)
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = false; subscribedToOdomInfo_ = false;
ROS_WARN("subscribe_odom_info ignored..."); 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_);
} }
else else
{ {
rgbdXSub_.unsubscribe(); rgbdXSub_.unsubscribe();
rgbdXSubOnly_ = nh.subscribe("rgbd_images", queueSize, &CommonDataSubscriber::rgbdXCallback, this); rgbdXSubOnly_ = nh.subscribe("rgbd_images", syncQueueSize_, &CommonDataSubscriber::rgbdXCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp
+42 -44
View File
@@ -250,9 +250,7 @@ void CommonDataSubscriber::setupScanCallbacks(
bool scanDescTopic, bool scanDescTopic,
bool subscribeOdom, bool subscribeOdom,
bool subscribeUserData, bool subscribeUserData,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup scan callback"); ROS_INFO("Setup scan callback");
@@ -261,36 +259,36 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic) if(scanDescTopic)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize); scanDescSub_.subscribe(nh, "scan_descriptor", topicQueueSize_);
} }
else if(scan2dTopic) else if(scan2dTopic)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scanSub_.subscribe(nh, "scan", queueSize); scanSub_.subscribe(nh, "scan", topicQueueSize_);
} }
else else
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSub_.subscribe(nh, "scan_cloud", queueSize); scan3dSub_.subscribe(nh, "scan_cloud", topicQueueSize_);
} }
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData) if(subscribeOdom && subscribeUserData)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(scanDescTopic) if(scanDescTopic)
{ {
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_); SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_);
} }
} }
else if(scan2dTopic) else if(scan2dTopic)
@@ -298,12 +296,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_); SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_);
} }
} }
else else
@@ -311,12 +309,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
} }
else 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 #endif
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(scanDescTopic) if(scanDescTopic)
{ {
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_); SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_);
} }
} }
else if(scan2dTopic) else if(scan2dTopic)
@@ -344,12 +342,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_); SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_);
} }
} }
else else
@@ -357,31 +355,31 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_); SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_);
} }
} }
} }
#ifdef RTABMAP_SYNC_USER_DATA #ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData) else if(subscribeUserData)
{ {
userDataSub_.subscribe(nh, "user_data", queueSize); userDataSub_.subscribe(nh, "user_data", topicQueueSize_);
if(scanDescTopic) if(scanDescTopic)
{ {
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_); SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_);
} }
} }
else if(scan2dTopic) else if(scan2dTopic)
@@ -389,12 +387,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_); SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_);
} }
} }
else else
@@ -402,12 +400,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_);
} }
else 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) else if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
if(scanDescTopic) if(scanDescTopic)
{ {
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_);
} }
else if(scan2dTopic) else if(scan2dTopic)
{ {
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_);
} }
else 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) if(scanDescTopic)
{ {
subscribedToScanDescriptor_ = true; subscribedToScanDescriptor_ = true;
scanDescSubOnly_ = nh.subscribe("scan_descriptor", queueSize, &CommonDataSubscriber::scanDescCallback, this); scanDescSubOnly_ = nh.subscribe("scan_descriptor", syncQueueSize_, &CommonDataSubscriber::scanDescCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
@@ -444,7 +442,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(scan2dTopic) else if(scan2dTopic)
{ {
subscribedToScan2d_ = true; subscribedToScan2d_ = true;
scan2dSubOnly_ = nh.subscribe("scan", queueSize, &CommonDataSubscriber::scan2dCallback, this); scan2dSubOnly_ = nh.subscribe("scan", syncQueueSize_, &CommonDataSubscriber::scan2dCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
@@ -453,7 +451,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else else
{ {
subscribedToScan3d_ = true; subscribedToScan3d_ = true;
scan3dSubOnly_ = nh.subscribe("scan_cloud", queueSize, &CommonDataSubscriber::scan3dCallback, this); scan3dSubOnly_ = nh.subscribe("scan_cloud", syncQueueSize_, &CommonDataSubscriber::scan3dCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(), ros::this_node::getName().c_str(),
rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp
+9 -11
View File
@@ -68,25 +68,23 @@ void CommonDataSubscriber::setupSensorDataCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeOdom, bool subscribeOdom,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup SensorData callback"); ROS_INFO("Setup SensorData callback");
sensorDataSub_.subscribe(nh, "sensor_data", queueSize); sensorDataSub_.subscribe(nh, "sensor_data", topicQueueSize_);
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_); SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_); SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_);
} }
} }
else else
@@ -94,13 +92,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_); SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_);
} }
else else
{ {
sensorDataSub_.unsubscribe(); sensorDataSub_.unsubscribe();
sensorDataSubOnly_ = nh.subscribe("sensor_data", queueSize, &CommonDataSubscriber::sensorDataCallback, this); sensorDataSubOnly_ = nh.subscribe("sensor_data", syncQueueSize_, &CommonDataSubscriber::sensorDataCallback, this);
subscribedTopicsMsg_ = subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s", uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp
+12 -14
View File
@@ -89,9 +89,7 @@ void CommonDataSubscriber::setupStereoCallbacks(
ros::NodeHandle & nh, ros::NodeHandle & nh,
ros::NodeHandle & pnh, ros::NodeHandle & pnh,
bool subscribeOdom, bool subscribeOdom,
bool subscribeOdomInfo, bool subscribeOdomInfo)
int queueSize,
bool approxSync)
{ {
ROS_INFO("Setup stereo callback"); ROS_INFO("Setup stereo callback");
@@ -104,24 +102,24 @@ void CommonDataSubscriber::setupStereoCallbacks(
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh); image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), queueSize, hintsLeft); imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), syncQueueSize_, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), queueSize, hintsRight); imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), syncQueueSize_, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize); cameraInfoLeft_.subscribe(left_nh, "camera_info", topicQueueSize_);
cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize); cameraInfoRight_.subscribe(right_nh, "camera_info", topicQueueSize_);
if(subscribeOdom) if(subscribeOdom)
{ {
odomSub_.subscribe(nh, "odom", queueSize); odomSub_.subscribe(nh, "odom", topicQueueSize_);
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
} }
} }
else else
@@ -129,12 +127,12 @@ void CommonDataSubscriber::setupStereoCallbacks(
if(subscribeOdomInfo) if(subscribeOdomInfo)
{ {
subscribedToOdomInfo_ = true; subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(nh, "odom_info", queueSize); odomInfoSub_.subscribe(nh, "odom_info", topicQueueSize_);
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
} }
else else
{ {
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_); SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
} }
} }
} }
rtabmap_sync/src/nodelets/rgb_sync.cpp
+21 -7
View File
@@ -77,18 +77,32 @@ private:
ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = false; bool approxSync = false;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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_); pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); 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_); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1); rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
@@ -96,14 +110,14 @@ private:
if(approxSync) 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) if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2)); approxSync_->registerCallback(boost::bind(&RgbSync::callback, this, boost::placeholders::_1, boost::placeholders::_2));
} }
else 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)); 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::ImageTransport rgb_it(rgb_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image_rect"), queueSize, hintsRgb);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s", std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
getName().c_str(), 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true; bool approxSync = true;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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("depth_scale", depthScale_, depthScale_);
pnh.param("decimation", decimation_, decimation_); pnh.param("decimation", decimation_, decimation_);
pnh.param("compressed_rate", compressedRate_, compressedRate_); pnh.param("compressed_rate", compressedRate_, compressedRate_);
@@ -99,7 +112,8 @@ private:
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); 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: depth_scale = %f", getName().c_str(), depthScale_);
NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_); NODELET_INFO("%s: decimation = %d", getName().c_str(), decimation_);
NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
@@ -109,14 +123,14 @@ private:
if(approxSync) 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) if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); approxSyncDepth_->registerCallback(boost::bind(&RGBDSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
} }
else 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)); 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 hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s", std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(), 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true; bool approxSync = true;
int rgbdCameras = 2; int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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); pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false");
if(approxSync) if(approxSync)
NODELET_INFO("%s: approx_sync_max_interval = %f", getName().c_str(), approxSyncMaxInterval); 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); NODELET_INFO("%s: rgbd_cameras = %d", getName().c_str(), rgbdCameras);
rgbdImagesPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("rgbd_images", 1); rgbdImagesPub_ = nh.advertise<rtabmap_msgs::RGBDImages>("rgbd_images", 1);
@@ -107,7 +121,7 @@ private:
std::string subscribedTopicsMsg_; std::string subscribedTopicsMsg_;
if(rgbdCameras==2) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd2ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -115,7 +129,7 @@ private:
} }
else if(rgbdCameras==3) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd3ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -123,7 +137,7 @@ private:
} }
else if(rgbdCameras==4) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd4ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -131,7 +145,7 @@ private:
} }
else if(rgbdCameras==5) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd5ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -139,7 +153,7 @@ private:
} }
else if(rgbdCameras==6) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd6ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -147,7 +161,7 @@ private:
} }
else if(rgbdCameras==7) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); rgbd7ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
@@ -155,7 +169,7 @@ private:
} }
else if(rgbdCameras==8) 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) if(approxSync && approxSyncMaxInterval>0.0)
{ {
rgbd8ApproximateSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = false; bool approxSync = false;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
if(approxSync) if(approxSync)
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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_); pnh.param("compressed_rate", compressedRate_, compressedRate_);
NODELET_INFO("%s: approx_sync = %s", getName().c_str(), approxSync?"true":"false"); 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: 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_); NODELET_INFO("%s: compressed_rate = %f", getName().c_str(), compressedRate_);
rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1); rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("rgbd_image", 1);
@@ -96,14 +110,14 @@ private:
if(approxSync) 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) if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxSync_->registerCallback(boost::bind(&StereoSync::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
else 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)); 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 hintsRgb("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), right_pnh);
imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), 1, hintsRgb); imageLeftSub_.subscribe(rgb_it, left_nh.resolveName("image_rect"), queueSize, hintsRgb);
imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), 1, hintsDepth); imageRightSub_.subscribe(depth_it, right_nh.resolveName("image_rect"), queueSize, hintsDepth);
cameraInfoLeftSub_.subscribe(left_nh, "camera_info", 1); cameraInfoLeftSub_.subscribe(left_nh, "camera_info", queueSize);
cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1); 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", std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(), getName().c_str(),
rtabmap_util/src/nodelets/obstacles_detection.cpp
-2
View File
@@ -113,8 +113,6 @@ private:
ULogger::setType(ULogger::kTypeConsole); ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning); ULogger::setLevel(ULogger::kWarning);
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 5; int queueSize = 1;
int syncQueueSize = 5;
int count = 2; int count = 2;
bool approx=true; bool approx=true;
double approxSyncMaxInterval = 0.0; 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("frame_id", frameId_, frameId_);
pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_); pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("approx_sync", approx, approx); pnh.param("approx_sync", approx, approx);
@@ -112,14 +125,14 @@ private:
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("xyz_output", xyzOutput_, xyzOutput_); pnh.param("xyz_output", xyzOutput_, xyzOutput_);
cloudSub_1_.subscribe(nh, "cloud1", 1); cloudSub_1_.subscribe(nh, "cloud1", queueSize);
cloudSub_2_.subscribe(nh, "cloud2", 1); cloudSub_2_.subscribe(nh, "cloud2", queueSize);
std::string subscribedTopicsMsg; std::string subscribedTopicsMsg;
if(count == 4) if(count == 4)
{ {
cloudSub_3_.subscribe(nh, "cloud3", 1); cloudSub_3_.subscribe(nh, "cloud3", queueSize);
cloudSub_4_.subscribe(nh, "cloud4", 1); cloudSub_4_.subscribe(nh, "cloud4", queueSize);
if(approx) if(approx)
{ {
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); 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) else if(count == 3)
{ {
cloudSub_3_.subscribe(nh, "cloud3", 1); cloudSub_3_.subscribe(nh, "cloud3", queueSize);
if(approx) if(approx)
{ {
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 5; int queueSize = 1;
int syncQueueSize = 5;
bool subscribeOdomInfo = false; 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("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("max_clouds", maxClouds_, maxClouds_); pnh.param("max_clouds", maxClouds_, maxClouds_);
@@ -131,7 +144,8 @@ private:
pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo); pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0); 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: 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: frame_id=%s", getName().c_str(), frameId_.c_str());
ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_); ROS_INFO("%s: max_clouds=%d", getName().c_str(), maxClouds_);
@@ -166,10 +180,10 @@ private:
} }
else if(subscribeOdomInfo) else if(subscribeOdomInfo)
{ {
syncCloudSub_.subscribe(nh, "cloud", 1); syncCloudSub_.subscribe(nh, "cloud", queueSize);
syncOdomSub_.subscribe(nh, "odom", 1); syncOdomSub_.subscribe(nh, "odom", queueSize);
syncOdomInfoSub_.subscribe(nh, "odom_info", 1); syncOdomInfoSub_.subscribe(nh, "odom_info", queueSize);
exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_); 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)); 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", subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
getName().c_str(), getName().c_str(),
@@ -181,9 +195,9 @@ private:
} }
else else
{ {
syncCloudSub_.subscribe(nh, "cloud", 1); syncCloudSub_.subscribe(nh, "cloud", queueSize);
syncOdomSub_.subscribe(nh, "odom", 1); syncOdomSub_.subscribe(nh, "odom", queueSize);
exactSync_ = new message_filters::Synchronizer<syncPolicy>(syncPolicy(queueSize), syncCloudSub_, syncOdomSub_); 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)); 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", subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n %s,\n %s",
getName().c_str(), 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true; bool approxSync = true;
std::string roiStr; std::string roiStr;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_); pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
@@ -171,22 +184,22 @@ private:
if(approxSync) 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) if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, boost::placeholders::_1, boost::placeholders::_2)); 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) if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2)); approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, boost::placeholders::_1, boost::placeholders::_2));
} }
else 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)); 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)); 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::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth);
cameraInfoSub_.subscribe(depth_nh, "camera_info", 1); cameraInfoSub_.subscribe(depth_nh, "camera_info", queueSize);
disparitySub_.subscribe(nh, "disparity/image", 1); disparitySub_.subscribe(nh, "disparity/image", queueSize);
disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1); disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", queueSize);
cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1); 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approxSync = true; bool approxSync = true;
std::string roiStr; std::string roiStr;
double approxSyncMaxInterval = 0.0; double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync); pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval); 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("max_depth", maxDepth_, maxDepth_);
pnh.param("min_depth", minDepth_, minDepth_); pnh.param("min_depth", minDepth_, minDepth_);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
@@ -199,30 +212,30 @@ private:
if(approxSync) 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) if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDepth_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZRGB::depthCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); 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) if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncDisparity_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZRGB::disparityCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); 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) if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSyncStereo_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); approxSyncStereo_->registerCallback(boost::bind(&PointCloudXYZRGB::stereoCallback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
} }
else 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)); 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)); 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)); 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 hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), queueSize, hintsRgb);
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), queueSize, hintsDepth);
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1); cameraInfoSub_.subscribe(rgb_nh, "camera_info", queueSize);
ros::NodeHandle left_nh(nh, "left"); ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right"); ros::NodeHandle right_nh(nh, "right");
@@ -248,12 +261,12 @@ private:
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh); image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_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); imageLeft_.subscribe(left_it, left_nh.resolveName("image"), queueSize, hintsLeft);
imageRight_.subscribe(right_it, right_nh.resolveName("image"), 1, hintsRight); imageRight_.subscribe(right_it, right_nh.resolveName("image"), queueSize, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1); cameraInfoLeft_.subscribe(left_nh, "camera_info", queueSize);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1); cameraInfoRight_.subscribe(right_nh, "camera_info", queueSize);
} }
void depthCallback( 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10; int queueSize = 1;
int syncQueueSize = 10;
bool approx = true; 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("fixed_frame_id", fixedFrameId_, fixedFrameId_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_); pnh.param("fill_holes_size", fillHolesSize_, fillHolesSize_);
@@ -115,7 +128,8 @@ private:
ROS_INFO("Params:"); ROS_INFO("Params:");
ROS_INFO(" approx=%s", approx?"true":"false"); 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(" fixed_frame_id=%s", fixedFrameId_.c_str());
ROS_INFO(" wait_for_transform=%fs", waitForTransform_); ROS_INFO(" wait_for_transform=%fs", waitForTransform_);
ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_); ROS_INFO(" fill_holes_size=%d pixels (0=disabled)", fillHolesSize_);
@@ -133,18 +147,18 @@ private:
if(approx) 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)); approxSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
} }
else else
{ {
fixedFrameId_.clear(); 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)); exactSync_->registerCallback(boost::bind(&PointCloudToDepthImage::callback, this, boost::placeholders::_1, boost::placeholders::_2));
} }
pointCloudSub_.subscribe(nh, "cloud", 1); pointCloudSub_.subscribe(nh, "cloud", queueSize);
cameraInfoSub_.subscribe(nh, "camera_info", 1); cameraInfoSub_.subscribe(nh, "camera_info", queueSize);
} }
void callback( void callback(
rtabmap_util/src/nodelets/rgbd_relay.cpp
-5
View File
@@ -73,14 +73,9 @@ private:
ros::NodeHandle & nh = getNodeHandle(); ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 10;
bool approxSync = true;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("compress", compress_, compress_); pnh.param("compress", compress_, compress_);
pnh.param("uncompress", uncompress_, uncompress_); pnh.param("uncompress", uncompress_, uncompress_);
NODELET_INFO("%s: queue_size = %d", getName().c_str(), queueSize);
rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this); rgbdImageSub_ = nh.subscribe("rgbd_image", 1, &RGBDRelay::callback, this);
rgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>(nh.resolveName("rgbd_image") + "_relay", 1); 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 & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle(); 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 rgb_nh(nh, nh.resolveName("rgbd_image") + "/rgb");
ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth"); ros::NodeHandle depth_nh(nh, nh.resolveName("rgbd_image") + "/depth");
image_transport::ImageTransport rgb_it(rgb_nh); image_transport::ImageTransport rgb_it(rgb_nh);
rtabmap_viz/src/GuiWrapper.cpp
+8 -8
View File
@@ -162,27 +162,27 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
if(subscribeInfoOnly) if(subscribeInfoOnly)
{ {
ROS_INFO("subscribe_info_only=true"); ROS_INFO("subscribe_info_only=true");
infoOnlyTopic_ = nh.subscribe("info", 1, &GuiWrapper::infoCallback, this); infoOnlyTopic_ = nh.subscribe("info", this->getTopicQueueSize(), &GuiWrapper::infoCallback, this);
} }
else else
{ {
infoTopic_.subscribe(nh, "info", 1); infoTopic_.subscribe(nh, "info", this->getTopicQueueSize());
mapDataTopic_.subscribe(nh, "mapData", 1); mapDataTopic_.subscribe(nh, "mapData", this->getTopicQueueSize());
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>( infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
MyInfoMapSyncPolicy(this->getQueueSize()), MyInfoMapSyncPolicy(this->getSyncQueueSize()),
infoTopic_, infoTopic_,
mapDataTopic_); mapDataTopic_);
infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2)); infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2));
} }
goalTopic_.subscribe(nh, "goal_node", 1); goalTopic_.subscribe(nh, "goal_node", this->getTopicQueueSize());
pathTopic_.subscribe(nh, "global_path", 1); pathTopic_.subscribe(nh, "global_path", this->getTopicQueueSize());
goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>( goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
MyGoalPathSyncPolicy(this->getQueueSize()), MyGoalPathSyncPolicy(this->getSyncQueueSize()),
goalTopic_, goalTopic_,
pathTopic_); pathTopic_);
goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, boost::placeholders::_1, boost::placeholders::_2)); goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, boost::placeholders::_1, boost::placeholders::_2));
goalReachedTopic_ = nh.subscribe("goal_reached", 1, &GuiWrapper::goalReachedCallback, this); goalReachedTopic_ = nh.subscribe("goal_reached", this->getTopicQueueSize(), &GuiWrapper::goalReachedCallback, this);
setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end setupCallbacks(nh, pnh, ros::this_node::getName()); // do it at the end
} }