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

Fixed bug where map id was incremented when recovering from odometry lost

Browse Source
This commit is contained in:
matlabbe
2018-06-15 16:51:44 -04:00
parent 1ffb287af0
commit 739923694c
1 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+18 -11
View File
@@ -828,20 +828,27 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti
{
if(!paused_)
{
Transform odom = rtabmap_ros::getTransform(odomMsg->header.frame_id, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(odom.isNull())
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
if(!odom.isNull())
{
static bool shown = false;
if(!shown)
Transform odomTF = rtabmap_ros::getTransform(odomMsg->header.frame_id, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0);
if(odomTF.isNull())
{
NODELET_WARN("We received odometry message, but we cannot get the "
"corresponding TF %s->%s at data stamp %fs (odom msg stamp is %fs). Make sure TF of odometry is "
"also published to get more accurate pose estimation. This "
"warning is only printed once.", odomMsg->header.frame_id.c_str(), frameId_.c_str(), stamp.toSec(), odomMsg->header.stamp.toSec());
shown = true;
static bool shown = false;
if(!shown)
{
NODELET_WARN("We received odometry message, but we cannot get the "
"corresponding TF %s->%s at data stamp %fs (odom msg stamp is %fs). Make sure TF of odometry is "
"also published to get more accurate pose estimation. This "
"warning is only printed once.", odomMsg->header.frame_id.c_str(), frameId_.c_str(), stamp.toSec(), odomMsg->header.stamp.toSec());
shown = true;
}
stamp = odomMsg->header.stamp;
}
else
{
odom = odomTF;
}
odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
stamp = odomMsg->header.stamp;
}
if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg->pose.covariance[0] >= BAD_COVARIANCE && odomMsg->twist.covariance[0] >= BAD_COVARIANCE)))