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

Merged master to ros2. ros2: Uniformized qos of all subscribers and publishers.

Browse Source
This commit is contained in:
matlabbe
2021-10-04 19:29:19 -04:00
parent ccf152d877 1ecce15878
commit af1c7edeef
121 changed files with 10854 additions and 3478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/impl/CommonDataSubscriberRGBD4.cpp
+288 -289
View File
@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/CommonDataSubscriber.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap_ros/MsgConversion.h>
#include <cv_bridge/cv_bridge.h>
@@ -40,11 +41,37 @@ namespace rtabmap_ros {
rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
rtabmap_ros::toCvShare(image4Msg, imageMsgs[3], depthMsgs[3]); \
if(!depthMsgs[0].get()) \
depthMsgs.clear(); \
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info);
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
std::vector<cv::Mat> localDescriptors; \
if(!image1Msg->global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(image1Msg->global_descriptor); \
if(!image2Msg->global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(image2Msg->global_descriptor); \
if(!image3Msg->global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(image3Msg->global_descriptor); \
if(!image4Msg->global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(image4Msg->global_descriptor); \
localKeyPoints.push_back(image1Msg->key_points); \
localKeyPoints.push_back(image2Msg->key_points); \
localKeyPoints.push_back(image3Msg->key_points); \
localKeyPoints.push_back(image4Msg->key_points); \
localPoints3d.push_back(image1Msg->points); \
localPoints3d.push_back(image2Msg->points); \
localPoints3d.push_back(image3Msg->points); \
localPoints3d.push_back(image4Msg->points); \
localDescriptors.push_back(rtabmap::uncompressData(image1Msg->descriptors)); \
localDescriptors.push_back(rtabmap::uncompressData(image2Msg->descriptors)); \
localDescriptors.push_back(rtabmap::uncompressData(image3Msg->descriptors)); \
localDescriptors.push_back(rtabmap::uncompressData(image4Msg->descriptors));
// 4 RGBD
void CommonDataSubscriber::rgbd4Callback(
@@ -57,10 +84,10 @@ void CommonDataSubscriber::rgbd4Callback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -73,9 +100,9 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -88,9 +115,27 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -103,39 +148,9 @@ void CommonDataSubscriber::rgbd4InfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4Scan2dInfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4Scan3dInfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
@@ -149,10 +164,10 @@ void CommonDataSubscriber::rgbd4OdomCallback(
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -165,9 +180,9 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -180,9 +195,27 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -195,41 +228,12 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4OdomScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4OdomScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
// 2 RGBD + User Data
void CommonDataSubscriber::rgbd4DataCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -241,10 +245,10 @@ void CommonDataSubscriber::rgbd4DataCallback(
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -257,9 +261,9 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -272,9 +276,27 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -287,39 +309,9 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4DataScan2dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4DataScan3dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -333,10 +325,10 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
{
IMAGE_CONVERSION();
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -349,9 +341,9 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
{
IMAGE_CONVERSION();
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -364,9 +356,27 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
{
IMAGE_CONVERSION();
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const rtabmap_ros::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
{
IMAGE_CONVERSION();
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
if(!scanDescMsg->global_descriptor.data.empty())
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -379,40 +389,11 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
{
IMAGE_CONVERSION();
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4OdomDataScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4Msg,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
IMAGE_CONVERSION();
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
void CommonDataSubscriber::setupRGBD4Callbacks(
rclcpp::Node& node,
@@ -420,6 +401,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
bool subscribeUserData,
bool subscribeScan2d,
bool subscribeScan3d,
bool subscribeScanDesc,
bool subscribeOdomInfo,
int queueSize,
bool approxSync)
@@ -430,182 +412,199 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
for(int i=0; i<4; ++i)
{
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i));
}
#ifdef RTABMAP_SYNC_USER_DATA
if(subscribeOdom && subscribeUserData)
{
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
odomSub_.subscribe(&node, "odom");
userDataSub_.subscribe(&node, "user_data");
if(subscribeScan2d)
if(subscribeScanDesc)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL8(rgbd4OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
else
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
if(subscribeOdomInfo)
{
SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
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_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
scan3dSub_.subscribe(&node, "scan_cloud");
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL8(rgbd4OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
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_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL7(rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info");
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else if(subscribeOdom)
{
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL7(rgbd4OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL7(rgbd4OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL6(rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL7(rgbd4DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
}
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL7(rgbd4DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
}
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL6(rgbd4DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd4Data, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomData, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
else
#endif
if(subscribeOdom)
{
if(subscribeScan2d)
odomSub_.subscribe(&node, "odom");
if(subscribeScanDesc)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL6(rgbd4Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
subscribedToOdomInfo_ = false;
RCLCPP_WARN(node.get_logger(), "subscribe_odom_info ignored...");
}
else
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
if(subscribeOdomInfo)
{
SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
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_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
scan3dSub_.subscribe(&node, "scan_cloud");
if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL6(rgbd4Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
}
else
{
SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
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_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
SYNC_DECL5(rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
odomInfoSub_.subscribe(&node, "odom_info");
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL4(rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
SYNC_DECL5(CommonDataSubscriber, rgbd4Odom, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
#ifdef RTABMAP_SYNC_USER_DATA
else if(subscribeUserData)
{
userDataSub_.subscribe(&node, "user_data");
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
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_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
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_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
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_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync, queueSize, 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]));
}
}
#endif
else
{
if(subscribeScanDesc)
{
subscribedToScanDescriptor_ = true;
scanDescSub_.subscribe(&node, "scan_descriptor");
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_);
}
else if(subscribeScan2d)
{
subscribedToScan2d_ = true;
scanSub_.subscribe(&node, "scan");
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_);
}
else if(subscribeScan3d)
{
subscribedToScan3d_ = true;
scan3dSub_.subscribe(&node, "scan_cloud");
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_);
}
else if(subscribeOdomInfo)
{
subscribedToOdomInfo_ = true;
odomInfoSub_.subscribe(&node, "odom_info");
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
}
else
{
SYNC_DECL4(CommonDataSubscriber, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
}
}
}