void CommonDataSubscriber::commonSingleDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const cv_bridge::CvImageConstPtr & imageMsg,
const cv_bridge::CvImageConstPtr & depthMsg,
const sensor_msgs::CameraInfo & cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
callbackCalled();
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
if(imageMsg.get())
{
imageMsgs.push_back(imageMsg);
}
if(depthMsg.get())
{
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(cameraInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::commonSingleDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
const cv_bridge::CvImageConstPtr & imageMsg,
const cv_bridge::CvImageConstPtr & depthMsg,
const sensor_msgs::CameraInfo & rgbCameraInfoMsg,
const sensor_msgs::CameraInfo & depthCameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
callbackCalled();
if(depthMsg.get() == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
if(imageMsg.get())
{
imageMsgs.push_back(imageMsg);
}
if(depthMsg.get())
{
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(rgbCameraInfoMsg);
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg);
}
else // assuming stereo
{
commonStereoCallback(odomMsg, userDataMsg, imageMsg, depthMsg, rgbCameraInfoMsg, depthCameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
}
CommonDataSubscriber::~CommonDataSubscriber()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}
// RGB + Depth
SYNC_DEL(depth);
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);
SYNC_DEL(stereoInfo);
// Stereo + Odom
SYNC_DEL(stereoOdom);
SYNC_DEL(stereoOdomInfo);
// RGB-only
SYNC_DEL(rgb);
SYNC_DEL(rgbScan2d);
SYNC_DEL(rgbScan3d);
SYNC_DEL(rgbInfo);
SYNC_DEL(rgbScan2dInfo);
SYNC_DEL(rgbScan3dInfo);
// RGB-only + Odom
SYNC_DEL(rgbOdom);
SYNC_DEL(rgbOdomScan2d);
SYNC_DEL(rgbOdomScan3d);
SYNC_DEL(rgbOdomInfo);
SYNC_DEL(rgbOdomScan2dInfo);
SYNC_DEL(rgbOdomScan3dInfo);
// RGB-only + User Data
SYNC_DEL(rgbData);
SYNC_DEL(rgbDataScan2d);
SYNC_DEL(rgbDataScan3d);
SYNC_DEL(rgbDataInfo);
SYNC_DEL(rgbDataScan2dInfo);
SYNC_DEL(rgbDataScan3dInfo);
// RGB-only + Odom + User Data
SYNC_DEL(rgbOdomData);
SYNC_DEL(rgbOdomDataScan2d);
SYNC_DEL(rgbOdomDataScan3d);
SYNC_DEL(rgbOdomDataInfo);
SYNC_DEL(rgbOdomDataScan2dInfo);
SYNC_DEL(rgbOdomDataScan3dInfo);
// 1 RGBD
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);
// 3 RGBD
SYNC_DEL(rgbd3);
SYNC_DEL(rgbd3Scan2d);
SYNC_DEL(rgbd3Scan3d);
SYNC_DEL(rgbd3Info);
SYNC_DEL(rgbd3Scan2dInfo);
SYNC_DEL(rgbd3Scan3dInfo);
// 3 RGBD + Odom
SYNC_DEL(rgbd3Odom);
SYNC_DEL(rgbd3OdomScan2d);
SYNC_DEL(rgbd3OdomScan3d);
SYNC_DEL(rgbd3OdomInfo);
SYNC_DEL(rgbd3OdomScan2dInfo);
SYNC_DEL(rgbd3OdomScan3dInfo);
// 3 RGBD + User Data
SYNC_DEL(rgbd3Data);
SYNC_DEL(rgbd3DataScan2d);
SYNC_DEL(rgbd3DataScan3d);
SYNC_DEL(rgbd3DataInfo);
SYNC_DEL(rgbd3DataScan2dInfo);
SYNC_DEL(rgbd3DataScan3dInfo);
// 3 RGBD + Odom + User Data
SYNC_DEL(rgbd3OdomData);
SYNC_DEL(rgbd3OdomDataScan2d);
SYNC_DEL(rgbd3OdomDataScan3d);
SYNC_DEL(rgbd3OdomDataInfo);
SYNC_DEL(rgbd3OdomDataScan2dInfo);
SYNC_DEL(rgbd3OdomDataScan3dInfo);
// 4 RGBD
SYNC_DEL(rgbd4);
SYNC_DEL(rgbd4Scan2d);
SYNC_DEL(rgbd4Scan3d);
SYNC_DEL(rgbd4Info);
SYNC_DEL(rgbd4Scan2dInfo);
SYNC_DEL(rgbd4Scan3dInfo);
// 4 RGBD + Odom
SYNC_DEL(rgbd4Odom);
SYNC_DEL(rgbd4OdomScan2d);
SYNC_DEL(rgbd4OdomScan3d);
SYNC_DEL(rgbd4OdomInfo);
SYNC_DEL(rgbd4OdomScan2dInfo);
SYNC_DEL(rgbd4OdomScan3dInfo);
// 4 RGBD + User Data
SYNC_DEL(rgbd4Data);
SYNC_DEL(rgbd4DataScan2d);
SYNC_DEL(rgbd4DataScan3d);
SYNC_DEL(rgbd4DataInfo);
SYNC_DEL(rgbd4DataScan2dInfo);
SYNC_DEL(rgbd4DataScan3dInfo);
// 4 RGBD + Odom + User Data
SYNC_DEL(rgbd4OdomData);
SYNC_DEL(rgbd4OdomDataScan2d);
SYNC_DEL(rgbd4OdomDataScan3d);
SYNC_DEL(rgbd4OdomDataInfo);
SYNC_DEL(rgbd4OdomDataScan2dInfo);
SYNC_DEL(rgbd4OdomDataScan3dInfo);
// Scan
SYNC_DEL(scan2dInfo);
SYNC_DEL(scan3dInfo);
// Scan + Odom
SYNC_DEL(odomScan2d);
SYNC_DEL(odomScan3d);
SYNC_DEL(odomScan2dInfo);
SYNC_DEL(odomScan3dInfo);
// Scan + User Data
SYNC_DEL(dataScan2d);
SYNC_DEL(dataScan3d);
SYNC_DEL(dataScan2dInfo);
SYNC_DEL(dataScan3dInfo);
// Scan + Odom + User Data
SYNC_DEL(odomDataScan2d);
SYNC_DEL(odomDataScan3d);
SYNC_DEL(odomDataScan2dInfo);
SYNC_DEL(odomDataScan3dInfo);
// Odom
SYNC_DEL(odomInfo);
// Odom + User Data
SYNC_DEL(odomData);
SYNC_DEL(odomDataInfo);
for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
{
delete rgbdSubs_[i];
}
rgbdSubs_.clear();
//clear params
ros::NodeHandle pnh("~");
pnh.deleteParam("subscribe_depth");
pnh.deleteParam("subscribe_laserScan");
pnh.deleteParam("subscribe_scan");
pnh.deleteParam("subscribe_scan_cloud");
pnh.deleteParam("subscribe_stereo");
pnh.deleteParam("subscribe_rgb");
pnh.deleteParam("subscribe_rgbd");
pnh.deleteParam("subscribe_odom_info");
pnh.deleteParam("subscribe_user_data");
pnh.deleteParam("odom_frame_id");
pnh.deleteParam("rgbd_cameras");
pnh.deleteParam("depth_cameras");
pnh.deleteParam("queue_size");
pnh.deleteParam("approx_sync");
pnh.deleteParam("stereo_approx_sync");
}
CommonDataSubscriber::~CommonDataSubscriber()
{
if(warningThread_)
{
callbackCalled();
warningThread_->join();
delete warningThread_;
}
// RGB + Depth
SYNC_DEL(depth);
SYNC_DEL(depthScan2d);
SYNC_DEL(depthScan3d);
SYNC_DEL(depthInfo);
// RGB + Depth + Odom
SYNC_DEL(depthOdom);
SYNC_DEL(depthOdomScan2d);
SYNC_DEL(depthOdomScan3d);
SYNC_DEL(depthOdomInfo);
// RGB + Depth + User Data
SYNC_DEL(depthData);
SYNC_DEL(depthDataScan2d);
SYNC_DEL(depthDataScan3d);
SYNC_DEL(depthDataInfo);
// RGB + Depth + Odom + User Data
SYNC_DEL(depthOdomData);
SYNC_DEL(depthOdomDataScan2d);
SYNC_DEL(depthOdomDataScan3d);
SYNC_DEL(depthOdomDataInfo);
// Stereo
SYNC_DEL(stereo);
SYNC_DEL(stereoInfo);
// Stereo + Odom
SYNC_DEL(stereoOdom);
SYNC_DEL(stereoOdomInfo);
// 1 RGBD
SYNC_DEL(rgbdScan2d);
SYNC_DEL(rgbdScan3d);
SYNC_DEL(rgbdInfo);
// 1 RGBD + Odom
SYNC_DEL(rgbdOdom);
SYNC_DEL(rgbdOdomScan2d);
SYNC_DEL(rgbdOdomScan3d);
SYNC_DEL(rgbdOdomInfo);
// 1 RGBD + User Data
SYNC_DEL(rgbdData);
SYNC_DEL(rgbdDataScan2d);
SYNC_DEL(rgbdDataScan3d);
SYNC_DEL(rgbdDataInfo);
// 1 RGBD + Odom + User Data
SYNC_DEL(rgbdOdomData);
SYNC_DEL(rgbdOdomDataScan2d);
SYNC_DEL(rgbdOdomDataScan3d);
SYNC_DEL(rgbdOdomDataInfo);
// 2 RGBD
SYNC_DEL(rgbd2);
SYNC_DEL(rgbd2Scan2d);
SYNC_DEL(rgbd2Scan3d);
SYNC_DEL(rgbd2Info);
// 2 RGBD + Odom
SYNC_DEL(rgbd2Odom);
SYNC_DEL(rgbd2OdomScan2d);
SYNC_DEL(rgbd2OdomScan3d);
SYNC_DEL(rgbd2OdomInfo);
// 2 RGBD + User Data
SYNC_DEL(rgbd2Data);
SYNC_DEL(rgbd2DataScan2d);
SYNC_DEL(rgbd2DataScan3d);
SYNC_DEL(rgbd2DataInfo);
// 2 RGBD + Odom + User Data
SYNC_DEL(rgbd2OdomData);
SYNC_DEL(rgbd2OdomDataScan2d);
SYNC_DEL(rgbd2OdomDataScan3d);
SYNC_DEL(rgbd2OdomDataInfo);
for(unsigned int i=0; i<rgbdSubs_.size(); ++i)
{
delete rgbdSubs_[i];
}
rgbdSubs_.clear();
}