mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-09 11:17:03 +08:00
Added rgb_sync nodelet. Added sync rgbd5 and rgbd6 callbacks. Removed OdomInfo from sync callbacks having both camera and lidar. Added BAYER_RGGB8 support. point_cloud_assembler: added output frame_id option.
This commit is contained in:
matlabbe
@@ -41,6 +41,8 @@ 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]); \
|
||||
if(!depthMsgs[0].get()) \
|
||||
depthMsgs.clear(); \
|
||||
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs; \
|
||||
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
|
||||
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
|
||||
@@ -150,54 +152,6 @@ void CommonDataSubscriber::rgbd4InfoCallback(
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScan scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4ScanDescInfoCallback(
|
||||
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image2Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image3Msg,
|
||||
const rtabmap_ros::RGBDImageConstPtr& image4Msg,
|
||||
const rtabmap_ros::ScanDescriptorConstPtr& scanDescMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom
|
||||
void CommonDataSubscriber::rgbd4OdomCallback(
|
||||
@@ -278,54 +232,6 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
sensor_msgs::LaserScan scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomScanDescInfoCallback(
|
||||
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::ScanDescriptorConstPtr& scanDescMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
// 2 RGBD + User Data
|
||||
@@ -407,54 +313,6 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
sensor_msgs::LaserScan scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4DataScanDescInfoCallback(
|
||||
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::ScanDescriptorConstPtr& scanDescMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
nav_msgs::OdometryConstPtr odomMsg; // Null
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
|
||||
// 2 RGBD + Odom + User Data
|
||||
void CommonDataSubscriber::rgbd4OdomDataCallback(
|
||||
@@ -535,54 +393,6 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::PointCloud2 scan3dMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
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)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
sensor_msgs::LaserScan scanMsg; // Null
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
void CommonDataSubscriber::rgbd4OdomDataScanDescInfoCallback(
|
||||
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::ScanDescriptorConstPtr& scanDescMsg,
|
||||
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
|
||||
{
|
||||
IMAGE_CONVERSION();
|
||||
|
||||
if(!scanDescMsg->global_descriptor.data.empty())
|
||||
{
|
||||
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
|
||||
}
|
||||
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
|
||||
}
|
||||
#endif
|
||||
|
||||
void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
@@ -616,14 +426,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL8(rgbd4OdomDataScanDescInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(rgbd4OdomDataScanDesc, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
@@ -631,14 +437,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanSub_.subscribe(nh, "scan", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL8(rgbd4OdomDataScan2dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(rgbd4OdomDataScan2d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
@@ -646,14 +448,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL8(rgbd4OdomDataScan3dInfo, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL7(rgbd4OdomDataScan3d, approxSync, queueSize, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -677,14 +475,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4OdomScanDescInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4OdomScanDesc, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
@@ -692,14 +486,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanSub_.subscribe(nh, "scan", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4OdomScan2dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4OdomScan2d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
@@ -707,14 +497,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4OdomScan3dInfo, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4OdomScan3d, approxSync, queueSize, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -737,14 +523,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4DataScanDescInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4DataScanDesc, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
@@ -752,14 +534,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanSub_.subscribe(nh, "scan", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4DataScan2dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4DataScan2d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
@@ -767,14 +545,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL7(rgbd4DataScan3dInfo, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL6(rgbd4DataScan3d, approxSync, queueSize, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
@@ -796,14 +570,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanDescSub_.subscribe(nh, "scan_descriptor", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL6(rgbd4ScanDescInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(rgbd4ScanDesc, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanDescSub_);
|
||||
}
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
@@ -811,14 +581,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scanSub_.subscribe(nh, "scan", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL6(rgbd4Scan2dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(rgbd4Scan2d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scanSub_);
|
||||
}
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
@@ -826,14 +592,10 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
scan3dSub_.subscribe(nh, "scan_cloud", queueSize);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(nh, "odom_info", queueSize);
|
||||
SYNC_DECL6(rgbd4Scan3dInfo, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
{
|
||||
SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
subscribedToOdomInfo_ = false;
|
||||
ROS_WARN("subscribe_odom_info ignored...");
|
||||
}
|
||||
SYNC_DECL5(rgbd4Scan3d, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), scan3dSub_);
|
||||
}
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user