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

rtabmap: added subscribe_inter_odom_info option (sync odometry and info msgs for intermediate nodes)

Browse Source
This commit is contained in:
matlabbe
2019-11-23 22:00:23 -05:00
parent 3b6792920e
commit 272568a0d9
2 changed files with 83 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+75 -13
View File
@@ -112,6 +112,7 @@ CoreWrapper::CoreWrapper() :
transformThread_(0),
tfThreadRunning_(false),
stereoToDepth_(false),
interOdomSync_(0),
odomSensorSync_(false),
rate_(Parameters::defaultRtabmapDetectionRate()),
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
@@ -518,8 +519,22 @@ void CoreWrapper::onInit()
NODELET_INFO("Create intermediate nodes");
if(rate_ == 0.0f)
{
NODELET_INFO("Subscribe to inter odom messges");
interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
bool interOdomInfo = false;
pnh.getParam("subscribe_inter_odom_info", interOdomInfo);
if(interOdomInfo)
{
NODELET_INFO("Subscribe to inter odom + info messages");
interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(queueSize_), interOdomSyncSub_, interOdomInfoSyncSub_);
interOdomSync_->registerCallback(boost::bind(&CoreWrapper::interOdomInfoCallback, this, _1, _2));
interOdomSyncSub_.subscribe(nh, "inter_odom", 1);
interOdomInfoSyncSub_.subscribe(nh, "inter_odom_info", 1);
}
else
{
NODELET_INFO("Subscribe to inter odom messages");
interOdomSub_ = nh.subscribe("inter_odom", 1, &CoreWrapper::interOdomCallback, this);
}
}
}
}
@@ -742,6 +757,7 @@ CoreWrapper::~CoreWrapper()
rtabmap_.close();
printf("rtabmap: Saving database/long-term memory...done! (located at %s, %ld MB)\n", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024));
delete interOdomSync_;
delete mbClient_;
}
@@ -1638,24 +1654,24 @@ void CoreWrapper::process(
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
// Add intermediate nodes?
for(std::list<nav_msgs::Odometry>::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
for(std::list<std::pair<nav_msgs::Odometry, rtabmap_ros::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
{
if(iter->header.stamp < lastPoseStamp_)
if(iter->first.header.stamp < lastPoseStamp_)
{
Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->pose.pose);
Transform interOdom = rtabmap_ros::transformFromPoseMsg(iter->first.pose.pose);
if(!interOdom.isNull())
{
cv::Mat covariance;
double variance = iter->twist.covariance[0];
double variance = iter->first.twist.covariance[0];
if(variance == BAD_COVARIANCE || variance <= 0.0f)
{
//use the one of the pose
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->pose.covariance.data()).clone();
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.pose.covariance.data()).clone();
covariance /= 2.0;
}
else
{
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->twist.covariance.data()).clone();
covariance = cv::Mat(6,6,CV_64FC1, (void*)iter->first.twist.covariance.data()).clone();
}
if(!uIsFinite(covariance.at<double>(0,0)) || covariance.at<double>(0,0)<=0.0f)
{
@@ -1684,18 +1700,56 @@ void CoreWrapper::process(
Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
0,
cv::Size(1,2));
SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->header.stamp));
SensorData interData(rgb, depth, model, -1, rtabmap_ros::timestampFromROS(iter->first.header.stamp));
Transform gt;
if(!groundTruthFrameId_.empty())
{
gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
gt = rtabmap_ros::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, iter->first.header.stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
}
interData.setGroundTruth(gt);
rtabmap_.process(interData, interOdom, covariance);
std::map<std::string, float> externalStats;
std::vector<float> odomVelocity;
if(iter->second.timeEstimation != 0.0f)
{
OdometryInfo info = odomInfoFromROS(iter->second);
externalStats.insert(std::make_pair("Odometry/LocalBundle/ms", info.localBundleTime*1000.0f));
externalStats.insert(std::make_pair("Odometry/LocalBundleConstraints/", info.localBundleConstraints));
externalStats.insert(std::make_pair("Odometry/LocalBundleOutliers/", info.localBundleOutliers));
externalStats.insert(std::make_pair("Odometry/TotalTime/ms", info.timeEstimation*1000.0f));
externalStats.insert(std::make_pair("Odometry/Registration/ms", info.reg.totalTime*1000.0f));
float speed = 0.0f;
if(info.interval>0.0)
speed = info.transform.x()/info.interval*3.6;
externalStats.insert(std::make_pair("Odometry/Speed/kph", speed));
externalStats.insert(std::make_pair("Odometry/Inliers/", info.reg.inliers));
externalStats.insert(std::make_pair("Odometry/Features/", info.features));
externalStats.insert(std::make_pair("Odometry/DistanceTravelled/m", info.distanceTravelled));
externalStats.insert(std::make_pair("Odometry/KeyFrameAdded/", info.keyFrameAdded));
externalStats.insert(std::make_pair("Odometry/LocalKeyFrames/", info.localKeyFrames));
externalStats.insert(std::make_pair("Odometry/LocalMapSize/", info.localMapSize));
externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", info.localScanMapSize));
externalStats.insert(std::make_pair("Odometry/RAM_usage/MB", info.memoryUsage));
if(info.interval>0.0)
{
odomVelocity.resize(6);
float x,y,z,roll,pitch,yaw;
info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odomVelocity[0] = x/info.interval;
odomVelocity[1] = y/info.interval;
odomVelocity[2] = z/info.interval;
odomVelocity[3] = roll/info.interval;
odomVelocity[4] = pitch/info.interval;
odomVelocity[5] = yaw/info.interval;
}
}
rtabmap_.process(interData, interOdom, covariance, odomVelocity, externalStats);
}
interOdoms_.erase(iter++);
}
else if(iter->header.stamp == lastPoseStamp_)
else if(iter->first.header.stamp == lastPoseStamp_)
{
interOdoms_.erase(iter++);
break;
@@ -2181,7 +2235,15 @@ void CoreWrapper::interOdomCallback(const nav_msgs::OdometryConstPtr & msg)
{
if(!paused_)
{
interOdoms_.push_back(*msg);
interOdoms_.push_back(std::make_pair(*msg, rtabmap_ros::OdomInfo()));
}
}
void CoreWrapper::interOdomInfoCallback(const nav_msgs::OdometryConstPtr & msg1, const rtabmap_ros::OdomInfoConstPtr & msg2)
{
if(!paused_)
{
interOdoms_.push_back(std::make_pair(*msg1, *msg2));
}
}