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

Fixed odom reset detection (new odom should not be null)

Browse Source
This commit is contained in:
matlabbe
2016-04-26 10:19:29 -04:00
parent 3f6811b156
commit af89e0c009
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -1
View File
@@ -610,7 +610,7 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
if(!paused_) if(!paused_)
{ {
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose); Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
if(!lastPose_.isIdentity() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE)) if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || odomMsg->pose.covariance[0] >= BAD_COVARIANCE))
{ {
UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]); UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomMsg->pose.covariance[0]);
rtabmap_.triggerNewMap(); rtabmap_.triggerNewMap();