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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-06-12 22:51:30 -07:00
parent 911e4e8941 ae44e1a215
commit d3d874c862
35 changed files with 857 additions and 771 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/src/CommonDataSubscriber.cpp
+29 -43
View File
@@ -31,7 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync {
CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
queueSize_(10),
topicQueueSize_(1),
syncQueueSize_(10),
approxSync_(true),
subscribedToDepth_(!gui),
subscribedToStereo_(false),
@@ -378,7 +379,17 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
odomFrameId_ = node.declare_parameter("odom_frame_id", odomFrameId_);
rgbdCameras_ = node.declare_parameter("rgbd_cameras", rgbdCameras_);
queueSize_ = node.declare_parameter("queue_size", queueSize_);
topicQueueSize_ = node.declare_parameter("topic_queue_size", topicQueueSize_);
int queueSize = node.declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize_ = queueSize;
RCLCPP_WARN(node.get_logger(), "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_);
}
syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_);
int qos = node.declare_parameter("qos", 0);
int qosOdom = node.declare_parameter("qos_odom", qos);
@@ -519,7 +530,8 @@ void CommonDataSubscriber::setupCallbacks(
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan = %s", name_.c_str(), subscribedToScan2d_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_cloud = %s", name_.c_str(), subscribedToScan3d_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: subscribe_scan_descriptor = %s", name_.c_str(), subscribedToScanDescriptor_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s: queue_size = %d", name_.c_str(), queueSize_);
RCLCPP_INFO(node.get_logger(), "%s: topic_queue_size = %d", name_.c_str(), topicQueueSize_);
RCLCPP_INFO(node.get_logger(), "%s: sync_queue_size = %d", name_.c_str(), syncQueueSize_);
RCLCPP_INFO(node.get_logger(), "%s: qos_image = %d", name_.c_str(), qosImage_);
RCLCPP_INFO(node.get_logger(), "%s: qos_camera_info = %d", name_.c_str(), qosCameraInfo_);
RCLCPP_INFO(node.get_logger(), "%s: qos_scan = %d", name_.c_str(), qosScan_);
@@ -537,18 +549,14 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToStereo_)
{
setupStereoCallbacks(
node,
subscribedToOdom_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToRGB_)
{
@@ -559,9 +567,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToRGBD_)
{
@@ -582,9 +588,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 5)
{
@@ -595,9 +599,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 4)
{
@@ -608,9 +610,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 3)
{
@@ -621,9 +621,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(rgbdCameras_ == 2)
{
@@ -634,9 +632,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
#else
if(rgbdCameras_>1)
@@ -656,9 +652,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else
{
@@ -669,9 +663,7 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScan2d_,
subscribedToScan3d_,
subscribedToScanDescriptor_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
}
else if(subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_)
@@ -682,27 +674,21 @@ void CommonDataSubscriber::setupCallbacks(
subscribedToScanDescriptor_,
subscribedToOdom_,
subscribedToUserData_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToSensorData_)
{
setupSensorDataCallbacks(
node,
subscribedToOdom_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
else if(subscribedToOdom_)
{
setupOdomCallbacks(
node,
subscribedToUserData_,
subscribedToOdomInfo_,
queueSize_,
approxSync_);
subscribedToOdomInfo_);
}
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_ || subscribedToScan2d_ || subscribedToScan3d_ || subscribedToScanDescriptor_ || subscribedToRGB_ || subscribedToOdom_)
@@ -716,7 +702,7 @@ void CommonDataSubscriber::setupCallbacks(
"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
name_.c_str(),
approxSync_?
uFormat("If topics are not published at the same rate, you could increase \"queue_size\" parameter (current=%d).", queueSize_).c_str():
uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str():
"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg_.c_str()),
otherTasks);
rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp
+69 -71
View File
@@ -466,202 +466,200 @@ void CommonDataSubscriber::setupDepthCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, depthOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, depthDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, depthData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
#endif
@@ -670,57 +668,57 @@ void CommonDataSubscriber::setupDepthCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, depthScanDesc, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan2d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, depthScan3d, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, depth, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, depth, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_);
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp
+9 -11
View File
@@ -66,29 +66,27 @@ void CommonDataSubscriber::odomDataInfoCallback(
void CommonDataSubscriber::setupOdomCallbacks(
rclcpp::Node& node,
bool subscribeUserData,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
if(subscribeUserData || subscribeOdomInfo)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync, queueSize, odomSub_, userDataSub_);
SYNC_DECL2(CommonDataSubscriber, odomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_);
}
}
else
@@ -96,13 +94,13 @@ void CommonDataSubscriber::setupOdomCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_);
}
}
else
{
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(1).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp
+67 -69
View File
@@ -466,201 +466,199 @@ void CommonDataSubscriber::setupRGBCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
image_transport::TransportHints hints(&node);
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan2d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbOdomScan3d, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbOdom, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan2d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbDataScan3d, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_);
SYNC_DECL3(CommonDataSubscriber, rgbData, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_);
}
}
#endif
@@ -669,57 +667,57 @@ void CommonDataSubscriber::setupRGBCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync, queueSize, imageSub_, cameraInfoSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScanDesc, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_);
}
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan2d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbScan3d, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync, queueSize, imageSub_, cameraInfoSub_);
SYNC_DECL2(CommonDataSubscriber, rgb, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_);
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp
+42 -44
View File
@@ -540,9 +540,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd callback");
@@ -557,152 +555,152 @@ void CommonDataSubscriber::setupRGBDCallbacks(
{
rgbdSubs_.resize(1);
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL3(CommonDataSubscriber, rgbdOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdOdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdOdom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdDataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]));
SYNC_DECL2(CommonDataSubscriber, rgbdData, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]));
}
}
#endif
@@ -711,41 +709,41 @@ void CommonDataSubscriber::setupRGBDCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdScan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
}
else
{
@@ -755,7 +753,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
}
else
{
rgbdSub_ = node.create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
rgbdSub_ = node.create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp
+42 -44
View File
@@ -352,9 +352,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd2 callback");
@@ -362,152 +360,152 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
for(int i=0; i<2; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd2DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL3(CommonDataSubscriber, rgbd2Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
#endif
@@ -516,45 +514,45 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbd2Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL2(CommonDataSubscriber, rgbd2, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp
+42 -44
View File
@@ -440,9 +440,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDescriptor,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd3 callback");
@@ -450,152 +448,152 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
for(int i=0; i<3; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd3DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL4(CommonDataSubscriber, rgbd3Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
#endif
@@ -604,46 +602,46 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
if(subscribeScanDescriptor)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbd3Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL3(CommonDataSubscriber, rgbd3, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp
+42 -44
View File
@@ -409,9 +409,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd4 callback");
@@ -419,152 +417,152 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
for(int i=0; i<4; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScanDesc, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan2d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd4DataScan3d, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Data, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#endif
@@ -573,45 +571,45 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
SYNC_DECL5(CommonDataSubscriber, rgbd4Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp
+21 -23
View File
@@ -261,9 +261,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd5 callback");
@@ -271,53 +269,53 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
for(int i=0; i<5; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL6(CommonDataSubscriber, rgbd5Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
else
@@ -325,45 +323,45 @@ void CommonDataSubscriber::setupRGBD5Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
SYNC_DECL6(CommonDataSubscriber, rgbd5Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL5(CommonDataSubscriber, rgbd5, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp
+21 -23
View File
@@ -279,9 +279,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbd6 callback");
@@ -289,53 +287,53 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
for(int i=0; i<6; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
}
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScanDesc, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan2d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomScan3d, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL7(CommonDataSubscriber, rgbd6Odom, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
else
@@ -343,45 +341,45 @@ void CommonDataSubscriber::setupRGBD6Callbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6ScanDesc, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan2d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
SYNC_DECL7(CommonDataSubscriber, rgbd6Scan3d, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_);
}
else
{
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL6(CommonDataSubscriber, rgbd6, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
}
}
}
rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp
+42 -44
View File
@@ -328,157 +328,155 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback");
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync, queueSize, odomSub_, userDataSub_, rgbdXSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomData, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_);
}
}
else
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync, queueSize, odomSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScanDesc, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync, queueSize, odomSub_, rgbdXSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan2d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync, queueSize, odomSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomScan3d, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync, queueSize, odomSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync, queueSize, odomSub_, rgbdXSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXOdom, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_);
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync, queueSize, userDataSub_, rgbdXSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScanDesc, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync, queueSize, userDataSub_, rgbdXSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan2d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync, queueSize, userDataSub_, rgbdXSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, rgbdXDataScan3d, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync, queueSize, userDataSub_, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync, queueSize, userDataSub_, rgbdXSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXData, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_);
}
}
#endif
@@ -487,46 +485,46 @@ void CommonDataSubscriber::setupRGBDXCallbacks(
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync, queueSize, rgbdXSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScanDesc, approxSync_, syncQueueSize_, rgbdXSub_, scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync, queueSize, rgbdXSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScan2d, approxSync_, syncQueueSize_, rgbdXSub_, scanSub_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync, queueSize, rgbdXSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, rgbdXScan3d, approxSync_, syncQueueSize_, rgbdXSub_, scan3dSub_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync, queueSize, rgbdXSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_);
}
else
{
rgbdXSub_.unsubscribe();
rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
rgbdXSubOnly_ = node.create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_), std::bind(&CommonDataSubscriber::rgbdXCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp
+42 -44
View File
@@ -253,9 +253,7 @@ void CommonDataSubscriber::setupScanCallbacks(
#else
bool,
#endif
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
@@ -268,36 +266,36 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
else
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(1).reliability(qosScan_).get_rmw_qos_profile());
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, scanDescSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScanDesc, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -305,12 +303,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, scanSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan2d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_);
}
}
else
@@ -318,12 +316,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_);
SYNC_DECL3(CommonDataSubscriber, odomDataScan3d, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_);
}
}
}
@@ -331,19 +329,19 @@ void CommonDataSubscriber::setupScanCallbacks(
#endif
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync, queueSize, odomSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync, queueSize, odomSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, odomScanDesc, approxSync_, syncQueueSize_, odomSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -351,12 +349,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync, queueSize, odomSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan2d, approxSync_, syncQueueSize_, odomSub_, scanSub_);
}
}
else
@@ -364,31 +362,31 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync, queueSize, odomSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, odomScan3d, approxSync_, syncQueueSize_, odomSub_, scan3dSub_);
}
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(1).reliability(qosUserData_).get_rmw_qos_profile());
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
if(scanDescTopic)
{
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync, queueSize, userDataSub_, scanDescSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync, queueSize, userDataSub_, scanDescSub_);
SYNC_DECL2(CommonDataSubscriber, dataScanDesc, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_);
}
}
else if(scan2dTopic)
@@ -396,12 +394,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync, queueSize, userDataSub_, scanSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan2d, approxSync_, syncQueueSize_, userDataSub_, scanSub_);
}
}
else
@@ -409,12 +407,12 @@ void CommonDataSubscriber::setupScanCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync, queueSize, userDataSub_, scan3dSub_);
SYNC_DECL2(CommonDataSubscriber, dataScan3d, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_);
}
}
}
@@ -422,18 +420,18 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(scanDescTopic)
{
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync, queueSize, scanDescSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_);
}
else if(scan2dTopic)
{
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan2dInfo, approxSync_, syncQueueSize_, scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
SYNC_DECL2(CommonDataSubscriber, scan3dInfo, approxSync_, syncQueueSize_, scan3dSub_, odomInfoSub_);
}
}
}
@@ -442,7 +440,7 @@ void CommonDataSubscriber::setupScanCallbacks(
if(scanDescTopic)
{
subscribedToScanDescriptor_ = true;
scanDescSubOnly_ = node.create_subscription<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
scanDescSubOnly_ = node.create_subscription<rtabmap_msgs::msg::ScanDescriptor>("scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scanDescCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -451,7 +449,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else if(scan2dTopic)
{
subscribedToScan2d_ = true;
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
@@ -460,7 +458,7 @@ void CommonDataSubscriber::setupScanCallbacks(
else
{
subscribedToScan3d_ = true;
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
node.get_name(),
rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp
+9 -12
View File
@@ -66,26 +66,23 @@ void CommonDataSubscriber::sensorDataOdomInfoCallback(
void CommonDataSubscriber::setupSensorDataCallbacks(
rclcpp::Node& node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup SensorData callback");
sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(1).reliability(qosSensorData_).get_rmw_qos_profile());
sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile());
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync, queueSize, odomSub_, sensorDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_);
}
else
{
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync, queueSize, odomSub_, sensorDataSub_);
SYNC_DECL2(CommonDataSubscriber, sensorDataOdom, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_);
}
}
else
@@ -93,13 +90,13 @@ void CommonDataSubscriber::setupSensorDataCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync, queueSize, sensorDataSub_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_);
}
else
{
sensorDataSub_.unsubscribe();
sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(1).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
sensorDataSubOnly_ = node.create_subscription<rtabmap_msgs::msg::SensorData>("sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_), std::bind(&CommonDataSubscriber::sensorDataCallback, this, std::placeholders::_1));
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp
+12 -14
View File
@@ -88,31 +88,29 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
void CommonDataSubscriber::setupStereoCallbacks(
rclcpp::Node& node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
bool subscribeOdomInfo)
{
RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
image_transport::TransportHints hints(&node);
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(1).reliability(qosCameraInfo_).get_rmw_qos_profile());
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL5(CommonDataSubscriber, stereoOdom, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
else
@@ -120,12 +118,12 @@ void CommonDataSubscriber::setupStereoCallbacks(
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(1).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
SYNC_DECL4(CommonDataSubscriber, stereo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
}
}
}
rtabmap_sync/src/nodelets/rgb_sync.cpp
+20 -8
View File
@@ -51,13 +51,24 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "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);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
@@ -65,8 +76,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
@@ -75,20 +87,20 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageSub_, cameraInfoSub_);
exactSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
get_name(),
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+21 -9
View File
@@ -53,13 +53,24 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
approxSyncDepth_(0),
exactSyncDepth_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "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);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
depthScale_ = this->declare_parameter("depth_scale", depthScale_);
@@ -74,8 +85,9 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
RCLCPP_INFO(this->get_logger(), "%s: depth_scale = %f", get_name(), depthScale_);
RCLCPP_INFO(this->get_logger(), "%s: decimation = %d", get_name(), decimation_);
@@ -86,21 +98,21 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_ = new message_filters::Synchronizer<MyExactSyncDepthPolicy>(MyExactSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+23 -11
View File
@@ -42,21 +42,33 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
SYNC_INIT(rgbd7),
SYNC_INIT(rgbd8)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "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);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
@@ -68,14 +80,14 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
for(int i=0; i<rgbdCameras; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
rgbdSubs_[i]->subscribe(this, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
}
std::string name_ = get_name();
std::string subscribedTopicsMsg_;
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd2ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -83,7 +95,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd3ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -91,7 +103,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==4)
{
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd4ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -99,7 +111,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==5)
{
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd5ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -107,7 +119,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==6)
{
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd6ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -115,7 +127,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==7)
{
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd7ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
@@ -123,7 +135,7 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
}
else if(rgbdCameras==8)
{
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, syncQueueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd8ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
rtabmap_sync/src/nodelets/stereo_sync.cpp
+22 -10
View File
@@ -50,21 +50,33 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
approxSync_(0),
exactSync_(0)
{
int queueSize = 10;
int topicQueueSize = 1;
int syncQueueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
{
syncQueueSize = queueSize;
RCLCPP_WARN(this->get_logger(), "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);
}
syncQueueSize = this->declare_parameter("sync_queue_size", syncQueueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
RCLCPP_INFO(this->get_logger(), "%s: compressed_rate = %f", get_name(), compressedRate_);
@@ -73,22 +85,22 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(syncQueueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
exactSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
image_transport::TransportHints hints(this);
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
imageLeftSub_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageRightSub_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),