mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
Added OdomInfo + scan2d/scan3d synchronization interface
This commit is contained in:
matlabbe
@@ -46,24 +46,32 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
|
||||
SYNC_INIT(depthScan2d),
|
||||
SYNC_INIT(depthScan3d),
|
||||
SYNC_INIT(depthInfo),
|
||||
SYNC_INIT(depthScan2dInfo),
|
||||
SYNC_INIT(depthScan3dInfo),
|
||||
|
||||
// RGB + Depth + Odom
|
||||
SYNC_INIT(depthOdom),
|
||||
SYNC_INIT(depthOdomScan2d),
|
||||
SYNC_INIT(depthOdomScan3d),
|
||||
SYNC_INIT(depthOdomInfo),
|
||||
SYNC_INIT(depthOdomScan2dInfo),
|
||||
SYNC_INIT(depthOdomScan3dInfo),
|
||||
|
||||
// RGB + Depth + User Data
|
||||
SYNC_INIT(depthData),
|
||||
SYNC_INIT(depthDataScan2d),
|
||||
SYNC_INIT(depthDataScan3d),
|
||||
SYNC_INIT(depthDataInfo),
|
||||
SYNC_INIT(depthDataScan2dInfo),
|
||||
SYNC_INIT(depthDataScan3dInfo),
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
SYNC_INIT(depthOdomData),
|
||||
SYNC_INIT(depthOdomDataScan2d),
|
||||
SYNC_INIT(depthOdomDataScan3d),
|
||||
SYNC_INIT(depthOdomDataInfo),
|
||||
SYNC_INIT(depthOdomDataScan2dInfo),
|
||||
SYNC_INIT(depthOdomDataScan3dInfo),
|
||||
|
||||
// Stereo
|
||||
SYNC_INIT(stereo),
|
||||
@@ -77,48 +85,64 @@ CommonDataSubscriber::CommonDataSubscriber(bool gui) :
|
||||
SYNC_INIT(rgbdScan2d),
|
||||
SYNC_INIT(rgbdScan3d),
|
||||
SYNC_INIT(rgbdInfo),
|
||||
SYNC_INIT(rgbdScan2dInfo),
|
||||
SYNC_INIT(rgbdScan3dInfo),
|
||||
|
||||
// 1 RGBD + Odom
|
||||
SYNC_INIT(rgbdOdom),
|
||||
SYNC_INIT(rgbdOdomScan2d),
|
||||
SYNC_INIT(rgbdOdomScan3d),
|
||||
SYNC_INIT(rgbdOdomInfo),
|
||||
SYNC_INIT(rgbdOdomScan2dInfo),
|
||||
SYNC_INIT(rgbdOdomScan3dInfo),
|
||||
|
||||
// 1 RGBD + User Data
|
||||
SYNC_INIT(rgbdData),
|
||||
SYNC_INIT(rgbdDataScan2d),
|
||||
SYNC_INIT(rgbdDataScan3d),
|
||||
SYNC_INIT(rgbdDataInfo),
|
||||
SYNC_INIT(rgbdDataScan2dInfo),
|
||||
SYNC_INIT(rgbdDataScan3dInfo),
|
||||
|
||||
// 1 RGBD + Odom + User Data
|
||||
SYNC_INIT(rgbdOdomData),
|
||||
SYNC_INIT(rgbdOdomDataScan2d),
|
||||
SYNC_INIT(rgbdOdomDataScan3d),
|
||||
SYNC_INIT(rgbdOdomDataInfo),
|
||||
SYNC_INIT(rgbdOdomDataScan2dInfo),
|
||||
SYNC_INIT(rgbdOdomDataScan3dInfo),
|
||||
|
||||
// 2 RGBD
|
||||
SYNC_INIT(rgbd2),
|
||||
SYNC_INIT(rgbd2Scan2d),
|
||||
SYNC_INIT(rgbd2Scan3d),
|
||||
SYNC_INIT(rgbd2Info),
|
||||
SYNC_INIT(rgbd2Scan2dInfo),
|
||||
SYNC_INIT(rgbd2Scan3dInfo),
|
||||
|
||||
// 2 RGBD + Odom
|
||||
SYNC_INIT(rgbd2Odom),
|
||||
SYNC_INIT(rgbd2OdomScan2d),
|
||||
SYNC_INIT(rgbd2OdomScan3d),
|
||||
SYNC_INIT(rgbd2OdomInfo),
|
||||
SYNC_INIT(rgbd2OdomScan2dInfo),
|
||||
SYNC_INIT(rgbd2OdomScan3dInfo),
|
||||
|
||||
// 2 RGBD + User Data
|
||||
SYNC_INIT(rgbd2Data),
|
||||
SYNC_INIT(rgbd2DataScan2d),
|
||||
SYNC_INIT(rgbd2DataScan3d),
|
||||
SYNC_INIT(rgbd2DataInfo),
|
||||
SYNC_INIT(rgbd2DataScan2dInfo),
|
||||
SYNC_INIT(rgbd2DataScan3dInfo),
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
SYNC_INIT(rgbd2OdomData),
|
||||
SYNC_INIT(rgbd2OdomDataScan2d),
|
||||
SYNC_INIT(rgbd2OdomDataScan3d),
|
||||
SYNC_INIT(rgbd2OdomDataInfo)
|
||||
SYNC_INIT(rgbd2OdomDataInfo),
|
||||
SYNC_INIT(rgbd2OdomDataScan2dInfo),
|
||||
SYNC_INIT(rgbd2OdomDataScan3dInfo)
|
||||
|
||||
{
|
||||
}
|
||||
@@ -280,24 +304,32 @@ CommonDataSubscriber::~CommonDataSubscriber()
|
||||
SYNC_DEL(depthScan2d);
|
||||
SYNC_DEL(depthScan3d);
|
||||
SYNC_DEL(depthInfo);
|
||||
SYNC_DEL(depthScan2dInfo);
|
||||
SYNC_DEL(depthScan3dInfo);
|
||||
|
||||
// RGB + Depth + Odom
|
||||
SYNC_DEL(depthOdom);
|
||||
SYNC_DEL(depthOdomScan2d);
|
||||
SYNC_DEL(depthOdomScan3d);
|
||||
SYNC_DEL(depthOdomInfo);
|
||||
SYNC_DEL(depthOdomScan2dInfo);
|
||||
SYNC_DEL(depthOdomScan3dInfo);
|
||||
|
||||
// RGB + Depth + User Data
|
||||
SYNC_DEL(depthData);
|
||||
SYNC_DEL(depthDataScan2d);
|
||||
SYNC_DEL(depthDataScan3d);
|
||||
SYNC_DEL(depthDataInfo);
|
||||
SYNC_DEL(depthDataScan2dInfo);
|
||||
SYNC_DEL(depthDataScan3dInfo);
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
SYNC_DEL(depthOdomData);
|
||||
SYNC_DEL(depthOdomDataScan2d);
|
||||
SYNC_DEL(depthOdomDataScan3d);
|
||||
SYNC_DEL(depthOdomDataInfo);
|
||||
SYNC_DEL(depthOdomDataScan2dInfo);
|
||||
SYNC_DEL(depthOdomDataScan3dInfo);
|
||||
|
||||
// Stereo
|
||||
SYNC_DEL(stereo);
|
||||
@@ -311,48 +343,64 @@ CommonDataSubscriber::~CommonDataSubscriber()
|
||||
SYNC_DEL(rgbdScan2d);
|
||||
SYNC_DEL(rgbdScan3d);
|
||||
SYNC_DEL(rgbdInfo);
|
||||
SYNC_DEL(rgbdScan2dInfo);
|
||||
SYNC_DEL(rgbdScan3dInfo);
|
||||
|
||||
// 1 RGBD + Odom
|
||||
SYNC_DEL(rgbdOdom);
|
||||
SYNC_DEL(rgbdOdomScan2d);
|
||||
SYNC_DEL(rgbdOdomScan3d);
|
||||
SYNC_DEL(rgbdOdomInfo);
|
||||
SYNC_DEL(rgbdOdomScan2dInfo);
|
||||
SYNC_DEL(rgbdOdomScan3dInfo);
|
||||
|
||||
// 1 RGBD + User Data
|
||||
SYNC_DEL(rgbdData);
|
||||
SYNC_DEL(rgbdDataScan2d);
|
||||
SYNC_DEL(rgbdDataScan3d);
|
||||
SYNC_DEL(rgbdDataInfo);
|
||||
SYNC_DEL(rgbdDataScan2dInfo);
|
||||
SYNC_DEL(rgbdDataScan3dInfo);
|
||||
|
||||
// 1 RGBD + Odom + User Data
|
||||
SYNC_DEL(rgbdOdomData);
|
||||
SYNC_DEL(rgbdOdomDataScan2d);
|
||||
SYNC_DEL(rgbdOdomDataScan3d);
|
||||
SYNC_DEL(rgbdOdomDataInfo);
|
||||
SYNC_DEL(rgbdOdomDataScan2dInfo);
|
||||
SYNC_DEL(rgbdOdomDataScan3dInfo);
|
||||
|
||||
// 2 RGBD
|
||||
SYNC_DEL(rgbd2);
|
||||
SYNC_DEL(rgbd2Scan2d);
|
||||
SYNC_DEL(rgbd2Scan3d);
|
||||
SYNC_DEL(rgbd2Info);
|
||||
SYNC_DEL(rgbd2Scan2dInfo);
|
||||
SYNC_DEL(rgbd2Scan3dInfo);
|
||||
|
||||
// 2 RGBD + Odom
|
||||
SYNC_DEL(rgbd2Odom);
|
||||
SYNC_DEL(rgbd2OdomScan2d);
|
||||
SYNC_DEL(rgbd2OdomScan3d);
|
||||
SYNC_DEL(rgbd2OdomInfo);
|
||||
SYNC_DEL(rgbd2OdomScan2dInfo);
|
||||
SYNC_DEL(rgbd2OdomScan3dInfo);
|
||||
|
||||
// 2 RGBD + User Data
|
||||
SYNC_DEL(rgbd2Data);
|
||||
SYNC_DEL(rgbd2DataScan2d);
|
||||
SYNC_DEL(rgbd2DataScan3d);
|
||||
SYNC_DEL(rgbd2DataInfo);
|
||||
SYNC_DEL(rgbd2DataScan2dInfo);
|
||||
SYNC_DEL(rgbd2DataScan3dInfo);
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
SYNC_DEL(rgbd2OdomData);
|
||||
SYNC_DEL(rgbd2OdomDataScan2d);
|
||||
SYNC_DEL(rgbd2OdomDataScan3d);
|
||||
SYNC_DEL(rgbd2OdomDataInfo);
|
||||
SYNC_DEL(rgbd2OdomDataScan2dInfo);
|
||||
SYNC_DEL(rgbd2OdomDataScan3dInfo);
|
||||
|
||||
for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
|
||||
{
|
||||
|
||||
@@ -78,6 +78,30 @@ void CommonDataSubscriber::depthInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom
|
||||
void CommonDataSubscriber::depthOdomCallback(
|
||||
@@ -128,6 +152,30 @@ void CommonDataSubscriber::depthOdomInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + User Data
|
||||
void CommonDataSubscriber::depthDataCallback(
|
||||
@@ -178,6 +226,30 @@ void CommonDataSubscriber::depthDataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
nav_msgs::OdometryConstPtr odomMsg; // null
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// RGB + Depth + Odom + User Data
|
||||
void CommonDataSubscriber::depthOdomDataCallback(
|
||||
@@ -228,6 +300,30 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *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)
|
||||
{
|
||||
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, scan2dMsg, scanMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupDepthCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
@@ -266,13 +362,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL7(depthOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(depthOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL7(depthOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(depthOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -293,13 +407,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(depthOdomScan2dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(depthOdomScan2d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(depthOdomScan3dInfo, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(depthOdomScan3d, approxSync, queueSize, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -320,13 +452,32 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(depthDataScan2dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(depthDataScan2d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(depthDataScan3dInfo, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(depthDataScan3d, approxSync, queueSize, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -345,13 +496,31 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(depthScan2dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(depthScan2d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(depthScan3dInfo, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(depthScan3d, approxSync, queueSize, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
|
||||
@@ -85,6 +85,32 @@ void CommonDataSubscriber::rgbdInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan2dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::LaserScanConstPtr& scanMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& 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->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
void CommonDataSubscriber::rgbdScan3dInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& 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->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom
|
||||
void CommonDataSubscriber::rgbdOdomCallback(
|
||||
@@ -139,6 +165,32 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg, rgb, depth, image1Msg->cameraInfo, 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)
|
||||
{
|
||||
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->cameraInfo, 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)
|
||||
{
|
||||
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->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + User Data
|
||||
void CommonDataSubscriber::rgbdDataCallback(
|
||||
@@ -193,6 +245,32 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, 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)
|
||||
{
|
||||
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->cameraInfo, 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)
|
||||
{
|
||||
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->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 1 RGBD camera + Odom + User Data
|
||||
void CommonDataSubscriber::rgbdOdomDataCallback(
|
||||
@@ -247,6 +325,32 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, 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)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, 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)
|
||||
{
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_ros::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonSingleDepthCallback(odomMsg, userDataMsg,rgb, depth, image1Msg->cameraInfo, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
ros::NodeHandle & nh,
|
||||
@@ -276,13 +380,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbdOdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbdOdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbdOdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbdOdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -302,13 +424,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbdOdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbdOdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbdOdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbdOdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -328,13 +468,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbdDataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbdDataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbdDataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbdDataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -353,13 +511,31 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL3(rgbdScan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(rgbdScan2d, approxSync, queueSize, (*rgbdSubs_[0]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL3(rgbdScan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL2(rgbdScan3d, approxSync, queueSize, (*rgbdSubs_[0]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
|
||||
@@ -95,6 +95,32 @@ void CommonDataSubscriber::rgbd2InfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd2OdomCallback(
|
||||
@@ -149,6 +175,32 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + User Data
|
||||
void CommonDataSubscriber::rgbd2DataCallback(
|
||||
@@ -203,6 +255,32 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd2OdomDataCallback(
|
||||
@@ -257,6 +335,32 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2ConstPtr 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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScanConstPtr scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
|
||||
void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
ros::NodeHandle & nh,
|
||||
@@ -285,13 +389,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(rgbd2OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(rgbd2OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL6(rgbd2OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(rgbd2OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -311,13 +433,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbd2OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbd2OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbd2OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbd2OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -337,13 +477,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbd2DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbd2DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL5(rgbd2DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL4(rgbd2DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -362,13 +520,31 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(nh, "scan", 1);
|
||||
SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbd2Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbd2Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scanSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", 1);
|
||||
SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", 1);
|
||||
SYNC_DECL4(rgbd2Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL3(rgbd2Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), scan3dSub_);
|
||||
}
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user