mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-05 17:27:46 +08:00
created ros2 branch
This commit is contained in:
matlabbe
@@ -31,303 +31,302 @@ namespace rtabmap_ros {
|
||||
|
||||
// RGB + Depth
|
||||
void CommonDataSubscriber::depthCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan2dCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan3dCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan2dInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthScan3dInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom
|
||||
void CommonDataSubscriber::depthOdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + User Data
|
||||
void CommonDataSubscriber::depthDataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthDataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
void CommonDataSubscriber::depthOdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::ImageConstPtr& depthMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupDepthCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -336,36 +335,26 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup depth callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
|
||||
|
||||
std::string rgbPrefix = "rgb";
|
||||
std::string depthPrefix = "depth";
|
||||
ros::NodeHandle rgb_nh(nh, rgbPrefix);
|
||||
ros::NodeHandle depth_nh(nh, depthPrefix);
|
||||
ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
|
||||
ros::NodeHandle depth_pnh(pnh, depthPrefix);
|
||||
image_transport::ImageTransport rgb_it(rgb_nh);
|
||||
image_transport::ImageTransport depth_it(depth_nh);
|
||||
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
|
||||
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
|
||||
|
||||
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
|
||||
imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
|
||||
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
|
||||
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rmw_qos_profile_sensor_data);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -376,11 +365,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -391,7 +380,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(depthOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -401,16 +390,16 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -421,11 +410,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -436,7 +425,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(depthOdomInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -446,17 +435,17 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -467,11 +456,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -482,7 +471,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(depthDataInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -495,11 +484,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -510,11 +499,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -525,7 +514,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(depthInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -30,62 +30,61 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_ros {
|
||||
|
||||
void CommonDataSubscriber::odomCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupOdomCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup scan callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
|
||||
|
||||
if(subscribeUserData || subscribeOdomInfo)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(odomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -96,17 +95,17 @@ void CommonDataSubscriber::setupOdomCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL2(odomInfo, approxSync, queueSize, odomSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
odomSubOnly_ = nh.subscribe("odom", 1, &CommonDataSubscriber::odomCallback, this);
|
||||
odomSubOnly_ = node.create_subscription<nav_msgs::msg::Odometry>("odom", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::odomCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
ros::this_node::getName().c_str(),
|
||||
odomSubOnly_.getTopic().c_str());
|
||||
node.get_name(),
|
||||
odomSubOnly_->get_topic_name());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -31,303 +31,302 @@ namespace rtabmap_ros {
|
||||
|
||||
// RGB
|
||||
void CommonDataSubscriber::rgbCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan2dCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan3dCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan2dInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbScan3dInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Odom
|
||||
void CommonDataSubscriber::rgbOdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + User Data
|
||||
void CommonDataSubscriber::rgbDataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbDataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
void CommonDataSubscriber::rgbOdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::ImageConstPtr& imageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -336,30 +335,25 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup rgb-only callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
|
||||
|
||||
std::string rgbPrefix = "rgb";
|
||||
ros::NodeHandle rgb_nh(nh, rgbPrefix);
|
||||
ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
|
||||
image_transport::ImageTransport rgb_it(rgb_nh);
|
||||
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
|
||||
|
||||
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
|
||||
cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rmw_qos_profile_sensor_data);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -370,11 +364,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -385,7 +379,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -395,16 +389,16 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -415,11 +409,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -430,7 +424,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbOdomInfo, approxSync, queueSize, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -440,17 +434,17 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -461,11 +455,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -476,7 +470,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbDataInfo, approxSync, queueSize, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -489,11 +483,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbScan2dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -504,11 +498,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbScan3dInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -519,7 +513,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbInfo, approxSync, queueSize, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -34,327 +34,326 @@ namespace rtabmap_ros {
|
||||
|
||||
// 1 RGBD camera
|
||||
void CommonDataSubscriber::rgbdCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan2dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan3dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan2dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan3dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom
|
||||
void CommonDataSubscriber::rgbdOdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + User Data
|
||||
void CommonDataSubscriber::rgbdDataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdDataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom + User Data
|
||||
void CommonDataSubscriber::rgbdOdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdOdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgbCameraInfo, image1Msg->depthCameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -363,27 +362,27 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup rgbd callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd callback");
|
||||
|
||||
if(subscribeOdom || subscribeUserData || subscribeScan2d || subscribeScan3d || subscribeOdomInfo)
|
||||
{
|
||||
rgbdSubs_.resize(1);
|
||||
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
|
||||
rgbdSubs_[0]->subscribe(nh, "rgbd_image", 1);
|
||||
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
|
||||
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rmw_qos_profile_sensor_data);
|
||||
|
||||
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -394,11 +393,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -409,7 +408,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbdOdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -419,15 +418,15 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -438,11 +437,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -453,7 +452,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbdOdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -463,15 +462,15 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -482,11 +481,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -497,7 +496,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbdDataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -510,11 +509,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -525,11 +524,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -540,23 +539,23 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL2(rgbdInfo, approxSync, queueSize, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_FATAL("Not supposed to be here!");
|
||||
UFATAL("Not supposed to be here!");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
|
||||
rgbdSub_ = node.create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::rgbdCallback, this, std::placeholders::_1));
|
||||
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
ros::this_node::getName().c_str(),
|
||||
rgbdSub_.getTopic().c_str());
|
||||
node.get_name(),
|
||||
rgbdSub_->get_topic_name());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -38,333 +38,332 @@ namespace rtabmap_ros {
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
|
||||
rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo);
|
||||
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info);
|
||||
|
||||
// 2 RGBD
|
||||
void CommonDataSubscriber::rgbd2Callback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan2dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan3dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2InfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan2dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2Scan3dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd2OdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd2DataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2DataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd2OdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd2OdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -373,26 +372,26 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup rgbd2 callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd2 callback");
|
||||
|
||||
rgbdSubs_.resize(2);
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
|
||||
}
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -403,11 +402,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -418,7 +417,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd2OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -428,15 +427,15 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -447,11 +446,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -462,7 +461,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbd2OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -472,15 +471,15 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -491,11 +490,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -506,7 +505,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbd2DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -519,11 +518,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -534,11 +533,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -549,7 +548,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(rgbd2Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -39,358 +39,357 @@ namespace rtabmap_ros {
|
||||
rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
rtabmap_ros::toCvShare(image2Msg, imageMsgs[1], depthMsgs[1]); \
|
||||
rtabmap_ros::toCvShare(image3Msg, imageMsgs[2], depthMsgs[2]); \
|
||||
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo);
|
||||
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);
|
||||
|
||||
// 3 RGBD
|
||||
void CommonDataSubscriber::rgbd3Callback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan2dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan3dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3InfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan2dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3Scan3dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2Msg,
|
||||
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd3OdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd3DataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3DataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd3OdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd3OdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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 sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -399,26 +398,26 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup rgbd3 callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd3 callback");
|
||||
|
||||
rgbdSubs_.resize(3);
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
|
||||
}
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL7(rgbd3OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -429,11 +428,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL7(rgbd3OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -444,7 +443,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd3OdomDataInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -454,15 +453,15 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd3OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -473,11 +472,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd3OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -488,7 +487,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd3OdomInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -498,15 +497,15 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd3DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -517,11 +516,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(rgbd3DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -532,7 +531,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd3DataInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -545,11 +544,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd3Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -560,11 +559,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd3Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -575,7 +574,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(rgbd3Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -40,383 +40,382 @@ 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]); \
|
||||
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image3Msg->rgbCameraInfo); \
|
||||
cameraInfoMsgs.push_back(image4Msg->rgbCameraInfo);
|
||||
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);
|
||||
|
||||
// 4 RGBD
|
||||
void CommonDataSubscriber::rgbd4Callback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
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
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4Scan2dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4Scan3dCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4InfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
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::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
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::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
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);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd4OdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // 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::rgbd4OdomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // 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::rgbd4OdomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd4DataCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
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::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
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::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd4OdomDataCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg)
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // 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::rgbd4OdomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr& odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr& userDataMsg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
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::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -425,26 +424,26 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup rgbd4 callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgbd4 callback");
|
||||
|
||||
rgbdSubs_.resize(4);
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(nh, uFormat("rgbd_image%d", i), 1);
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_ros::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rmw_qos_profile_sensor_data);
|
||||
}
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data");
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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_);
|
||||
}
|
||||
else
|
||||
@@ -455,11 +454,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -470,7 +469,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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_);
|
||||
}
|
||||
else
|
||||
@@ -480,15 +479,15 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -499,11 +498,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -514,7 +513,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -524,15 +523,15 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data");
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -543,11 +542,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -558,7 +557,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -571,11 +570,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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_);
|
||||
}
|
||||
else
|
||||
@@ -586,11 +585,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
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
|
||||
@@ -601,7 +600,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(rgbd4Info, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -30,172 +30,171 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_ros {
|
||||
|
||||
void CommonDataSubscriber::scan2dCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scan3dCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scan2dInfoCallback(
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::scan3dInfoCallback(
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::odomScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::dataScan2dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan3dCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan2dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::dataScan3dInfoCallback(
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::odomDataScan2dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan3dCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan2dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::odomDataScan3dInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const rtabmap_ros::UserDataConstPtr & userDataMsg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::ConstSharedPtr scan2dMsg; // Null
|
||||
commonLaserScanCallback(odomMsg, userDataMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupScanCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool scan2dTopic,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
@@ -203,32 +202,32 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup scan callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup scan callback");
|
||||
|
||||
if(subscribeOdom || subscribeUserData || subscribeOdomInfo)
|
||||
{
|
||||
if(scan2dTopic)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
scanSub_.subscribe(&node, "scan", rmw_qos_profile_sensor_data);
|
||||
}
|
||||
else
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rmw_qos_profile_sensor_data);
|
||||
}
|
||||
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(odomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -241,7 +240,7 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL4(odomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -252,14 +251,14 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
}
|
||||
else if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(odomScan2dInfo, approxSync, queueSize, odomSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -272,7 +271,7 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(odomScan3dInfo, approxSync, queueSize, odomSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -283,14 +282,14 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
}
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(nh, "user_data", 1);
|
||||
userDataSub_.subscribe(&node, "user_data", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(scan2dTopic)
|
||||
{
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(dataScan2dInfo, approxSync, queueSize, userDataSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -303,7 +302,7 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL3(dataScan3dInfo, approxSync, queueSize, userDataSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -319,7 +318,7 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL2(scan2dInfo, approxSync, queueSize, scanSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
@@ -328,7 +327,7 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL2(scan3dInfo, approxSync, queueSize, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
@@ -339,22 +338,24 @@ void CommonDataSubscriber::setupScanCallbacks(
|
||||
if(scan2dTopic)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scan2dSubOnly_ = nh.subscribe("scan", 1, &CommonDataSubscriber::scan2dCallback, this);
|
||||
scan2dSubOnly_ = node.create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::scan2dCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
ros::this_node::getName().c_str(),
|
||||
scan2dSubOnly_.getTopic().c_str());
|
||||
node.get_name(),
|
||||
scan2dSubOnly_->get_topic_name());
|
||||
}
|
||||
else
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSubOnly_ = nh.subscribe("scan_cloud", 1, &CommonDataSubscriber::scan3dCallback, this);
|
||||
scan3dSubOnly_ = node.create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::SensorDataQoS(), std::bind(&CommonDataSubscriber::scan3dCallback, this, std::placeholders::_1));
|
||||
subscribedTopicsMsg_ =
|
||||
uFormat("\n%s subscribed to:\n %s",
|
||||
ros::this_node::getName().c_str(),
|
||||
scan3dSubOnly_.getTopic().c_str());
|
||||
node.get_name(),
|
||||
scan3dSubOnly_->get_topic_name());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
} /* namespace rtabmap_ros */
|
||||
|
||||
|
||||
@@ -31,96 +31,87 @@ namespace rtabmap_ros {
|
||||
|
||||
// Stereo
|
||||
void CommonDataSubscriber::stereoCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::stereoInfoCallback(
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// Stereo + Odom
|
||||
void CommonDataSubscriber::stereoOdomCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
|
||||
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // null
|
||||
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::stereoOdomInfoCallback(
|
||||
const nav_msgs::OdometryConstPtr & odomMsg,
|
||||
const sensor_msgs::ImageConstPtr& leftImageMsg,
|
||||
const sensor_msgs::ImageConstPtr& rightImageMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
|
||||
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr rightImageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg,
|
||||
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
callbackCalled();
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupStereoCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
ros::NodeHandle & pnh,
|
||||
rclcpp::Node& node,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo,
|
||||
int queueSize,
|
||||
bool approxSync)
|
||||
{
|
||||
ROS_INFO("Setup stereo callback");
|
||||
RCLCPP_INFO(node.get_logger(), "Setup stereo callback");
|
||||
|
||||
ros::NodeHandle left_nh(nh, "left");
|
||||
ros::NodeHandle right_nh(nh, "right");
|
||||
ros::NodeHandle left_pnh(pnh, "left");
|
||||
ros::NodeHandle right_pnh(pnh, "right");
|
||||
image_transport::ImageTransport left_it(left_nh);
|
||||
image_transport::ImageTransport right_it(right_nh);
|
||||
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
|
||||
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
|
||||
|
||||
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
|
||||
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
|
||||
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
|
||||
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
|
||||
imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rmw_qos_profile_sensor_data);
|
||||
cameraInfoLeft_.subscribe(&node, "left/camera_info", rmw_qos_profile_sensor_data);
|
||||
cameraInfoRight_.subscribe(&node, "right/camera_info", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(nh, "odom", 1);
|
||||
odomSub_.subscribe(&node, "odom", rmw_qos_profile_sensor_data);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL6(stereoOdomInfo, approxSync, queueSize, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -133,7 +124,7 @@ void CommonDataSubscriber::setupStereoCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rmw_qos_profile_sensor_data);
|
||||
SYNC_DECL5(stereoInfo, approxSync, queueSize, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
Reference in New Issue
Block a user