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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/impl/CommonDataSubscriberRGBD4.cpp
+25 -20
View File
@@ -48,6 +48,11 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_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; \
@@ -87,7 +92,7 @@ void CommonDataSubscriber::rgbd4Callback(
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, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthcameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -102,7 +107,7 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -117,7 +122,7 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -135,7 +140,7 @@ void CommonDataSubscriber::rgbd4ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -150,7 +155,7 @@ void CommonDataSubscriber::rgbd4InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
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);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
@@ -167,7 +172,7 @@ void CommonDataSubscriber::rgbd4OdomCallback(
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, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -182,7 +187,7 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -197,7 +202,7 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -215,7 +220,7 @@ void CommonDataSubscriber::rgbd4OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -230,7 +235,7 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
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);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -248,7 +253,7 @@ void CommonDataSubscriber::rgbd4DataCallback(
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, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -263,7 +268,7 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -278,7 +283,7 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -296,7 +301,7 @@ void CommonDataSubscriber::rgbd4DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -311,7 +316,7 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
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);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -328,7 +333,7 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
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, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -343,7 +348,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -358,7 +363,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -376,7 +381,7 @@ void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -391,7 +396,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
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);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif