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

Odom: on odom auto reset (Odom/ResetCountdown), if guess_frame_id is set, reset pose accordingly to guess frame since it got lost.

Browse Source
This commit is contained in:
matlabbe
2022-03-11 15:18:34 -08:00
parent 547f285448
commit 3a025d9e75
1 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+18 -9
View File
@@ -848,20 +848,29 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
{
// Check TF to see if sensor fusion is used (e.g., the output of robot_localization)
Transform tfPose = this->getTransform(odomFrameId_, frameId_, header.stamp);
if(tfPose.isNull())
if(!guess_.isNull())
{
NODELET_WARN( "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
NODELET_WARN( "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
{
NODELET_WARN( "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 = this->getTransform(odomFrameId_, frameId_, header.stamp);
if(tfPose.isNull())
{
NODELET_WARN( "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(odometry_->getPose());
}
}
}
}