i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 18:27:46 +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/CommonDataSubscriberRGBD5.cpp
+16 -10
View File
@@ -50,6 +50,12 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->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); \
depthCameraInfoMsgs.push_back(image5Msg->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; \
@@ -95,7 +101,7 @@ void CommonDataSubscriber::rgbd5Callback(
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::rgbd5Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -111,7 +117,7 @@ void CommonDataSubscriber::rgbd5Scan2dCallback(
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::rgbd5Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -127,7 +133,7 @@ void CommonDataSubscriber::rgbd5Scan3dCallback(
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::rgbd5ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -146,7 +152,7 @@ void CommonDataSubscriber::rgbd5ScanDescCallback(
{
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::rgbd5InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -162,7 +168,7 @@ void CommonDataSubscriber::rgbd5InfoCallback(
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);
}
// 5 RGBD + Odom
@@ -180,7 +186,7 @@ void CommonDataSubscriber::rgbd5OdomCallback(
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::rgbd5OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -196,7 +202,7 @@ void CommonDataSubscriber::rgbd5OdomScan2dCallback(
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::rgbd5OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -212,7 +218,7 @@ void CommonDataSubscriber::rgbd5OdomScan3dCallback(
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::rgbd5OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -231,7 +237,7 @@ void CommonDataSubscriber::rgbd5OdomScanDescCallback(
{
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::rgbd5OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -247,7 +253,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback(
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::setupRGBD5Callbacks(