i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 10:47:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2022-10-01 18:09:19 -07:00
parent 4fb73b4279 5024574e4a
commit 99b9891ab8
57 changed files with 1988 additions and 1096 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CommonDataSubscriber.cpp
+25 -45
View File
@@ -991,7 +991,7 @@ CommonDataSubscriber::~CommonDataSubscriber()
rgbdSubs_.clear();
}
void CommonDataSubscriber::commonSingleDepthCallback(
void CommonDataSubscriber::commonSingleCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const cv_bridge::CvImageConstPtr & imageMsg,
@@ -1015,54 +1015,34 @@ void CommonDataSubscriber::commonSingleDepthCallback(
std::vector<cv::Mat> localDescriptorsMsgs;
localDescriptorsMsgs.push_back(localDescriptors);
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::msg::CameraInfo> cameraInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs;
if(imageMsg.get())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::msg::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,
globalDescriptorMsgs,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs);
imageMsgs.push_back(imageMsg);
}
else // assuming stereo
if(depthMsg.get())
{
commonStereoCallback(
odomMsg,
userDataMsg,
imageMsg,
depthMsg,
rgbCameraInfoMsg,
depthCameraInfoMsg,
scanMsg,
scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs,
localKeyPoints,
localPoints3d,
localDescriptors);
depthMsgs.push_back(depthMsg);
}
cameraInfoMsgs.push_back(rgbCameraInfoMsg);
depthCameraInfoMsgs.push_back(depthCameraInfoMsg);
commonMultiCameraCallback(
odomMsg,
userDataMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
scanMsg,
scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs);
}
} /* namespace rtabmap_ros */
src/CoreWrapper.cpp
+137 -279
View File
@@ -468,18 +468,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
int estimationType = Parameters::defaultVisEstimationType();
Parameters::parse(parameters_, Parameters::kVisEstimationType(), estimationType);
int cameras = this->rgbdCameras();
bool subscribeRGBD = this->isSubscribedToRGBD();
if(subscribeRGBD && cameras> 1 && estimationType>0)
{
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 0 (%d is not supported "
"for multi-cameras) as \"subscribe_rgbd\" is "
"true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
Parameters::kVisEstimationType().c_str(),
estimationType,
Parameters::kVisEstimationType().c_str());
uInsert(parameters_, ParametersPair(Parameters::kVisEstimationType(), "0"));
}
// modify default parameters with those in the database
if(!deleteDbOnStart)
@@ -1002,6 +990,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti
lastPoseIntermediate_ = false;
lastPose_ = odom;
lastPoseStamp_ = stamp;
lastPoseVelocity_.resize(6);
lastPoseVelocity_[0] = odomMsg.twist.twist.linear.x;
lastPoseVelocity_[1] = odomMsg.twist.twist.linear.y;
lastPoseVelocity_[2] = odomMsg.twist.twist.linear.z;
lastPoseVelocity_[3] = odomMsg.twist.twist.angular.x;
lastPoseVelocity_[4] = odomMsg.twist.twist.angular.y;
lastPoseVelocity_[5] = odomMsg.twist.twist.angular.z;
// Only update variance if odom is not null
if(!odom.isNull())
@@ -1087,6 +1082,7 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp)
lastPoseIntermediate_ = false;
lastPose_ = odom;
lastPoseStamp_ = stamp;
lastPoseVelocity_.clear();
bool ignoreFrame = false;
if(stamp.seconds() == 0.0)
@@ -1122,12 +1118,13 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp)
return false;
}
void CoreWrapper::commonDepthCallback(
void CoreWrapper::commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -1178,11 +1175,12 @@ void CoreWrapper::commonDepthCallback(
return;
}
commonDepthCallbackImpl(odomFrameId,
commonMultiCameraCallbackImpl(odomFrameId,
userDataMsg,
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
scan2dMsg,
scan3dMsg,
odomInfoMsg,
@@ -1192,12 +1190,13 @@ void CoreWrapper::commonDepthCallback(
localDescriptors);
}
void CoreWrapper::commonDepthCallbackImpl(
void CoreWrapper::commonMultiCameraCallbackImpl(
const std::string & odomFrameId,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -1210,21 +1209,26 @@ void CoreWrapper::commonDepthCallbackImpl(
cv::Mat rgb;
cv::Mat depth;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
cv::Mat descriptors;
if(!rtabmap_ros::convertRGBDMsgs(
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
rgb,
depth,
cameraModels,
stereoCameraModels,
*tfBuffer_,
waitForTransform_,
alreadyRectifiedImages_,
localKeyPointsMsgs,
localPoints3dMsgs,
localDescriptorsMsgs,
@@ -1235,6 +1239,73 @@ void CoreWrapper::commonDepthCallbackImpl(
RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmap update...");
return;
}
UDEBUG("cameraModels=%ld stereoCameraModels=%ld", cameraModels.size(), stereoCameraModels.size());
UDEBUG("rgb=%dx%d(type=%d), depth/right=%dx%d(type=%d)", rgb.rows, rgb.cols, rgb.type(), depth.rows, depth.cols, depth.type());
if(stereoCameraModels.size() && stereoToDepth_)
{
UASSERT(depth.type() == CV_8UC1);
cv::Mat leftMono;
if(rgb.channels() == 3)
{
cv::cvtColor(rgb, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = rgb;
}
cv::Mat rightMono = depth;
depth = cv::Mat();
UASSERT(int((leftMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == leftMono.cols);
UASSERT(int((rightMono.cols/stereoCameraModels.size())*stereoCameraModels.size()) == rightMono.cols);
int subImageWidth = leftMono.cols/stereoCameraModels.size();
for(size_t i=0; i<stereoCameraModels.size(); ++i)
{
cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
cv::Mat right(rightMono, cv::Rect(subImageWidth*i, 0, subImageWidth, rightMono.rows));
// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
left,
right,
parameters_);
if(disparity.empty())
{
RCLCPP_ERROR(this->get_logger(), "Could not compute disparity image (\"stereo_to_depth\" is true)!");
return;
}
cv::Mat subDepth = rtabmap::util2d::depthFromDisparity(
disparity,
stereoCameraModels[i].left().fx(),
stereoCameraModels[i].baseline());
if(subDepth.empty())
{
RCLCPP_ERROR(this->get_logger(), "Could not compute depth image (\"stereo_to_depth\" is true)!");
return;
}
UASSERT(subDepth.type() == CV_16UC1 || subDepth.type() == CV_32FC1);
if(depth.empty())
{
depth = cv::Mat(subDepth.rows, subDepth.cols*stereoCameraModels.size(), subDepth.type());
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*subDepth.cols, 0, subDepth.cols, subDepth.rows)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type!");
return;
}
cameraModels.push_back(stereoCameraModels[i].left());
}
stereoCameraModels.clear();
}
UASSERT(uContains(parameters_, rtabmap::Parameters::kMemSaveDepth16Format()));
if(!depth.empty() && depth.type() == CV_32FC1 && uStr2Bool(parameters_.at(Parameters::kMemSaveDepth16Format())))
@@ -1253,7 +1324,7 @@ void CoreWrapper::commonDepthCallbackImpl(
LaserScan scan;
bool genMaxScanPts = 0;
if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && genScan_)
if(scan2dMsg.ranges.empty() && scan3dMsg.data.empty() && !depth.empty() && stereoCameraModels.empty() && genScan_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud2d(new pcl::PointCloud<pcl::PointXYZ>);
*scanCloud2d = util3d::laserScanFromDepthImages(
@@ -1364,14 +1435,29 @@ void CoreWrapper::commonDepthCallbackImpl(
userData_ = cv::Mat();
}
SensorData data(
scan,
rgb,
depth,
cameraModels,
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
SensorData data;
if(!stereoCameraModels.empty())
{
data = SensorData(
scan,
rgb,
depth,
stereoCameraModels,
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
}
else
{
data = SensorData(
scan,
rgb,
depth,
cameraModels,
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
@@ -1394,6 +1480,7 @@ void CoreWrapper::commonDepthCallbackImpl(
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
@@ -1401,255 +1488,6 @@ void CoreWrapper::commonDepthCallbackImpl(
covariance_ = cv::Mat();
}
void CoreWrapper::commonStereoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
const cv_bridge::CvImageConstPtr& leftImageMsg,
const cv_bridge::CvImageConstPtr& rightImageMsg,
const sensor_msgs::msg::CameraInfo& leftCamInfoMsg,
const sensor_msgs::msg::CameraInfo& rightCamInfoMsg,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
const std::vector<rtabmap_ros::msg::GlobalDescriptor> & globalDescriptorMsgs,
const std::vector<rtabmap_ros::msg::KeyPoint> & localKeyPointsMsg,
const std::vector<rtabmap_ros::msg::Point3f> & localPoints3dMsg,
const cv::Mat & localDescriptorsMsg)
{
UTimer timerConversion;
std::string odomFrameId = odomFrameId_;
if(odomMsg.get())
{
odomFrameId = odomMsg->header.frame_id;
if(!scan2dMsg.ranges.empty())
{
if(!odomUpdate(*odomMsg, scan2dMsg.header.stamp))
{
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!odomUpdate(*odomMsg, scan3dMsg.header.stamp))
{
return;
}
}
else if(leftImageMsg.get() == 0 || !odomUpdate(*odomMsg, leftImageMsg->header.stamp))
{
return;
}
}
else if(!scan2dMsg.ranges.empty())
{
if(!odomTFUpdate(scan2dMsg.header.stamp))
{
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!odomTFUpdate(scan3dMsg.header.stamp))
{
return;
}
}
else if(leftImageMsg.get() == 0 || !odomTFUpdate(leftImageMsg->header.stamp))
{
return;
}
cv::Mat left;
cv::Mat right;
StereoCameraModel stereoModel;
if(!rtabmap_ros::convertStereoMsg(
leftImageMsg,
rightImageMsg,
leftCamInfoMsg,
rightCamInfoMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
left,
right,
stereoModel,
*tfBuffer_,
waitForTransform_,
alreadyRectifiedImages_))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert stereo msgs! Aborting rtabmap update...");
return;
}
if(stereoToDepth_)
{
// cv::stereoBM() see "$ rosrun rtabmap_ros rtabmap --params | grep StereoBM" for parameters
cv::Mat disparity = rtabmap::util2d::disparityFromStereoImages(
left,
right,
parameters_);
if(disparity.empty())
{
RCLCPP_ERROR(this->get_logger(), "Could not compute disparity image (\"stereo_to_depth\" is true)!");
return;
}
cv::Mat depth = rtabmap::util2d::depthFromDisparity(
disparity,
stereoModel.left().fx(),
stereoModel.baseline());
if(depth.empty())
{
RCLCPP_ERROR(this->get_logger(), "Could not compute depth image (\"stereo_to_depth\" is true)!");
return;
}
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
// move to common depth callback
cv_bridge::CvImagePtr imgDepth(new cv_bridge::CvImage);
if(depth.type() == CV_16UC1)
{
imgDepth->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
else // CV_32FC1
{
imgDepth->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
imgDepth->image = depth;
imgDepth->header = leftImageMsg->header;
std::vector<cv_bridge::CvImageConstPtr> rgbImages(1);
std::vector<cv_bridge::CvImageConstPtr> depthImages(1);
std::vector<sensor_msgs::msg::CameraInfo> cameraInfos(1);
rgbImages[0] = leftImageMsg;
depthImages[0] = imgDepth;
cameraInfos[0] = leftCamInfoMsg;
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPointsMsgs;
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3dMsgs;
std::vector<cv::Mat> localDescriptorsMsgs;
if(!localKeyPointsMsg.empty())
{
localKeyPointsMsgs.push_back(localKeyPointsMsg);
}
if(!localPoints3dMsg.empty())
{
localPoints3dMsgs.push_back(localPoints3dMsg);
}
if(!localDescriptorsMsg.empty())
{
localDescriptorsMsgs.push_back(localDescriptorsMsg);
}
commonDepthCallbackImpl(odomFrameId,
rtabmap_ros::msg::UserData::SharedPtr(),
rgbImages, depthImages, cameraInfos,
scan2dMsg, scan3dMsg,
odomInfoMsg,
globalDescriptorMsgs, localKeyPointsMsgs, localPoints3dMsgs, localDescriptorsMsgs);
return;
}
LaserScan scan;
if(!scan2dMsg.ranges.empty())
{
if(!rtabmap_ros::convertScanMsg(
scan2dMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
scan,
*tfBuffer_,
waitForTransform_,
// backward compatibility, project 2D scan in /base_link frame
rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update...");
return;
}
}
else if(!scan3dMsg.data.empty())
{
if(!rtabmap_ros::convertScan3dMsg(
scan3dMsg,
frameId_,
odomSensorSync_?odomFrameId:"",
lastPoseStamp_,
scan,
*tfBuffer_,
waitForTransform_,
scanCloudMaxPoints_))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
return;
}
}
cv::Mat userData;
if(userDataMsg.get())
{
userData = rtabmap_ros::userDataFromROS(*userDataMsg);
UScopeMutex lock(userDataMutex_);
if(!userData_.empty())
{
RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
userData_ = cv::Mat();
}
}
else
{
UScopeMutex lock(userDataMutex_);
userData = userData_;
userData_ = cv::Mat();
}
SensorData data(
scan,
left,
right,
stereoModel,
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
OdometryInfo odomInfo;
if(odomInfoMsg.get())
{
odomInfo = odomInfoFromROS(*odomInfoMsg);
}
if(!globalDescriptorMsgs.empty())
{
data.setGlobalDescriptors(rtabmap_ros::globalDescriptorsFromROS(globalDescriptorMsgs));
}
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> points;
if(!localKeyPointsMsg.empty())
{
keypoints = rtabmap_ros::keypointsFromROS(localKeyPointsMsg);
}
if(!localPoints3dMsg.empty())
{
// Points should be in base frame
points = rtabmap_ros::points3fFromROS(localPoints3dMsg, stereoModel.localTransform());
}
if(!keypoints.empty())
{
UASSERT(points.empty() || points.size() == keypoints.size());
UASSERT(localDescriptorsMsg.empty() || localDescriptorsMsg.rows == (int)keypoints.size());
data.setFeatures(keypoints, points, localDescriptorsMsg);
}
process(lastPoseStamp_,
data,
lastPose_,
odomFrameId,
covariance_,
odomInfo,
timerConversion.ticks());
covariance_ = cv::Mat();
}
void CoreWrapper::commonLaserScanCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -1758,7 +1596,7 @@ void CoreWrapper::commonLaserScanCallback(
scan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1777,6 +1615,7 @@ void CoreWrapper::commonLaserScanCallback(
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
@@ -1821,7 +1660,7 @@ void CoreWrapper::commonOdomCallback(
SensorData data(
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
lastPoseIntermediate_?-1:0,
rtabmap_ros::timestampFromROS(lastPoseStamp_),
userData);
@@ -1835,6 +1674,7 @@ void CoreWrapper::commonOdomCallback(
process(lastPoseStamp_,
data,
lastPose_,
lastPoseVelocity_,
odomFrameId,
covariance_,
odomInfo,
@@ -1847,6 +1687,7 @@ void CoreWrapper::process(
const rclcpp::Time & stamp,
SensorData & data,
const Transform & odom,
const std::vector<float> & odomVelocityIn,
const std::string & odomFrameId,
const cv::Mat & odomCovariance,
const OdometryInfo & odomInfo,
@@ -1904,7 +1745,7 @@ void CoreWrapper::process(
covariance.at<double>(4,4) = uIsFinite(covariance.at<double>(4,4)) && covariance.at<double>(4,4)!=0?covariance.at<double>(4,4):1;
}
SensorData interData(cv::Mat(), cv::Mat(), CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
SensorData interData(cv::Mat(), cv::Mat(), rtabmap::CameraModel(), -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
Transform gt;
if(!groundTruthFrameId_.empty())
{
@@ -1932,6 +1773,16 @@ void CoreWrapper::process(
odomVelocity[5] = yaw/info.interval;
}
}
if(odomVelocity.empty())
{
odomVelocity.resize(6);
odomVelocity[0] = iter->first.twist.twist.linear.x;
odomVelocity[1] = iter->first.twist.twist.linear.y;
odomVelocity[2] = iter->first.twist.twist.linear.z;
odomVelocity[3] = iter->first.twist.twist.angular.x;
odomVelocity[4] = iter->first.twist.twist.angular.y;
odomVelocity[5] = iter->first.twist.twist.angular.z;
}
rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
}
@@ -2099,6 +1950,10 @@ void CoreWrapper::process(
odomVelocity[5] = yaw/odomInfo.interval;
}
}
if(odomVelocity.empty())
{
odomVelocity = odomVelocityIn;
}
if(rtabmapROSStats_.size())
{
externalStats.insert(rtabmapROSStats_.begin(), rtabmapROSStats_.end());
@@ -2806,6 +2661,7 @@ void CoreWrapper::resetRtabmapCallback(
rtabmap_.resetMemory();
covariance_ = cv::Mat();
lastPose_.setIdentity();
lastPoseVelocity_.clear();
lastPoseIntermediate_ = false;
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
@@ -2899,6 +2755,7 @@ void CoreWrapper::loadDatabaseCallback(
covariance_ = cv::Mat();
lastPose_.setIdentity();
lastPoseVelocity_.clear();
lastPoseIntermediate_ = false;
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
@@ -3025,6 +2882,7 @@ void CoreWrapper::backupDatabaseCallback(
covariance_ = cv::Mat();
lastPose_.setIdentity();
lastPoseVelocity_.clear();
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
goalFrameId_.clear();
@@ -4112,8 +3970,8 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
int id = rtabmap::graph::findNearestNode(nodesOnly, rtabmap_.getLastLocalizationPose());
if(id>0)
{
std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, false, false, true);
std::map<int, int> missingIds;
std::map<int, int> ids = rtabmap_.getMemory()->getNeighborsId(id, 0, 0, true, false, true);
std::multimap<int, int> missingIds;
for(std::map<int, int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
{
if(nodesToRepublish_.find(iter->first) != nodesToRepublish_.end())
@@ -4140,7 +3998,7 @@ void CoreWrapper::publishStats(const rclcpp::Time & stamp)
int loaded = 0;
std::stringstream stream;
for(std::map<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
for(std::multimap<int, int>::iterator iter=missingIds.begin(); iter!=missingIds.end() && loaded<maxNodesRepublished_; ++iter)
{
signatures.insert(std::make_pair(iter->second, rtabmap_.getMemory()->getNodeData(iter->second, true, true, true, true)));
nodesToRepublish_.erase(iter->second);
src/DbPlayerNode.cpp
+29 -22
View File
@@ -293,30 +293,37 @@ int main(int argc, char** argv)
}
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{
//stereo
if(odom.data().stereoCameraModel().isValidForProjection())
if(odom.data().stereoCameraModels().size() > 1)
{
camInfoA.D.resize(8,0);
camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//stereo
if(odom.data().stereoCameraModels()[0].isValidForProjection())
{
camInfoA.D.resize(8,0);
type=1;
camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
camInfoB = camInfoA;
camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
}
type=1;
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
}
}
else
@@ -383,9 +390,9 @@ int main(int argc, char** argv)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModel().isValidForProjection())
else if(odom.data().stereoCameraModels().size() == 1)
{
localTransform = odom.data().stereoCameraModel().left().localTransform();
localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
}
if(!localTransform.isNull())
{
src/GuiWrapper.cpp
+29 -12
View File
@@ -480,12 +480,13 @@ bool GuiWrapper::handleEvent(UEvent * anEvent)
return false;
}
void GuiWrapper::commonDepthCallback(
void GuiWrapper::commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_ros::msg::UserData::ConstSharedPtr &,
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const sensor_msgs::msg::LaserScan & scan2dMsg,
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
@@ -570,7 +571,8 @@ void GuiWrapper::commonDepthCallback(
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
std::vector<rtabmap::StereoCameraModel> stereoCameraModels;
LaserScan scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
@@ -585,18 +587,25 @@ void GuiWrapper::commonDepthCallback(
if(imageMsgs.size() && imageMsgs[0].get() && depthMsgs.size() && depthMsgs[0].get())
{
ParametersMap allParameters = prefDialog_->getAllParameters();
bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
if(!rtabmap_ros::convertRGBDMsgs(
imageMsgs,
depthMsgs,
cameraInfoMsgs,
depthCameraInfoMsgs,
frameId,
odomSensorSync_?odomHeader.frame_id:"",
odomHeader.stamp,
rgb,
depth,
cameraModels,
stereoCameraModels,
*tfBuffer_,
waitForTransform_))
waitForTransform_,
imagesAlreadyRectified))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert rgb/depth msgs! Aborting rtabmapviz update...");
return;
@@ -653,13 +662,21 @@ void GuiWrapper::commonDepthCallback(
info.reg.covariance = covariance;
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
!stereoCameraModels.empty()?
rtabmap::SensorData(
scan,
rgb,
depth,
stereoCameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)):
rtabmap::SensorData(
scan,
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
info);
@@ -984,7 +1001,7 @@ void GuiWrapper::commonLaserScanCallback(
scan,
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
@@ -1056,7 +1073,7 @@ void GuiWrapper::commonOdomCallback(
rtabmap::SensorData(
cv::Mat(),
cv::Mat(),
CameraModel(),
rtabmap::CameraModel(),
0,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
src/MsgConversion.cpp
+238 -72
View File
@@ -889,7 +889,7 @@ void cameraModelToROS(
memcpy(camInfo.k.data(), model.K_raw().data, 9*sizeof(double));
}
if(camInfo.d.size() == 6)
if(model.D_raw().total() == 6)
{
camInfo.d = std::vector<double>(4);
camInfo.d[0] = model.D_raw().at<double>(0,0);
@@ -1121,27 +1121,30 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
}
}
rtabmap::StereoCameraModel stereoModel;
std::vector<rtabmap::StereoCameraModel> stereoModels;
std::vector<rtabmap::CameraModel> models;
if(msg.baseline > 0.0f)
if(msg.baseline.size())
{
// stereo model
if(msg.fx.size() == 1 &&
msg.fy.size() == 1 &&
msg.cx.size() == 1 &&
msg.cy.size() == 1 &&
msg.width.size() == 1 &&
msg.height.size() == 1 &&
msg.local_transform.size() == 1)
if(msg.fx.size() == msg.baseline.size() &&
msg.fy.size() == msg.baseline.size() &&
msg.cx.size() == msg.baseline.size() &&
msg.cy.size() == msg.baseline.size() &&
msg.width.size() == msg.baseline.size() &&
msg.height.size() == msg.baseline.size() &&
msg.local_transform.size() == msg.baseline.size())
{
stereoModel = rtabmap::StereoCameraModel(
msg.fx[0],
msg.fy[0],
msg.cx[0],
msg.cy[0],
msg.baseline,
transformFromGeometryMsg(msg.local_transform[0]),
cv::Size(msg.width[0], msg.height[0]));
for(unsigned int i=0; i<msg.fx.size(); ++i)
{
stereoModels.push_back(rtabmap::StereoCameraModel(
msg.fx[i],
msg.fy[i],
msg.cx[i],
msg.cy[i],
msg.baseline[i],
transformFromGeometryMsg(msg.local_transform[i]),
cv::Size(msg.width[i], msg.height[i])));
}
}
}
else
@@ -1182,7 +1185,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
msg.label,
transformFromPoseMsg(msg.pose),
transformFromPoseMsg(msg.ground_truth_pose),
stereoModel.isValidForProjection()?
stereoModels.size()?
rtabmap::SensorData(
rtabmap::LaserScan(compressedMatFromBytes(msg.laser_scan),
msg.laser_scan_max_pts,
@@ -1191,7 +1194,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::msg::NodeData & msg)
transformFromGeometryMsg(msg.laser_scan_local_transform)),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
stereoModel,
stereoModels,
msg.id,
msg.stamp,
compressedMatFromBytes(msg.user_data)):
@@ -1248,7 +1251,6 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::msg::NodeD
msg.laser_scan_max_range = signature.sensorData().laserScanCompressed().rangeMax();
msg.laser_scan_format = signature.sensorData().laserScanCompressed().format();
transformToGeometryMsg(signature.sensorData().laserScanCompressed().localTransform(), msg.laser_scan_local_transform);
msg.baseline = 0;
if(signature.sensorData().cameraModels().size())
{
msg.fx.resize(signature.sensorData().cameraModels().size());
@@ -1269,17 +1271,27 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::msg::NodeD
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.local_transform[i]);
}
}
else if(signature.sensorData().stereoCameraModels().size()==1)
else if(signature.sensorData().stereoCameraModels().size())
{
msg.fx.push_back(signature.sensorData().stereoCameraModels()[0].left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModels()[0].left().fy());
msg.cx.push_back(signature.sensorData().stereoCameraModels()[0].left().cx());
msg.cy.push_back(signature.sensorData().stereoCameraModels()[0].left().cy());
msg.width.push_back(signature.sensorData().stereoCameraModels()[0].left().imageWidth());
msg.height.push_back(signature.sensorData().stereoCameraModels()[0].left().imageHeight());
msg.baseline = signature.sensorData().stereoCameraModels()[0].baseline();
msg.local_transform.resize(1);
transformToGeometryMsg(signature.sensorData().stereoCameraModels()[0].left().localTransform(), msg.local_transform[0]);
msg.fx.resize(signature.sensorData().stereoCameraModels().size());
msg.fy.resize(signature.sensorData().stereoCameraModels().size());
msg.cx.resize(signature.sensorData().stereoCameraModels().size());
msg.cy.resize(signature.sensorData().stereoCameraModels().size());
msg.width.resize(signature.sensorData().stereoCameraModels().size());
msg.height.resize(signature.sensorData().stereoCameraModels().size());
msg.baseline.resize(signature.sensorData().stereoCameraModels().size());
msg.local_transform.resize(signature.sensorData().stereoCameraModels().size());
for(unsigned int i=0; i<signature.sensorData().stereoCameraModels().size(); ++i)
{
msg.fx[i] = signature.sensorData().stereoCameraModels()[i].left().fx();
msg.fy[i] = signature.sensorData().stereoCameraModels()[i].left().fy();
msg.cx[i] = signature.sensorData().stereoCameraModels()[i].left().cx();
msg.cy[i] = signature.sensorData().stereoCameraModels()[i].left().cy();
msg.width[i] = signature.sensorData().stereoCameraModels()[i].left().imageWidth();
msg.height[i] = signature.sensorData().stereoCameraModels()[i].left().imageHeight();
msg.baseline[i] = signature.sensorData().stereoCameraModels()[i].baseline();
transformToGeometryMsg(signature.sensorData().stereoCameraModels()[i].left().localTransform(), msg.local_transform[i]);
}
}
//Features stuff...
@@ -1478,12 +1490,14 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::msg::OdomInfo & msg, bo
info.localBundleConstraints = msg.local_bundle_constraints;
info.localBundleTime = msg.local_bundle_time;
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size());
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_model_transforms.size());
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size());
for(size_t i=0; i<msg.local_bundle_ids.size(); ++i)
{
std::vector<rtabmap::CameraModel> models;
models.push_back(cameraModelFromROS(msg.local_bundle_models[i], transformFromGeometryMsg(msg.local_bundle_model_transforms[i])));
for(size_t j=0; j<msg.local_bundle_models[i].models.size(); ++j)
{
models.push_back(cameraModelFromROS(msg.local_bundle_models[i].models[j].camera_info, transformFromGeometryMsg(msg.local_bundle_models[i].models[j].local_transform)));
}
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
}
@@ -1559,20 +1573,22 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::msg::OdomInf
iter!=info.localBundleModels.end();
++iter)
{
if(iter->second.size())
msg.local_bundle_ids.push_back(iter->first);
UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
geometry_msgs::msg::Pose pose;
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
msg.local_bundle_poses.push_back(pose);
rtabmap_ros::msg::CameraModels models;
for(size_t i=0; i<iter->second.size(); ++i)
{
msg.local_bundle_ids.push_back(iter->first);
sensor_msgs::msg::CameraInfo camInfo;
cameraModelToROS(iter->second[0], camInfo);
msg.local_bundle_models.push_back(camInfo);
geometry_msgs::msg::Transform localT;
transformToGeometryMsg(iter->second[0].localTransform(), localT);
msg.local_bundle_model_transforms.push_back(localT);
UASSERT(info.localBundlePoses.find(iter->first)!=info.localBundlePoses.end());
geometry_msgs::msg::Pose pose;
transformToPoseMsg(info.localBundlePoses.at(iter->first), pose);
msg.local_bundle_poses.push_back(pose);
rtabmap_ros::msg::CameraModel modelMsg;
cameraModelToROS(iter->second[i], modelMsg.camera_info);
transformToGeometryMsg(iter->second[i].localTransform(), modelMsg.local_transform);
models.models.push_back(modelMsg);
}
msg.local_bundle_models.push_back(models);
}
msg.key_frame_added = info.keyFrameAdded;
msg.time_estimation = info.timeEstimation;
@@ -1782,14 +1798,17 @@ bool convertRGBDMsgs(
const std::vector<cv_bridge::CvImageConstPtr> & imageMsgs,
const std::vector<cv_bridge::CvImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & cameraInfoMsgs,
const std::vector<sensor_msgs::msg::CameraInfo> & depthCameraInfoMsgs,
const std::string & frameId,
const std::string & odomFrameId,
const rclcpp::Time & odomStamp,
cv::Mat & rgb,
cv::Mat & depth,
std::vector<rtabmap::CameraModel> & cameraModels,
std::vector<rtabmap::StereoCameraModel> & stereoCameraModels,
tf2_ros::Buffer & listener,
double waitForTransform,
bool alreadRectifiedImages,
const std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > & localKeyPointsMsgs,
const std::vector<std::vector<rtabmap_ros::msg::Point3f> > & localPoints3dMsgs,
const std::vector<cv::Mat> & localDescriptorsMsgs,
@@ -1797,16 +1816,41 @@ bool convertRGBDMsgs(
std::vector<cv::Point3f> * localPoints3d,
cv::Mat * localDescriptors)
{
UASSERT(!cameraInfoMsgs.empty()>0 &&
UASSERT(!cameraInfoMsgs.empty() &&
(cameraInfoMsgs.size() == imageMsgs.size() || imageMsgs.empty()) &&
(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()));
(cameraInfoMsgs.size() == depthMsgs.size() || depthMsgs.empty()) &&
(cameraInfoMsgs.size() == depthCameraInfoMsgs.size() || depthCameraInfoMsgs.empty()));
int imageWidth = imageMsgs.size()?imageMsgs[0]->image.cols:cameraInfoMsgs[0].width;
int imageHeight = imageMsgs.size()?imageMsgs[0]->image.rows:cameraInfoMsgs[0].height;
int depthWidth = depthMsgs.size()?depthMsgs[0]->image.cols:0;
int depthHeight = depthMsgs.size()?depthMsgs[0]->image.rows:0;
if(!depthMsgs.empty())
bool isDepth = depthMsgs.empty() || (depthMsgs[0].get() != 0 && (
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0));
// Note that right image can be also MONO16, check the camera info if Tx is set, if so assume it is stereo instead
if(isDepth &&
!depthMsgs.empty() &&
depthMsgs[0]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 &&
cameraInfoMsgs.size() == depthCameraInfoMsgs.size())
{
isDepth = cameraInfoMsgs[0].p[3] == 0.0 && depthCameraInfoMsgs[0].p[3] == 0.0;
static bool warned = false;
if(!warned && isDepth)
{
UWARN("Input depth/left image has encoding \"mono16\" and "
"camera info P[3] is null for both cameras, thus image is "
"considered a depth image. If the depth image is in "
"fact the right image, please convert the right image to "
"\"mono8\". This warning is shown only once.");
warned = true;
}
}
if(isDepth)
{
UASSERT_MSG(
imageWidth/depthWidth == imageHeight/depthHeight,
@@ -1828,7 +1872,7 @@ bool convertRGBDMsgs(
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_RGGB8) == 0))
{
UERROR("Input rgb type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb=%s",
UERROR("Input rgb/left type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current rgb/left=%s",
imageMsgs[i]->encoding.c_str());
return false;
}
@@ -1840,18 +1884,35 @@ bool convertRGBDMsgs(
imageHeight,
imageMsgs[i]->image.rows).c_str());
}
if(!depthMsgs.empty() &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
if(!depthMsgs.empty())
{
UERROR("Input depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
depthMsgs[i]->encoding.c_str());
return false;
if(isDepth &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
UERROR("Input depth type must be image_depth=32FC1,16UC1,mono16. Current depth=%s",
depthMsgs[i]->encoding.c_str());
return false;
}
else if(!isDepth &&
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
UERROR("Input right type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8. Current right=%s",
depthMsgs[i]->encoding.c_str());
return false;
}
}
rclcpp::Time stamp;
if(!depthMsgs.empty())
if(isDepth && !depthMsgs.empty())
{
UASSERT_MSG(depthMsgs[i]->image.cols == depthWidth && depthMsgs[i]->image.rows == depthHeight,
uFormat("depthWidth=%d vs %d imageHeight=%d vs %d",
@@ -1889,8 +1950,8 @@ bool convertRGBDMsgs(
waitForTransform);
if(sensorT.isNull())
{
UWARN("Could not get odometry value for depth image stamp (%fs). Latest odometry "
"stamp is %fs. The depth image pose will not be synchronized with odometry.", stamp.seconds(), odomStamp.seconds());
UWARN("Could not get odometry value for image stamp (%fs). Latest odometry "
"stamp is %fs. The image pose will not be synchronized with odometry.", timestampFromROS(stamp), timestampFromROS(odomStamp));
}
else
{
@@ -1928,33 +1989,138 @@ bool convertRGBDMsgs(
}
else
{
UERROR("Some RGB images are not the same type!");
UERROR("Some RGB/left images are not the same type!");
return false;
}
}
if(!depthMsgs.empty())
{
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
if(depth.empty())
if(isDepth)
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}
cv_bridge::CvImageConstPtr ptrDepth = depthMsgs[i];
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
UERROR("Some Depth images are not the same type!");
return false;
}
}
else
{
UERROR("Some Depth images are not the same type!");
return false;
cv_bridge::CvImageConstPtr ptrImage = depthMsgs[i];
if( depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
{
// do nothing
}
else
{
ptrImage = cv_bridge::cvtColor(depthMsgs[i], "mono8");
}
// initialize
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrImage->image.type());
}
if(ptrImage->image.type() == depth.type())
{
ptrImage->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
UERROR("Some right images are not the same type!");
return false;
}
}
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
if(isDepth)
{
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfoMsgs[i], localTransform));
}
else //stereo
{
UASSERT(cameraInfoMsgs.size() == depthCameraInfoMsgs.size());
rtabmap::Transform stereoTransform;
if(!alreadRectifiedImages)
{
stereoTransform = getTransform(
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
UERROR("Parameter %s is false but we cannot get TF between the two cameras!", rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(cameraInfoMsgs[i], depthCameraInfoMsgs[i], localTransform, stereoTransform);
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
UWARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). You may need to calibrate your camera. "
"This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
else if(stereoModel.baseline() == 0 && alreadRectifiedImages)
{
rtabmap::Transform stereoTransform = getTransform(
cameraInfoMsgs[i].header.frame_id,
depthCameraInfoMsgs[i].header.frame_id,
cameraInfoMsgs[i].header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull() || stereoTransform.x()<=0)
{
UWARN("We cannot estimated the baseline of the rectified images with tf! (%s->%s = %s)",
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.prettyPrint().c_str());
}
else
{
static bool warned = false;
if(!warned)
{
UWARN("Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
depthCameraInfoMsgs[i].header.frame_id.c_str(), cameraInfoMsgs[i].header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
stereoModel.left().fx(),
stereoModel.left().fy(),
stereoModel.left().cx(),
stereoModel.left().cy(),
stereoTransform.x(),
stereoModel.localTransform(),
stereoModel.left().imageSize());
}
}
stereoCameraModels.push_back(stereoModel);
}
if(localKeyPoints && localKeyPointsMsgs.size() == cameraInfoMsgs.size())
{
src/OdometryROS.cpp
+61 -32
View File
@@ -499,7 +499,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odometry_->getPose().isIdentity())
// Use only XYZ to handle the case odometry was previously initialized with IMU,
// we assume that the ground truth contains also a real initial orientation
float x,y,z;
odometry_->getPose().getTranslation(x, y, z);
if(x==0.0f && y==0.0f && z==0.0f)
{
// sync with the first value of the ground truth
if(groundTruth.isNull())
@@ -772,30 +776,46 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
{
return;
}
else if(publishNullWhenLost_)
else // pose is null / lost
{
//RCLCPP_WARN(this->get_logger(), "Odometry lost!");
if(publishNullWhenLost_)
{
//RCLCPP_WARN(this->get_logger(), "Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::msg::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.pose.pose.orientation.w=0; // invalid (null transform)
//publish the message
odomPub_->publish(odom);
}
// Publish the Tf correction using guess pose directly so that TF tree is not broken when vo is lost
if(publishTf_ && !guess_.isNull())
{
geometry_msgs::msg::TransformStamped correctionMsg;
correctionMsg.child_frame_id = guessFrameId_;
correctionMsg.header.frame_id = odomFrameId_;
correctionMsg.header.stamp = header.stamp;
Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse();
rtabmap_ros::transformToGeometryMsg(correction, correctionMsg.transform);
tfBroadcaster_->sendTransform(correctionMsg);
}
//send null pose to notify that odometry is lost
nav_msgs::msg::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_;
odom.pose.covariance.at(0) = BAD_COVARIANCE; // xx
odom.pose.covariance.at(7) = BAD_COVARIANCE; // yy
odom.pose.covariance.at(14) = BAD_COVARIANCE; // zz
odom.pose.covariance.at(21) = BAD_COVARIANCE; // rr
odom.pose.covariance.at(28) = BAD_COVARIANCE; // pp
odom.pose.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.twist.covariance.at(0) = BAD_COVARIANCE; // xx
odom.twist.covariance.at(7) = BAD_COVARIANCE; // yy
odom.twist.covariance.at(14) = BAD_COVARIANCE; // zz
odom.twist.covariance.at(21) = BAD_COVARIANCE; // rr
odom.twist.covariance.at(28) = BAD_COVARIANCE; // pp
odom.twist.covariance.at(35) = BAD_COVARIANCE; // yawyaw
odom.pose.pose.orientation.w=0; // invalid (null transform)
//publish the message
odomPub_->publish(odom);
}
if(pose.isNull() && resetCurrentCount_ > 0)
@@ -805,20 +825,29 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
if(tfPose.isNull())
if(!guess_.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset based on latest guess available from TF (%s->%s, moved %s since got lost)!",
guessFrameId_.c_str(), frameId_.c_str(), guess_.prettyPrint().c_str());
odometry_->reset(odometry_->getPose() * guess_);
guess_.setNull();
}
else
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_);
if(tfPose.isNull())
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
}
}
}
}
src/StereoCameraNode.cpp
+14 -7
View File
@@ -86,13 +86,20 @@ int main(int argc, char** argv)
cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageRightPub.publish(imageRight.toImageMsg());
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
if(data.stereoCameraModels().size())
{
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModels()[0].right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
}
else
{
ROS_ERROR("No calibration loaded!");
}
ros::spinOnce();
}
src/costmap_2d/voxel_markers.cpp
+1 -1
View File
@@ -149,7 +149,7 @@ ros::Subscriber sub;
void connectCb()
{
ros::NodeHandle n;
sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, _1));
sub = n.subscribe < costmap_2d::VoxelGrid > ("voxel_grid", 1, boost::bind(voxelCallback, pub, boost::placeholders::_1));
}
void disconnectCb()
src/impl/CommonDataSubscriberDepth.cpp
+32 -32
View File
@@ -40,7 +40,7 @@ void CommonDataSubscriber::depthCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -52,7 +52,7 @@ void CommonDataSubscriber::depthScan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -64,7 +64,7 @@ void CommonDataSubscriber::depthScan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScanDescCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -80,7 +80,7 @@ void CommonDataSubscriber::depthScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -92,7 +92,7 @@ void CommonDataSubscriber::depthInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan2dInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -104,7 +104,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScan3dInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -116,7 +116,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthScanDescInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -132,7 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom
@@ -146,7 +146,7 @@ void CommonDataSubscriber::depthOdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -158,7 +158,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -170,7 +170,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -186,7 +186,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthOdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -198,7 +198,7 @@ void CommonDataSubscriber::depthOdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -210,7 +210,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
{
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -222,7 +222,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
{
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomScanDescInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -238,7 +238,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -253,7 +253,7 @@ void CommonDataSubscriber::depthDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -265,7 +265,7 @@ void CommonDataSubscriber::depthDataScan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -277,7 +277,7 @@ void CommonDataSubscriber::depthDataScan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -293,7 +293,7 @@ void CommonDataSubscriber::depthDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthDataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -305,7 +305,7 @@ void CommonDataSubscriber::depthDataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan2dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -317,7 +317,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
{
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScan3dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -329,7 +329,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
{
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthDataScanDescInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -345,7 +345,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom + User Data
@@ -359,7 +359,7 @@ void CommonDataSubscriber::depthOdomDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -371,7 +371,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
{
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -383,7 +383,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
{
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -399,7 +399,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::depthOdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -411,7 +411,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
{
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -423,7 +423,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -435,7 +435,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
const rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
{
sensor_msgs::msg::LaserScan scan2dMsg; // Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -451,7 +451,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#endif
src/impl/CommonDataSubscriberRGB.cpp
+32 -32
View File
@@ -40,7 +40,7 @@ void CommonDataSubscriber::rgbCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan2dCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -52,7 +52,7 @@ void CommonDataSubscriber::rgbScan2dCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan3dCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -64,7 +64,7 @@ void CommonDataSubscriber::rgbScan3dCallback(
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScanDescCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -80,7 +80,7 @@ void CommonDataSubscriber::rgbScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -92,7 +92,7 @@ void CommonDataSubscriber::rgbInfoCallback(
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan2dInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -104,7 +104,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScan3dInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -116,7 +116,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbScanDescInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
@@ -132,7 +132,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Odom
@@ -146,7 +146,7 @@ void CommonDataSubscriber::rgbOdomCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -158,7 +158,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -170,7 +170,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -186,7 +186,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbOdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -198,7 +198,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -210,7 +210,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -222,7 +222,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -238,7 +238,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -253,7 +253,7 @@ void CommonDataSubscriber::rgbDataCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -265,7 +265,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -277,7 +277,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -293,7 +293,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbDataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -305,7 +305,7 @@ void CommonDataSubscriber::rgbDataInfoCallback(
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan2dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -317,7 +317,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScan3dInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -329,7 +329,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbDataScanDescInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -345,7 +345,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
// RGB + Depth + Odom + User Data
@@ -359,7 +359,7 @@ void CommonDataSubscriber::rgbOdomDataCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -371,7 +371,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -383,7 +383,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
sensor_msgs::msg::LaserScan 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -399,7 +399,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
void CommonDataSubscriber::rgbOdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -411,7 +411,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -423,7 +423,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
{
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -435,7 +435,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
{
sensor_msgs::msg::LaserScan scan2dMsg; // Null
cv_bridge::CvImageConstPtr depthMsg;// Null
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
}
void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -451,7 +451,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
{
globalDescriptor.push_back(scanMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scanMsg->scan, scanMsg->scan_cloud, odomInfoMsg, globalDescriptor);
}
#endif
src/impl/CommonDataSubscriberRGBD.cpp
+20 -20
View File
@@ -52,7 +52,7 @@ void CommonDataSubscriber::rgbdCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -76,7 +76,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -100,7 +100,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -123,7 +123,7 @@ void CommonDataSubscriber::rgbdScanDescCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -147,7 +147,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -173,7 +173,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -197,7 +197,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -221,7 +221,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -248,7 +248,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -272,7 +272,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -299,7 +299,7 @@ void CommonDataSubscriber::rgbdDataCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -323,7 +323,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -347,7 +347,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -374,7 +374,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -398,7 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -424,7 +424,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -448,7 +448,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
*scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -472,7 +472,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, *scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -499,7 +499,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg ,rgb,
commonSingleCameraCallback(odomMsg, userDataMsg ,rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
@@ -522,7 +522,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
globalDescriptorMsgs.push_back(image1Msg->global_descriptor);
}
commonSingleDepthCallback(odomMsg, userDataMsg, rgb,
commonSingleCameraCallback(odomMsg, userDataMsg, rgb,
depth, image1Msg->rgb_camera_info, image1Msg->depth_camera_info,
scanMsg, scan3dMsg, odomInfoMsg,
globalDescriptorMsgs, image1Msg->key_points, image1Msg->points,
src/impl/CommonDataSubscriberRGBD2.cpp
+23 -20
View File
@@ -44,6 +44,9 @@ namespace rtabmap_ros {
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -71,7 +74,7 @@ void CommonDataSubscriber::rgbd2Callback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -84,7 +87,7 @@ void CommonDataSubscriber::rgbd2Scan2dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -97,7 +100,7 @@ void CommonDataSubscriber::rgbd2Scan3dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -113,7 +116,7 @@ void CommonDataSubscriber::rgbd2ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -126,7 +129,7 @@ void CommonDataSubscriber::rgbd2InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
void CommonDataSubscriber::rgbd2OdomCallback(
@@ -140,7 +143,7 @@ void CommonDataSubscriber::rgbd2OdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -153,7 +156,7 @@ void CommonDataSubscriber::rgbd2OdomScan2dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -166,7 +169,7 @@ void CommonDataSubscriber::rgbd2OdomScan3dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -182,7 +185,7 @@ void CommonDataSubscriber::rgbd2OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -195,7 +198,7 @@ void CommonDataSubscriber::rgbd2OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -211,7 +214,7 @@ void CommonDataSubscriber::rgbd2DataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -224,7 +227,7 @@ void CommonDataSubscriber::rgbd2DataScan2dCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -237,7 +240,7 @@ void CommonDataSubscriber::rgbd2DataScan3dCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -253,7 +256,7 @@ void CommonDataSubscriber::rgbd2DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2DataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -266,7 +269,7 @@ void CommonDataSubscriber::rgbd2DataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -281,7 +284,7 @@ void CommonDataSubscriber::rgbd2OdomDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -294,7 +297,7 @@ void CommonDataSubscriber::rgbd2OdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -307,7 +310,7 @@ void CommonDataSubscriber::rgbd2OdomDataScan3dCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -323,7 +326,7 @@ void CommonDataSubscriber::rgbd2OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -336,7 +339,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberRGBD3.cpp
+44 -40
View File
@@ -46,6 +46,10 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image1Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -79,8 +83,8 @@ void CommonDataSubscriber::rgbd3Callback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -96,8 +100,8 @@ void CommonDataSubscriber::rgbd3Scan2dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -113,8 +117,8 @@ void CommonDataSubscriber::rgbd3Scan3dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -133,8 +137,8 @@ void CommonDataSubscriber::rgbd3ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -150,8 +154,8 @@ void CommonDataSubscriber::rgbd3InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -169,8 +173,8 @@ void CommonDataSubscriber::rgbd3OdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -186,8 +190,8 @@ void CommonDataSubscriber::rgbd3OdomScan2dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -203,8 +207,8 @@ void CommonDataSubscriber::rgbd3OdomScan3dCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -223,8 +227,8 @@ void CommonDataSubscriber::rgbd3OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -240,8 +244,8 @@ void CommonDataSubscriber::rgbd3OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -260,8 +264,8 @@ void CommonDataSubscriber::rgbd3DataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -277,8 +281,8 @@ void CommonDataSubscriber::rgbd3DataScan2dCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -294,8 +298,8 @@ void CommonDataSubscriber::rgbd3DataScan3dCallback(
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -314,8 +318,8 @@ void CommonDataSubscriber::rgbd3DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -331,8 +335,8 @@ void CommonDataSubscriber::rgbd3DataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -349,8 +353,8 @@ void CommonDataSubscriber::rgbd3OdomDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -366,8 +370,8 @@ void CommonDataSubscriber::rgbd3OdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, *scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -383,8 +387,8 @@ void CommonDataSubscriber::rgbd3OdomDataScan3dCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::SharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
*scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -403,8 +407,8 @@ void CommonDataSubscriber::rgbd3OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanDescMsg->scan,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan,
scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
@@ -420,8 +424,8 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, scanMsg,
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs,
depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg,
scan3dMsg, odomInfoMsg, globalDescriptorMsgs,
localKeyPoints, localPoints3d, localDescriptors);
}
src/impl/CommonDataSubscriberRGBD4.cpp
+25 -20
View File
@@ -48,6 +48,11 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image2Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -87,7 +92,7 @@ void CommonDataSubscriber::rgbd4Callback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthcameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -102,7 +107,7 @@ void CommonDataSubscriber::rgbd4Scan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -117,7 +122,7 @@ void CommonDataSubscriber::rgbd4Scan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -135,7 +140,7 @@ void CommonDataSubscriber::rgbd4ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -150,7 +155,7 @@ void CommonDataSubscriber::rgbd4InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom
@@ -167,7 +172,7 @@ void CommonDataSubscriber::rgbd4OdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -182,7 +187,7 @@ void CommonDataSubscriber::rgbd4OdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -197,7 +202,7 @@ void CommonDataSubscriber::rgbd4OdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -215,7 +220,7 @@ void CommonDataSubscriber::rgbd4OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -230,7 +235,7 @@ void CommonDataSubscriber::rgbd4OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -248,7 +253,7 @@ void CommonDataSubscriber::rgbd4DataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -263,7 +268,7 @@ void CommonDataSubscriber::rgbd4DataScan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -278,7 +283,7 @@ void CommonDataSubscriber::rgbd4DataScan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -296,7 +301,7 @@ void CommonDataSubscriber::rgbd4DataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4DataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -311,7 +316,7 @@ void CommonDataSubscriber::rgbd4DataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 2 RGBD + Odom + User Data
@@ -328,7 +333,7 @@ void CommonDataSubscriber::rgbd4OdomDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -343,7 +348,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -358,7 +363,7 @@ void CommonDataSubscriber::rgbd4OdomDataScan3dCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -376,7 +381,7 @@ void CommonDataSubscriber::rgbd4OdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -391,7 +396,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberRGBD5.cpp
+16 -10
View File
@@ -50,6 +50,12 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image3Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -95,7 +101,7 @@ void CommonDataSubscriber::rgbd5Callback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -111,7 +117,7 @@ void CommonDataSubscriber::rgbd5Scan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -127,7 +133,7 @@ void CommonDataSubscriber::rgbd5Scan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -146,7 +152,7 @@ void CommonDataSubscriber::rgbd5ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -162,7 +168,7 @@ void CommonDataSubscriber::rgbd5InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 5 RGBD + Odom
@@ -180,7 +186,7 @@ void CommonDataSubscriber::rgbd5OdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -196,7 +202,7 @@ void CommonDataSubscriber::rgbd5OdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -212,7 +218,7 @@ void CommonDataSubscriber::rgbd5OdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -231,7 +237,7 @@ void CommonDataSubscriber::rgbd5OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd5OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -247,7 +253,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::setupRGBD5Callbacks(
src/impl/CommonDataSubscriberRGBD6.cpp
+17 -10
View File
@@ -52,6 +52,13 @@ namespace rtabmap_ros {
cameraInfoMsgs.push_back(image4Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image5Msg->rgb_camera_info); \
cameraInfoMsgs.push_back(image6Msg->rgb_camera_info); \
std::vector<sensor_msgs::CameraInfo> depthCameraInfoMsgs; \
depthCameraInfoMsgs.push_back(image1Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image2Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image3Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image4Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image5Msg->depth_camera_info); \
depthCameraInfoMsgs.push_back(image6Msg->depth_camera_info); \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -103,7 +110,7 @@ void CommonDataSubscriber::rgbd6Callback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6Scan2dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -120,7 +127,7 @@ void CommonDataSubscriber::rgbd6Scan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6Scan3dCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -137,7 +144,7 @@ void CommonDataSubscriber::rgbd6Scan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6ScanDescCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -157,7 +164,7 @@ void CommonDataSubscriber::rgbd6ScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6InfoCallback(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image1Msg,
@@ -174,7 +181,7 @@ void CommonDataSubscriber::rgbd6InfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// 6 RGBD + Odom
@@ -193,7 +200,7 @@ void CommonDataSubscriber::rgbd6OdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -210,7 +217,7 @@ void CommonDataSubscriber::rgbd6OdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -227,7 +234,7 @@ void CommonDataSubscriber::rgbd6OdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -247,7 +254,7 @@ void CommonDataSubscriber::rgbd6OdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbd6OdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -264,7 +271,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::setupRGBD6Callbacks(
src/impl/CommonDataSubscriberRGBDX.cpp
+22 -20
View File
@@ -39,6 +39,7 @@ namespace rtabmap_ros {
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(imagesMsg->rgbd_images.size()); \
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(imagesMsg->rgbd_images.size()); \
std::vector<sensor_msgs::msg::CameraInfo> cameraInfoMsgs; \
std::vector<sensor_msgs::msg::CameraInfo> depthCameraInfoMsgs; \
std::vector<rtabmap_ros::msg::GlobalDescriptor> globalDescriptorMsgs; \
std::vector<std::vector<rtabmap_ros::msg::KeyPoint> > localKeyPoints; \
std::vector<std::vector<rtabmap_ros::msg::Point3f> > localPoints3d; \
@@ -47,6 +48,7 @@ namespace rtabmap_ros {
{ \
rtabmap_ros::toCvShare(imagesMsg->rgbd_images[i], imagesMsg, imageMsgs[i], depthMsgs[i]); \
cameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].rgb_camera_info); \
depthCameraInfoMsgs.push_back(imagesMsg->rgbd_images[i].depth_camera_info); \
if(!imagesMsg->rgbd_images[i].global_descriptor.data.empty()) \
globalDescriptorMsgs.push_back(imagesMsg->rgbd_images[i].global_descriptor); \
localKeyPoints.push_back(imagesMsg->rgbd_images[i].key_points); \
@@ -67,7 +69,7 @@ void CommonDataSubscriber::rgbdXCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan2dCallback(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
@@ -79,7 +81,7 @@ void CommonDataSubscriber::rgbdXScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScan3dCallback(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
@@ -91,7 +93,7 @@ void CommonDataSubscriber::rgbdXScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXScanDescCallback(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
@@ -106,7 +108,7 @@ void CommonDataSubscriber::rgbdXScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXInfoCallback(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr imagesMsg,
@@ -118,7 +120,7 @@ void CommonDataSubscriber::rgbdXInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom
void CommonDataSubscriber::rgbdXOdomCallback(
@@ -131,7 +133,7 @@ void CommonDataSubscriber::rgbdXOdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -143,7 +145,7 @@ void CommonDataSubscriber::rgbdXOdomScan2dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -155,7 +157,7 @@ void CommonDataSubscriber::rgbdXOdomScan3dCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -170,7 +172,7 @@ void CommonDataSubscriber::rgbdXOdomScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -182,7 +184,7 @@ void CommonDataSubscriber::rgbdXOdomInfoCallback(
rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#ifdef RTABMAP_SYNC_USER_DATA
@@ -197,7 +199,7 @@ void CommonDataSubscriber::rgbdXDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan2dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -209,7 +211,7 @@ void CommonDataSubscriber::rgbdXDataScan2dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScan3dCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -221,7 +223,7 @@ void CommonDataSubscriber::rgbdXDataScan3dCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataScanDescCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -236,7 +238,7 @@ void CommonDataSubscriber::rgbdXDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXDataInfoCallback(
const rtabmap_ros::msg::UserData::ConstSharedPtr userDataMsg,
@@ -248,7 +250,7 @@ void CommonDataSubscriber::rgbdXDataInfoCallback(
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
// X RGBD + Odom + User Data
@@ -262,7 +264,7 @@ void CommonDataSubscriber::rgbdXOdomDataCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -274,7 +276,7 @@ void CommonDataSubscriber::rgbdXOdomDataScan2dCallback(
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, *scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -286,7 +288,7 @@ void CommonDataSubscriber::rgbdXOdomDataScan3dCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
rtabmap_ros::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, *scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -301,7 +303,7 @@ void CommonDataSubscriber::rgbdXOdomDataScanDescCallback(
{
globalDescriptorMsgs.push_back(scanDescMsg->global_descriptor);
}
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanDescMsg->scan, scanDescMsg->scan_cloud, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -313,7 +315,7 @@ void CommonDataSubscriber::rgbdXOdomDataInfoCallback(
sensor_msgs::msg::LaserScan scanMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
commonDepthCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
commonMultiCameraCallback(odomMsg, userDataMsg, imageMsgs, depthMsgs, cameraInfoMsgs, depthCameraInfoMsgs, scanMsg, scan3dMsg, odomInfoMsg, globalDescriptorMsgs, localKeyPoints, localPoints3d, localDescriptors);
}
#endif
src/impl/CommonDataSubscriberStereo.cpp
+4 -5
View File
@@ -36,13 +36,12 @@ void CommonDataSubscriber::stereoCallback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr leftCamInfoMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr rightCamInfoMsg)
{
callbackCalled();
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scanMsg; // null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoInfoCallback(
const sensor_msgs::msg::Image::ConstSharedPtr leftImageMsg,
@@ -56,7 +55,7 @@ void CommonDataSubscriber::stereoInfoCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
// Stereo + Odom
@@ -72,7 +71,7 @@ void CommonDataSubscriber::stereoOdomCallback(
sensor_msgs::msg::LaserScan scanMsg; // null
sensor_msgs::msg::PointCloud2 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);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scanMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::stereoOdomInfoCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
@@ -86,7 +85,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
rtabmap_ros::msg::UserData::SharedPtr userDataMsg; // Null
sensor_msgs::msg::LaserScan scan2dMsg; // Null
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
commonStereoCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(leftImageMsg), cv_bridge::toCvShare(rightImageMsg), *leftCamInfoMsg, *rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
}
void CommonDataSubscriber::setupStereoCallbacks(
src/nodelets/data_throttle.cpp
+9 -3
View File
@@ -89,6 +89,7 @@ private:
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
if(private_nh.getParam("max_rate", rate_))
{
NODELET_WARN("\"max_rate\" is now known as \"rate\".");
@@ -96,16 +97,21 @@ private:
private_nh.param("rate", rate_, rate_);
private_nh.param("queue_size", queueSize, queueSize);
private_nh.param("approx_sync", approxSync, approxSync);
private_nh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
private_nh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), image_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(std::bind(&DataThrottleNodelet::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
}
else
{
src/nodelets/point_cloud_aggregator.cpp
+115 -11
View File
@@ -49,7 +49,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
approxSync3_(0),
exactSync2_(0),
approxSync2_(0),
waitForTransform_(0.1)
waitForTransform_(0.1),
xyzOutput_(false)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -61,14 +62,17 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
int queueSize = 5;
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
int qos;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
frameId_ = this->declare_parameter("frame_id", frameId_);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
approx = this->declare_parameter("approx_sync", approx);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
count = this->declare_parameter("count", count);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
xyzOutput_ = this->declare_parameter("xyz_output", xyzOutput_);
cloudPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("combined_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
@@ -83,6 +87,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync4_ = new message_filters::Synchronizer<ApproxSync4Policy>(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -90,9 +96,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync4_ = new message_filters::Synchronizer<ExactSync4Policy>(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_);
exactSync4_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds4_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str(),
@@ -104,6 +111,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync3_ = new message_filters::Synchronizer<ApproxSync3Policy>(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -111,9 +120,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync3_ = new message_filters::Synchronizer<ExactSync3Policy>(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_);
exactSync3_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds3_callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str(),
cloudSub_3_.getTopic().c_str());
@@ -123,6 +133,8 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
if(approx)
{
approxSync2_ = new message_filters::Synchronizer<ApproxSync2Policy>(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -130,9 +142,10 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
exactSync2_ = new message_filters::Synchronizer<ExactSync2Policy>(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_);
exactSync2_->registerCallback(std::bind(&rtabmap_ros::PointCloudAggregator::clouds2_callback, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
this->get_name(),
approx?"approx":"exact",
approx&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
cloudSub_1_.getTopic().c_str(),
cloudSub_2_.getTopic().c_str());
}
@@ -233,6 +246,42 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
frameId = cloudMsgs[0]->header.frame_id;
}
if(xyzOutput_ && !output->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<output->fields.size(); ++i)
{
if(output->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(output->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(output->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*output, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *output);
}
}
for(unsigned int i=1; i<cloudMsgs.size(); ++i)
{
rtabmap::Transform cloudDisplacement;
@@ -286,15 +335,71 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
cloud2 = rtabmap::util3d::removeNaNFromPointCloud(cloud2);
}
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
if(xyzOutput_ && !cloud2->data.empty())
{
// convert only if not already XYZ cloud
bool hasField[4] = {false};
for(size_t i=0; i<cloud2->fields.size(); ++i)
{
if(cloud2->fields[i].name.compare("x") == 0)
{
hasField[0] = true;
}
else if(cloud2->fields[i].name.compare("y") == 0)
{
hasField[1] = true;
}
else if(cloud2->fields[i].name.compare("z") == 0)
{
hasField[2] = true;
}
else
{
hasField[3] = true; // other
break;
}
}
if(hasField[0] && hasField[1] && hasField[2] && !hasField[3])
{
// do nothing, already XYZ
}
else
{
pcl::PointCloud<pcl::PointXYZ> cloudxyz;
pcl::fromPCLPointCloud2(*cloud2, cloudxyz);
pcl::toPCLPointCloud2(cloudxyz, *cloud2);
}
}
if(output->data.empty())
{
output = cloud2;
}
else if(!cloud2->data.empty())
{
if(output->fields.size() != cloud2->fields.size())
{
RCLCPP_WARN(this->get_logger(), "%s: Input topics don't have all the "
"same number of fields (cloud1=%d, cloud%d=%d), concatenation "
"may fails. You can enable \"xyz_output\" option "
"to convert all inputs to XYZ.",
get_name(),
(int)output->fields.size(),
i+1,
(int)output->fields.size());
}
pcl::PCLPointCloud2::Ptr tmp_output(new pcl::PCLPointCloud2);
#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
pcl::concatenate(*output, *cloud2, *tmp_output);
pcl::concatenate(*output, *cloud2, *tmp_output);
#else
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
pcl::concatenatePointCloud(*output, *cloud2, *tmp_output);
#endif
//Make sure row_step is the sum of both
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
//Make sure row_step is the sum of both
tmp_output->row_step = tmp_output->width * tmp_output->point_step;
output = tmp_output;
}
}
sensor_msgs::msg::PointCloud2::UniquePtr rosCloud(new sensor_msgs::msg::PointCloud2);
@@ -304,7 +409,6 @@ void PointCloudAggregator::combineClouds(const std::vector<sensor_msgs::msg::Poi
cloudPub_->publish(std::move(rosCloud));
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
src/nodelets/point_cloud_xyz.cpp
+62 -17
View File
@@ -66,7 +66,9 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
int qos = 0;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -119,9 +121,13 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZ::callback, this, std::placeholders::_1, std::placeholders::_2));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), disparitySub_, disparityCameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZ::callbackDisparity, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -151,12 +157,12 @@ PointCloudXYZ::~PointCloudXYZ()
delete exactSyncDisparity_;
}
void PointCloudXYZ::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
if(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)
{
RCLCPP_ERROR(this->get_logger(), "Input type depth=32FC1,16UC1,MONO16");
return;
@@ -166,29 +172,68 @@ void PointCloudXYZ::callback(
{
rclcpp::Time time = now();
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depthMsg);
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb;
if(model.imageWidth() && model.imageHeight())
{
roiRgb = rtabmap::util2d::computeRoi(model.imageSize(), roiRatios_);
}
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
(roiRgb.width != 0 ||
(roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)))
{
depth = cv::Mat(depth, roiDepth);
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
model = model.roi(roiRgb);
}
else
{
model = model.roi(roiDepth);
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
m,
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depth->header);
processAndPublish(pclCloud, indices, depthMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from depth time = %f s", (now() - time).seconds());
}
}
@@ -222,6 +267,7 @@ void PointCloudXYZ::callbackDisparity(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparity(
@@ -233,7 +279,6 @@ void PointCloudXYZ::callbackDisparity(
indices.get());
processAndPublish(pclCloud, indices, disparityMsg->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyz from disparity time = %f s", (now() - time).seconds());
}
}
src/nodelets/point_cloud_xyzrgb.cpp
+52 -16
View File
@@ -69,7 +69,9 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
std::string roiStr;
int queueSize = 10;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -139,12 +141,18 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&PointCloudXYZRGB::depthCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncDisparity_ = new message_filters::Synchronizer<MyApproxSyncDisparityPolicy>(MyApproxSyncDisparityPolicy(queueSize), imageLeft_, imageDisparitySub_, cameraInfoLeft_);
if(approxSyncMaxInterval > 0.0)
approxSyncDisparity_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDisparity_->registerCallback(std::bind(&PointCloudXYZRGB::disparityCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
approxSyncStereo_ = new message_filters::Synchronizer<MyApproxSyncStereoPolicy>(MyApproxSyncStereoPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval > 0.0)
approxSyncStereo_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncStereo_->registerCallback(std::bind(&PointCloudXYZRGB::stereoCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -224,31 +232,56 @@ void PointCloudXYZRGB::depthCallback(
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
UASSERT(imageDepthPtr->image.cols == imagePtr->image.cols);
UASSERT(imageDepthPtr->image.rows == imagePtr->image.rows);
rtabmap::CameraModel model = cameraModelFromROS(*cameraInfo);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
cv::Mat rgb = imagePtr->image;
cv::Mat depth = imageDepthPtr->image;
if( roiRatios_.size() == 4 &&
((roiRatios_[0] > 0.0f && roiRatios_[0] <= 1.0f) ||
(roiRatios_[1] > 0.0f && roiRatios_[1] <= 1.0f) ||
(roiRatios_[2] > 0.0f && roiRatios_[2] <= 1.0f) ||
(roiRatios_[3] > 0.0f && roiRatios_[3] <= 1.0f)))
{
cv::Rect roiDepth = rtabmap::util2d::computeRoi(depth, roiRatios_);
cv::Rect roiRgb = rtabmap::util2d::computeRoi(rgb, roiRatios_);
if( roiDepth.width%decimation_==0 &&
roiDepth.height%decimation_==0 &&
roiRgb.width%decimation_==0 &&
roiRgb.height%decimation_==0)
{
depth = cv::Mat(depth, roiDepth);
rgb = cv::Mat(rgb, roiRgb);
model = model.roi(roiRgb);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios_[0],
roiRatios_[1],
roiRatios_[2],
roiRatios_[3],
roiDepth.width,
roiDepth.height,
roiRgb.width,
roiRgb.height,
decimation_);
}
}
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
m,
rgb,
depth,
model,
decimation_,
maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, imagePtr->header);
RCLCPP_DEBUG(this->get_logger(), "point_cloud_xyzrgb from RGB-D time = %f s", (now() - time).seconds());
@@ -300,6 +333,8 @@ void PointCloudXYZRGB::disparityCallback(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
UASSERT(disparity.cols == leftModel.imageWidth() && disparity.rows == leftModel.imageHeight());
UASSERT(imagePtr->image.cols == leftModel.imageWidth() && imagePtr->image.rows == leftModel.imageHeight());
rtabmap::StereoCameraModel stereoModel(imageDisparity->f, imageDisparity->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), imageDisparity->t);
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDisparityRGB(
@@ -397,7 +432,8 @@ void PointCloudXYZRGB::rgbdImageCallback(
maxDepth_,
minDepth_,
indices.get(),
stereoBMParameters_);
stereoBMParameters_,
roiRatios_);
processAndPublish(pclCloud, indices, image->header);
}
src/nodelets/pointcloud_to_depthimage.cpp
+14 -2
View File
@@ -95,6 +95,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
depthImage16Pub_ = image_transport::create_camera_publisher(node.get(), "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_camera_publisher(node.get(), "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
if(approx)
{
@@ -160,12 +162,15 @@ void PointCloudToDepthImage::callback(
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
sensor_msgs::msg::CameraInfo cameraInfoMsgOut = *cameraInfoMsg;
if(decimation_ > 1)
{
if(model.imageWidth()%decimation_ == 0 && model.imageHeight()%decimation_ == 0)
{
model = model.scaled(1.0f/float(decimation_));
float scale = 1.0f/float(decimation_);
model = model.scaled(scale);
rtabmap_ros::cameraModelToROS(model, cameraInfoMsgOut);
}
else
{
@@ -216,6 +221,10 @@ void PointCloudToDepthImage::callback(
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
if(cameraInfo32Pub_->get_subscription_count())
{
cameraInfo32Pub_->publish(cameraInfoMsgOut);
}
}
if(depthImage16Pub_.getNumSubscribers())
@@ -223,6 +232,10 @@ void PointCloudToDepthImage::callback(
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
if(cameraInfo16Pub_->get_subscription_count())
{
cameraInfo16Pub_->publish(cameraInfoMsgOut);
}
}
if( cloudStamp != timestampFromROS(pointCloud2Msg->header.stamp) ||
@@ -237,7 +250,6 @@ void PointCloudToDepthImage::callback(
}
}
}
}
#include "rclcpp_components/register_node_macro.hpp"
src/nodelets/rgb_sync.cpp
+8 -1
View File
@@ -52,13 +52,17 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
int queueSize = 10;
bool approxSync = true;
int qos = 0;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCaminfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCaminfo);
@@ -70,6 +74,8 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBSync::callback, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -82,9 +88,10 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCaminfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=std::numeric_limits<double>::max()?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
src/nodelets/rgbd_odometry.cpp
+109 -23
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/image_encodings.hpp>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap_ros/msg/rgbd_images.hpp>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
@@ -72,6 +73,8 @@ RGBDOdometry::~RGBDOdometry()
delete exactSync3_;
delete approxSync4_;
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
}
void RGBDOdometry::onOdomInit()
@@ -79,7 +82,9 @@ void RGBDOdometry::onOdomInit()
int rgbdCameras = 1;
bool approxSync = true;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
@@ -90,11 +95,13 @@ void RGBDOdometry::onOdomInit()
}
if(rgbdCameras > 5)
{
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet.");
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
}
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
@@ -107,19 +114,19 @@ void RGBDOdometry::onOdomInit()
{
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2)
@@ -130,6 +137,8 @@ void RGBDOdometry::onOdomInit()
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
else
@@ -140,9 +149,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name());
}
@@ -155,6 +165,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -166,9 +178,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name());
@@ -183,6 +196,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -195,9 +210,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -214,6 +230,8 @@ void RGBDOdometry::onOdomInit()
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
else
@@ -227,9 +245,10 @@ void RGBDOdometry::onOdomInit()
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getSubscriber()->get_topic_name(),
rgbd_image2_sub_.getSubscriber()->get_topic_name(),
rgbd_image3_sub_.getSubscriber()->get_topic_name(),
@@ -237,9 +256,17 @@ void RGBDOdometry::onOdomInit()
rgbd_image5_sub_.getSubscriber()->get_topic_name());
}
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdxSub_->get_topic_name());
}
else
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
@@ -250,13 +277,15 @@ void RGBDOdometry::onOdomInit()
else
{
image_transport::TransportHints hints(this);
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(queueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -265,9 +294,10 @@ void RGBDOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&RGBDOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name());
@@ -322,8 +352,7 @@ void RGBDOdometry::commonCallback(
int cameraCount = rgbImages.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
std::vector<rtabmap::CameraModel> cameraModels;
for(unsigned int i=0; i<rgbImages.size(); ++i)
{
if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
@@ -374,6 +403,28 @@ void RGBDOdometry::commonCallback(
return;
}
if(i>0)
{
double stampDiff = fabs(timestampFromROS(rgbImages[i]->header.stamp) - timestampFromROS(rgbImages[i-1]->header.stamp));
if(stampDiff > 1.0/60.0)
{
static bool warningShown = false;
if(!warningShown)
{
RCLCPP_WARN(this->get_logger(), "The time difference between cameras %d and %d is "
"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
"to set approx_sync_max_interval to reject bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp. This "
"message is only printed once.",
i-1, i,
stampDiff,
i-1, timestampFromROS(rgbImages[i-1]->header.stamp),
i, timestampFromROS(rgbImages[i]->header.stamp));
warningShown = true;
}
}
}
cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
@@ -389,7 +440,6 @@ void RGBDOdometry::commonCallback(
}
cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
@@ -398,7 +448,7 @@ void RGBDOdometry::commonCallback(
}
if(depth.empty())
{
depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type());
}
if(ptrImage->image.type() == rgb.type())
@@ -407,17 +457,17 @@ void RGBDOdometry::commonCallback(
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type!");
RCLCPP_ERROR(this->get_logger(), "Some RGB images are not the same type! %d vs %d", ptrImage->image.type(), rgb.type());
return;
}
if(subDepth.type() == depth.type())
if(ptrDepth->image.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", subDepth.type(), depth.type());
RCLCPP_ERROR(this->get_logger(), "Some Depth images are not the same type! %d vs %d", ptrDepth->image.type(), depth.type());
return;
}
@@ -452,6 +502,42 @@ void RGBDOdometry::callback(
depthMsgs[0] = cv_bridge::toCvShare(depth);
infoMsgs.push_back(*cameraInfo);
double stampDiff = fabs(timestampFromROS(image->header.stamp) - timestampFromROS(depth->header.stamp));
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
timestampFromROS(image->header.stamp),
timestampFromROS(depth->header.stamp));
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
void RGBDOdometry::callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
{
RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
return;
}
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(images->rgbd_images.size());
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
rtabmap_ros::toCvShare(images->rgbd_images[i], images, imageMsgs[i], depthMsgs[i]);
infoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
}
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
src/nodelets/rgbd_sync.cpp
+21 -1
View File
@@ -53,8 +53,10 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
@@ -68,6 +70,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
}
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
@@ -81,6 +85,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -94,9 +100,10 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
imageDepthSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
@@ -142,6 +149,19 @@ void RGBDSync::callback(
{
double rgbStamp = timestampFromROS(image->header.stamp);
double depthStamp = timestampFromROS(depth->header.stamp);
double infoStamp = timestampFromROS(cameraInfo->header.stamp);
double stampDiff = fabs(rgbStamp - depthStamp);
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
rgbStamp,
depthStamp);
}
rtabmap_ros::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfo->header.frame_id;
src/nodelets/rgbdicp_odometry.cpp
+24 -2
View File
@@ -110,7 +110,9 @@ private:
bool approxSync = true;
bool subscribeScanCloud = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
@@ -126,6 +128,8 @@ private:
pnh.param("keep_color", keepColor_, keepColor_);
NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("RGBDIcpOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
@@ -154,6 +158,8 @@ private:
if(approxSync)
{
approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
if(approxSyncMaxInterval > 0.0)
approxCloudSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxCloudSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -162,9 +168,10 @@ private:
exactCloudSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackCloud, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s, \n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name(),
@@ -176,6 +183,8 @@ private:
if(approxSync)
{
approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
if(approxSyncMaxInterval > 0.0)
approxScanSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxScanSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackScan, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -184,9 +193,10 @@ private:
exactScanSync_->registerCallback(std::bind(&RGBDICPOdometry::callbackScan, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s \\\n %s \\\n %s \\\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
image_mono_sub_.getSubscriber().getTopic().c_str(),
image_depth_sub_.getSubscriber().getTopic().c_str(),
info_sub_.getSubscriber()->get_topic_name(),
@@ -277,6 +287,18 @@ private:
return;
}
double stampDiff = fabs(image->header.stamp.toSec() - depth->header.stamp.toSec());
if(stampDiff > 0.010)
{
NODELET_WARN("The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
stampDiff,
image->header.stamp.toSec(),
depth->header.stamp.toSec());
}
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
src/nodelets/rgbdx_sync.cpp
+34 -1
View File
@@ -47,13 +47,17 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
int queueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: rgbd_cameras = %d", get_name(), rgbdCameras);
@@ -74,33 +78,62 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
if(rgbdCameras==2)
{
SYNC_DECL2(RGBDXSync, rgbd2, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd2ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==3)
{
SYNC_DECL3(RGBDXSync, rgbd3, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd3ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==4)
{
SYNC_DECL4(RGBDXSync, rgbd4, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd4ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==5)
{
SYNC_DECL5(RGBDXSync, rgbd5, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd5ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==6)
{
SYNC_DECL6(RGBDXSync, rgbd6, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd6ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==7)
{
SYNC_DECL7(RGBDXSync, rgbd7, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd7ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
else if(rgbdCameras==8)
{
SYNC_DECL8(RGBDXSync, rgbd8, approxSync, queueSize, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), (*rgbdSubs_[6]), (*rgbdSubs_[7]));
if(approxSync && approxSyncMaxInterval>0.0)
{
rgbd8ApproximateSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
}
}
RCLCPP_INFO(this->get_logger(), "%s", subscribedTopicsMsg_.c_str());
RCLCPP_INFO(this->get_logger(), "%s%s", subscribedTopicsMsg_.c_str(),
approxSync&&approxSyncMaxInterval!=0.0?uFormat(" (approx sync max interval=%fs)", approxSyncMaxInterval).c_str():"");
warningThread_ = new std::thread([&](){
rclcpp::Rate r(1/5.0);
src/nodelets/stereo_odometry.cpp
+492 -211
View File
@@ -31,9 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_geometry/stereo_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap_ros/msg/rgbd_images.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
@@ -50,6 +49,12 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
rtabmap_ros::OdometryROS("stereo_odometry", options),
approxSync_(0),
exactSync_(0),
approxSync2_(0),
exactSync2_(0),
approxSync3_(0),
exactSync3_(0),
approxSync4_(0),
exactSync4_(0),
queueSize_(5),
keepColor_(false)
{
@@ -66,13 +71,19 @@ void StereoOdometry::onOdomInit()
{
bool approxSync = false;
bool subscribeRGBD = false;
double approxSyncMaxInterval = 0.0;
int rgbdCameras = 1;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize_ = this->declare_parameter("queue_size", queueSize_);
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync = %s", approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "StereoOdometry: approx_sync_max_interval = %f", approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "StereoOdometry: queue_size = %d", queueSize_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos = %d", (int)qos());
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: qos_camera_info = %d", qosCamInfo);
@@ -82,12 +93,133 @@ void StereoOdometry::onOdomInit()
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
if(rgbdCameras >= 2)
{
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
if(rgbdCameras >= 3)
{
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 4)
{
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdSub_->get_topic_name());
if(rgbdCameras == 2)
{
if(approxSync)
{
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync2_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
else
{
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str());
}
else if(rgbdCameras == 3)
{
if(approxSync)
{
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync3_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
{
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str());
}
else if(rgbdCameras == 4)
{
if(approxSync)
{
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync4_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
{
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str());
}
else
{
RCLCPP_FATAL(this->get_logger(), "%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", get_name(), rgbdCameras);
}
}
else if(rgbdCameras == 0)
{
rgbdxSub_ = create_subscription<rtabmap_ros::msg::RGBDImages>("rgbd_images", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdxSub_->get_topic_name());
}
else
{
rgbdSub_ = create_subscription<rtabmap_ros::msg::RGBDImage>("rgbd_image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
subscribedTopicsMsg =
uFormat("\n%s subscribed to:\n %s",
get_name(),
rgbdSub_->get_topic_name());
}
}
else
{
@@ -100,6 +232,8 @@ void StereoOdometry::onOdomInit()
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -108,13 +242,14 @@ void StereoOdometry::onOdomInit()
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
imageRectLeft_.getSubscriber().getTopic().c_str(),
imageRectRight_.getSubscriber().getTopic().c_str(),
cameraInfoLeft_.getSubscriber()->get_topic_name(),
cameraInfoRight_.getSubscriber()->get_topic_name());
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
@@ -131,62 +266,111 @@ void StereoOdometry::updateParameters(ParametersMap & parameters)
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
}
void StereoOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
void StereoOdometry::commonCallback(
const std::vector<cv_bridge::CvImageConstPtr> & leftImages,
const std::vector<cv_bridge::CvImageConstPtr> & rightImages,
const std::vector<sensor_msgs::msg::CameraInfo>& leftCameraInfos,
const std::vector<sensor_msgs::msg::CameraInfo>& rightCameraInfos)
{
callbackCalled();
if(!this->isPaused())
UASSERT(leftImages.size() > 0 &&
leftImages.size() == rightImages.size() &&
leftImages.size() == leftCameraInfos.size() &&
rightImages.size() == rightCameraInfos.size());
rclcpp::Time higherStamp;
int leftWidth = leftImages[0]->image.cols;
int leftHeight = leftImages[0]->image.rows;
int rightWidth = rightImages[0]->image.cols;
int rightHeight = rightImages[0]->image.rows;
UASSERT_MSG(
leftWidth == rightWidth && leftHeight == rightHeight,
uFormat("left=%dx%d right=%dx%d", leftWidth, leftHeight, rightWidth, rightHeight).c_str());
int cameraCount = leftImages.size();
cv::Mat left;
cv::Mat right;
std::vector<rtabmap::StereoCameraModel> cameraModels;
for(unsigned int i=0; i<leftImages.size(); ++i)
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
if(!(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
leftImages[i]->encoding.c_str(), rightImages[i]->encoding.c_str());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
rclcpp::Time stamp = timestampFromROS(leftImages[i]->header.stamp)>timestampFromROS(rightImages[i]->header.stamp)?leftImages[i]->header.stamp:rightImages[i]->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = rtabmap_ros::getTransform(this->frameId(), leftImages[i]->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
{
return;
}
if(imageRectLeft->data.size() && imageRectRight->data.size())
if(i>0)
{
double stampDiff = fabs(timestampFromROS(leftImages[i]->header.stamp) - timestampFromROS(leftImages[i-1]->header.stamp));
if(stampDiff > 1.0/60.0)
{
static bool warningShown = false;
if(!warningShown)
{
RCLCPP_WARN(this->get_logger(), "The time difference between cameras %d and %d is "
"high (diff=%fs, cam%d=%fs, cam%d=%fs). You may want "
"to set approx_sync_max_interval to reject bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp. This "
"message is only printed once.",
i-1, i,
stampDiff,
i-1, timestampFromROS(leftImages[i-1]->header.stamp),
i, timestampFromROS(leftImages[i]->header.stamp));
warningShown = true;
}
}
}
int quality = -1;
if(!leftImages[i]->image.empty() && !rightImages[i]->image.empty())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.frame_id,
cameraInfoLeft->header.stamp,
stereoTransform = rtabmap_ros::getTransform(
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
tfBuffer(),
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras! (between frames %s and %s)",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
else if(stereoTransform.isIdentity())
@@ -195,20 +379,20 @@ void StereoOdometry::callback(
"Identity transform returned between left and right cameras. Verify that if TF between "
"the cameras is valid: \"rosrun tf tf_echo %s %s\".",
Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(),
cameraInfoLeft->header.frame_id.c_str());
rightCameraInfos[i].header.frame_id.c_str(),
leftCameraInfos[i].header.frame_id.c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform);
if(stereoModel.baseline() == 0 && alreadyRectified)
{
stereoTransform = getTransform(
cameraInfoLeft->header.frame_id,
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.stamp,
stereoTransform = rtabmap_ros::getTransform(
leftCameraInfos[i].header.frame_id,
rightCameraInfos[i].header.frame_id,
leftCameraInfos[i].header.stamp,
tfBuffer(),
waitForTransform());
@@ -221,7 +405,7 @@ void StereoOdometry::callback(
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
cameraInfoRight->header.frame_id.c_str(), cameraInfoLeft->header.frame_id.c_str(), stereoTransform.x());
rightCameraInfos[i].header.frame_id.c_str(), leftCameraInfos[i].header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
@@ -234,7 +418,6 @@ void StereoOdometry::callback(
stereoModel.left().imageSize());
}
}
if(alreadyRectified && stereoModel.baseline() <= 0)
{
RCLCPP_ERROR(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
@@ -254,35 +437,111 @@ void StereoOdometry::callback(
shown = true;
}
}
cv_bridge::CvImageConstPtr ptrLeft = leftImages[i];
if(leftImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
if(keepColor_ && leftImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "bgr8");
}
else
{
ptrLeft = cv_bridge::cvtColor(leftImages[i], "mono8");
}
}
cv_bridge::CvImageConstPtr ptrRight = rightImages[i];
if(rightImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
rightImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrRight = cv_bridge::cvtColor(rightImages[i], "mono8");
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft,
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight,
imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":"mono8");
// initialize
if(left.empty())
{
left = cv::Mat(leftHeight, leftWidth*cameraCount, ptrLeft->image.type());
}
if(right.empty())
{
right = cv::Mat(rightHeight, rightWidth*cameraCount, ptrRight->image.type());
}
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
if(ptrLeft->image.type() == left.type())
{
ptrLeft->image.copyTo(cv::Mat(left, cv::Rect(i*leftWidth, 0, leftWidth, leftHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some left images are not the same type! %d vs %d", ptrLeft->image.type(), left.type());
return;
}
std_msgs::msg::Header header;
header.stamp = stamp;
header.frame_id = imageRectLeft->header.frame_id;
this->processData(data, header);
if(ptrRight->image.type() == right.type())
{
ptrRight->image.copyTo(cv::Mat(right, cv::Rect(i*rightWidth, 0, rightWidth, rightHeight)));
}
else
{
RCLCPP_ERROR(this->get_logger(), "Some right images are not the same type! %d vs %d", ptrRight->image.type(), right.type());
return;
}
cameraModels.push_back(stereoModel);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
RCLCPP_ERROR(this->get_logger(), "Odom: input images empty?!?");
return;
}
}
//
rtabmap::SensorData data(
left,
right,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
std_msgs::msg::Header header;
header.stamp = higherStamp;
header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:"";
this->processData(data, header);
}
void StereoOdometry::callback(
const sensor_msgs::msg::Image::ConstSharedPtr imageRectLeft,
const sensor_msgs::msg::Image::ConstSharedPtr imageRectRight,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
leftMsgs[0] = cv_bridge::toCvShare(imageRectLeft);
rightMsgs[0] = cv_bridge::toCvShare(imageRectRight);
leftInfoMsgs.push_back(*cameraInfoLeft);
rightInfoMsgs.push_back(*cameraInfoRight);
double stampDiff = fabs(timestampFromROS(imageRectLeft->header.stamp) - timestampFromROS(imageRectRight->header.stamp));
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
"synchronized, use approx_sync:=false. Otherwise, you may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
stampDiff,
timestampFromROS(imageRectLeft->header.stamp),
timestampFromROS(imageRectRight->header.stamp));
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::callbackRGBD(
@@ -291,157 +550,119 @@ void StereoOdometry::callbackRGBD(
callbackCalled();
if(!this->isPaused())
{
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(1);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
leftInfoMsgs.push_back(image->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0))
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::callbackRGBDX(
const rtabmap_ros::msg::RGBDImages::ConstSharedPtr images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
{
RCLCPP_ERROR(this->get_logger(), "Input type must be image=mono8,mono16,rgb8,bgr8,rgba8,bgra8 (mono8 recommended), received types are %s (left) and %s (right)",
imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
RCLCPP_ERROR(this->get_logger(), "Input topic \"%s\" doesn't contain any image(s)!", rgbdxSub_->get_topic_name());
return;
}
rclcpp::Time stamp = timestampFromROS(imageRectLeft->header.stamp)>timestampFromROS(imageRectRight->header.stamp)?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, tfBuffer(), waitForTransform());
if(localTransform.isNull())
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(images->rgbd_images.size());
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(images->rgbd_images.size());
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
for(size_t i=0; i<images->rgbd_images.size(); ++i)
{
return;
rtabmap_ros::toCvShare(images->rgbd_images[i], images, leftMsgs[i], rightMsgs[i]);
leftInfoMsgs.push_back(images->rgbd_images[i].rgb_camera_info);
rightInfoMsgs.push_back(images->rgbd_images[i].depth_camera_info);
}
if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
{
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
image->depth_camera_info.header.frame_id,
image->rgb_camera_info.header.frame_id,
image->rgb_camera_info.header.stamp,
tfBuffer(),
waitForTransform());
if(stereoTransform.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgb_camera_info, image->depth_camera_info, localTransform);
void StereoOdometry::callbackRGBD2(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(2);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
if(stereoModel.baseline() == 0 && alreadyRectified)
{
stereoTransform = getTransform(
image->rgb_camera_info.header.frame_id,
image->depth_camera_info.header.frame_id,
image->rgb_camera_info.header.stamp,
tfBuffer(),
waitForTransform());
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
if(!stereoTransform.isNull() && stereoTransform.x()>0)
{
static bool warned = false;
if(!warned)
{
RCLCPP_WARN(get_logger(), "Right camera info doesn't have Tx set but we are assuming that stereo images are already rectified (see %s parameter). While not "
"recommended, we used TF to get the baseline (%s->%s = %fm) for convenience (e.g., D400 ir stereo issue). It is preferred to feed "
"a valid right camera info if stereo images are already rectified. This message is only printed once...",
rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str(),
image->depth_camera_info.header.frame_id.c_str(), image->rgb_camera_info.header.frame_id.c_str(), stereoTransform.x());
warned = true;
}
stereoModel = rtabmap::StereoCameraModel(
stereoModel.left().fx(),
stereoModel.left().fy(),
stereoModel.left().cx(),
stereoModel.left().cy(),
stereoTransform.x(),
stereoModel.localTransform(),
stereoModel.left().imageSize());
}
}
void StereoOdometry::callbackRGBD3(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(3);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_ros::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
RCLCPP_FATAL(this->get_logger(), "The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
RCLCPP_WARN(this->get_logger(), "Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
void StereoOdometry::callbackRGBD4(
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_ros::msg::RGBDImage::ConstSharedPtr image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(4);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_ros::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_ros::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_ros::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
rtabmap_ros::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
leftInfoMsgs.push_back(image4->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
rightInfoMsgs.push_back(image4->depth_camera_info);
cv::Mat left;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
if(keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) != 0)
{
left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
}
else
{
left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
}
}
else
{
left = imageRectLeft->image.clone();
}
cv::Mat right;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
}
else
{
right = imageRectRight->image.clone();
}
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
left,
right,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
std_msgs::msg::Header header;
header.stamp = stamp;
header.frame_id = image->header.frame_id;
this->processData(data, header);
}
else
{
RCLCPP_WARN(this->get_logger(), "Odom: input images empty?!?");
}
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
@@ -460,6 +681,66 @@ void StereoOdometry::flushCallbacks()
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(std::bind(&StereoOdometry::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(approxSync2_)
{
delete approxSync2_;
approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
MyApproxSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
approxSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
if(exactSync2_)
{
delete exactSync2_;
exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
MyExactSync2Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_);
exactSync2_->registerCallback(std::bind(&StereoOdometry::callbackRGBD2, this, std::placeholders::_1, std::placeholders::_2));
}
if(approxSync3_)
{
delete approxSync3_;
approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
MyApproxSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
approxSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(exactSync3_)
{
delete exactSync3_;
exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
MyExactSync3Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_);
exactSync3_->registerCallback(std::bind(&StereoOdometry::callbackRGBD3, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
if(approxSync4_)
{
delete approxSync4_;
approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
MyApproxSync4Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
approxSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
if(exactSync4_)
{
delete exactSync4_;
exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
MyExactSync4Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
}
}
src/nodelets/stereo_sync.cpp
+19 -1
View File
@@ -50,14 +50,17 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
{
int queueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
int qosCamInfo = this->declare_parameter("qos_camera_info", qos);
compressedRate_ = this->declare_parameter("compressed_rate", compressedRate_);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: queue_size = %d", get_name(), queueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
RCLCPP_INFO(this->get_logger(), "%s: qos_camera_info = %d", get_name(), qosCamInfo);
@@ -69,6 +72,8 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeftSub_, imageRightSub_, cameraInfoLeftSub_, cameraInfoRightSub_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
@@ -83,9 +88,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
cameraInfoLeftSub_.subscribe(this, "left/camera_info"), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile();
cameraInfoRightSub_.subscribe(this, "right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
imageLeftSub_.getSubscriber().getTopic().c_str(),
imageRightSub_.getSubscriber().getTopic().c_str(),
cameraInfoLeftSub_.getSubscriber()->get_topic_name(),
@@ -135,6 +141,18 @@ void StereoSync::callback(
double leftStamp = timestampFromROS(imageLeft->header.stamp);
double rightStamp = timestampFromROS(imageRight->header.stamp);
double stampDiff = fabs(leftStamp - rightStamp);
if(stampDiff > 0.010)
{
RCLCPP_WARN(this->get_logger(), "The time difference between left and right frames is "
"high (diff=%fs, left=%fs, right=%fs). If your left and right cameras are hardware "
"synchronized, use approx_sync:=false. Otherwise, you may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations.",
stampDiff,
leftStamp,
rightStamp);
}
rtabmap_ros::msg::RGBDImage::UniquePtr msg(new rtabmap_ros::msg::RGBDImage);
msg->header.frame_id = cameraInfoLeft->header.frame_id;
msg->header.stamp = leftStamp>rightStamp?imageLeft->header.stamp:imageRight->header.stamp;
src/nodelets/stereo_throttle.cpp
+9 -3
View File
@@ -88,18 +88,24 @@ private:
int queueSize = 5;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync_max_interval", approxSyncMaxInterval, approxSyncMaxInterval);
pnh.param("rate", rate_, rate_);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("decimation", decimation_, decimation_);
ROS_ASSERT(decimation_ >= 1);
NODELET_INFO("Rate=%f Hz", rate_);
NODELET_INFO("Decimation=%d", decimation_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
NODELET_INFO("rate=%f Hz", rate_);
NODELET_INFO("decimation=%d", decimation_);
NODELET_INFO("approx_sync = %s", approxSync?"true":"false");
if(approxSync)
NODELET_INFO("approx_sync_max_interval = %f", approxSyncMaxInterval);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageLeft_, imageRight_, cameraInfoLeft_, cameraInfoRight_);
if(approxSyncMaxInterval>0.0)
approxSync_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval));
approxSync_->registerCallback(std::bind(&StereoThrottleNodelet::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
}
else
src/rviz/MapCloudDisplay.cpp
+5 -5
View File
@@ -167,14 +167,14 @@ MapCloudDisplay::MapCloudDisplay()
"Filter the floor up to maximum height set here "
"(only appropriate for 2D mapping).",
this, SLOT( updateCloudParameters() ), this );
cloud_filter_floor_height_->setMin( 0.0f );
cloud_filter_floor_height_->setMin( -999.0f );
cloud_filter_floor_height_->setMax( 999.0f );
cloud_filter_ceiling_height_ = new rviz_common::properties::FloatProperty( "Filter ceiling (m)", 0.0f,
"Filter the ceiling at the specified height set here "
"(only appropriate for 2D mapping).",
this, SLOT( updateCloudParameters() ), this );
cloud_filter_ceiling_height_->setMin( 0.0f );
cloud_filter_ceiling_height_->setMin( -999.0f );
cloud_filter_ceiling_height_->setMax( 999.0f );
node_filtering_radius_ = new rviz_common::properties::FloatProperty( "Node filtering radius (m)", 0.0f,
@@ -308,13 +308,13 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::msg::MapData& map)
cloud = rtabmap::util3d::voxelize(cloud, validIndices, cloud_voxel_size_->getFloat());
}
if(cloud_filter_floor_height_->getFloat() > 0.0f || cloud_filter_ceiling_height_->getFloat() > 0.0f)
if(cloud_filter_floor_height_->getFloat() != 0.0f || cloud_filter_ceiling_height_->getFloat() != 0.0f)
{
// convert in /odom frame
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose());
cloud = rtabmap::util3d::passThrough(cloud, "z",
cloud_filter_floor_height_->getFloat()>0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()>0.0f && (cloud_filter_floor_height_->getFloat()<=0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
// convert back in /base_link frame
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
}