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

Odom reset on time jump in the past (#1333)

Browse Source 
* Odom reset on time jump

* Adding more logs to debug

* refactored

* dont skip frame on clock jump

* Making clock check independent of the topic stamp check

* fixed post check

* Making diagnostic more robust to time jump

* Added node name to warning

* make sync warning msg working in case of time jump

* not need to reset timer

* reset timer

* timer auto reset already

* typo

* Added more time checks to make sure we don't republish a tf frame with stamp from a topic in the future

* dont send tf if time jump happened while processing

* fixed errors

* addressing comments

* fixing time comparison
This commit is contained in:
matlabbe
2025-07-03 17:50:38 -07:00
committed by GitHub
parent fa342bd853
commit 3cc9db8f87
5 changed files with 136 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/nodelets/icp_odometry.cpp
+6 -6
View File
@@ -380,11 +380,11 @@ private:
-1.0,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
if(guessFrameId().empty() && previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
{
// deskew with constant velocity model (we are in frameId)
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess()))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
@@ -405,11 +405,11 @@ private:
{
projection.projectLaser(*scanMsg, scanOut, -1.0, laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
if(deskewing_ && previousStamp() > 0 && !velocityGuess().isNull())
if(deskewing_ && previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
{
// deskew with constant velocity model
sensor_msgs::PointCloud2 scanOutDeskewed;
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp(), velocityGuess()))
if(!rtabmap_conversions::deskew(scanOut, scanOutDeskewed, previousStamp().toSec(), velocityGuess()))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;
@@ -628,7 +628,7 @@ private:
return;
}
}
else if(previousStamp() > 0 && !velocityGuess().isNull())
else if(previousStamp().toSec() > 0.0 && !velocityGuess().isNull())
{
// deskew with constant velocity model
bool alreadyInBaseFrame = frameId().compare(pointCloudMsg->header.frame_id) == 0;
@@ -648,7 +648,7 @@ private:
}
sensor_msgs::PointCloud2::Ptr cloudDeskewed(new sensor_msgs::PointCloud2);
if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp(), velocityGuess()))
if(!rtabmap_conversions::deskew(*cloudPtr, *cloudDeskewed, previousStamp().toSec(), velocityGuess()))
{
ROS_ERROR("Failed to deskew input cloud, aborting odometry update!");
return;