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);
}
// 2 RGBD + Odom
void CommonDataSubscriber::rgbd2OdomCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::RGBDImageConstPtr& image1Msg,
const rtabmap_ros::RGBDImageConstPtr& image2Msg)
{
IMAGE_CONVERSION();
rtabmap_ros::UserDataConstPtr userDataMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
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);
}
}
