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/CommonDataSubscriberRGBD6.cpp
+17 -10
View File
@@ -52,6 +52,13 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
std::vector<sensor_msgs::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); \
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image6Msg->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; \
@@ -103,7 +110,7 @@ void CommonDataSubscriber::rgbd6Callback(
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::rgbd6Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -120,7 +127,7 @@ void CommonDataSubscriber::rgbd6Scan2dCallback(
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::rgbd6Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -137,7 +144,7 @@ void CommonDataSubscriber::rgbd6Scan3dCallback(
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::rgbd6ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -157,7 +164,7 @@ void CommonDataSubscriber::rgbd6ScanDescCallback(
{
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::rgbd6InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -174,7 +181,7 @@ void CommonDataSubscriber::rgbd6InfoCallback(
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);
}
// 6 RGBD + Odom
@@ -193,7 +200,7 @@ void CommonDataSubscriber::rgbd6OdomCallback(
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::rgbd6OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -210,7 +217,7 @@ void CommonDataSubscriber::rgbd6OdomScan2dCallback(
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::rgbd6OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -227,7 +234,7 @@ void CommonDataSubscriber::rgbd6OdomScan3dCallback(
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::rgbd6OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -247,7 +254,7 @@ void CommonDataSubscriber::rgbd6OdomScanDescCallback(
{
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::rgbd6OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -264,7 +271,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback(
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);
}
void CommonDataSubscriber::setupRGBD6Callbacks(