From fa342bd85350a887454d8105b9a29eae8e2c16c2 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 8 Jun 2025 16:15:37 -0700 Subject: [PATCH 1/2] removed ros1 workflow --- .github/workflows/ros1.yml | 67 -------------------------------------- 1 file changed, 67 deletions(-) delete mode 100644 .github/workflows/ros1.yml diff --git a/.github/workflows/ros1.yml b/.github/workflows/ros1.yml deleted file mode 100644 index c0d6c707..00000000 --- a/.github/workflows/ros1.yml +++ /dev/null @@ -1,67 +0,0 @@ -name: ros1 - -on: - push: - branches: [ master ] - pull_request: - branches: [ master ] - -env: - # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) - BUILD_TYPE: Release - -jobs: - build: - # Disabling because Ubuntu 20.04 doesn't exist anymore on CI: - # This is a scheduled Ubuntu 20.04 retirement. Ubuntu 20.04 LTS - # runner will be removed on 2025-04-15. For more details, see https://github.com/actions/runner-images/issues/11101 - if: false - - # The CMake configure and build commands are platform agnostic and should work equally - # well on Windows or Mac. You can convert this to a matrix build if you need - # cross-platform coverage. - # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix - name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }} - runs-on: ${{ matrix.os }} - strategy: - matrix: - os: [ubuntu-20.04] - include: - - os: ubuntu-20.04 - ros_distro: 'noetic' - - - steps: - - uses: ros-tooling/setup-ros@v0.2 - with: - required-ros-distributions: ${{ matrix.ros_distro }} - - - name: Install dependencies - run: | - sudo apt-get update - sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools - sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap - sudo pip3 uninstall empy --yes - - - name: Setup catkin workspace - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - mkdir -p ${{github.workspace}}/catkin_ws/src - cd ${{github.workspace}}/catkin_ws/src - cd .. - catkin config --init --cmake-args -DSETUPTOOLS_DEB_LAYOUT=OFF -DCMAKE_C_FLAGS="-Wformat -Werror=format-security" -DCMAKE_CXX_FLAGS="-Wformat -Werror=format-security" - - - uses: actions/checkout@v2 - with: - repository: 'introlab/rtabmap' - path: 'catkin_ws/src/rtabmap' - - - uses: actions/checkout@v2 - with: - path: 'catkin_ws/src/rtabmap_ros' - - - name: caktkin build - run: | - source /opt/ros/${{ matrix.ros_distro }}/setup.bash - cd ${{github.workspace}}/catkin_ws - catkin build -p 1 -i --verbose From 3cc9db8f87ade67eb5a80a2a2caabfe9a83d056c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 3 Jul 2025 17:50:38 -0700 Subject: [PATCH 2/2] Odom reset on time jump in the past (#1333) * 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 --- .../include/rtabmap_odom/OdometryROS.h | 5 +- rtabmap_odom/src/OdometryROS.cpp | 106 ++++++++++++++---- rtabmap_odom/src/nodelets/icp_odometry.cpp | 12 +- rtabmap_slam/src/CoreWrapper.cpp | 18 +++ .../include/rtabmap_sync/SyncDiagnostic.h | 27 ++++- 5 files changed, 136 insertions(+), 32 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index c5672d36..24d20160 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -87,7 +87,7 @@ protected: tf::TransformListener & tfListener() {return tfListener_;} double waitForTransformDuration() const {return waitForTransform_?waitForTransformDuration_:0.0;} rtabmap::Transform velocityGuess() const; - double previousStamp() const {return previousStamp_;} + ros::Time previousStamp() const {return previousStamp_;} virtual void postProcessData(const rtabmap::SensorData & data, const std_msgs::Header & header) const {} private: @@ -158,7 +158,8 @@ private: bool icpParams_; rtabmap::Transform guess_; rtabmap::Transform guessPreviousPose_; - double previousStamp_; + ros::Time previousStamp_; + ros::Time previousClockTime_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index f79fca44..f5b54ba9 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -78,7 +78,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : stereoParams_(stereoParams), visParams_(visParams), icpParams_(icpParams), - previousStamp_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), @@ -543,27 +542,56 @@ void OdometryROS::mainLoop() Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { - if(previousStamp_>0.0 && previousStamp_ >= header.stamp.toSec()) + // Detect time jump in the past + ros::Time clockNow = ros::Time::now(); + if(previousClockTime_ > clockNow) + { + NODELET_WARN("Odometry: Detected jump back in time of %f sec. Odometry is " + "automatically reset to latest computed pose!", + (previousClockTime_ - clockNow).toSec()); + SensorData dataCpy = dataToProcess_; + std_msgs::Header headerCpy = dataHeaderToProcess_; + ros::Time previousCpy = previousClockTime_; + this->reset(odometry_->getPose()); + if(previousCpy > headerCpy.stamp) { + // new frame is using new clock, process it now + dataToProcess_ = dataCpy; + dataHeaderToProcess_ = headerCpy; + dataReady_.release(); + NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + else { + // skip that old frame + NODELET_WARN("Odometry: skipping frame: %f (clock previous=%f, new=%f)", + headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); + } + previousClockTime_ = clockNow; + return; + } + previousClockTime_ = clockNow; + + if(previousStamp_ >= header.stamp) { NODELET_WARN("Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). " - "New stamp should be always greater than previous stamp. This new data is ignored.", - previousStamp_, header.stamp.toSec()); + "New stamp should be always greater than previous stamp. This new data is ignored. ", + previousStamp_.toSec(), header.stamp.toSec()); return; } else if(maxUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) + previousStamp_.toSec() > 0 && + ((header.stamp-previousStamp_).toSec()+(expectedUpdateRate_ > 0?1.0/expectedUpdateRate_:0)) < 1.0/maxUpdateRate_) { // throttling return; } else if(maxUpdateRate_ == 0 && expectedUpdateRate_ > 0 && - previousStamp_ > 0 && - (header.stamp.toSec()-previousStamp_) < 1.0/expectedUpdateRate_) + previousStamp_.toSec() > 0 && + (header.stamp-previousStamp_).toSec() < 1.0/expectedUpdateRate_) { NODELET_WARN("Odometry: Aborting odometry update, higher frame rate detected (%f Hz) than the expected one (%f Hz). (stamps: previous=%fs new=%fs)", - 1.0/(header.stamp.toSec()-previousStamp_), expectedUpdateRate_, previousStamp_, header.stamp.toSec()); + 1.0/(header.stamp-previousStamp_).toSec(), expectedUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); return; } @@ -632,7 +660,7 @@ void OdometryROS::mainLoop() guess_.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); if((guessMinTranslation_ <= 0.0 || uMax3(fabs(x), fabs(y), fabs(z)) < guessMinTranslation_) && (guessMinRotation_ <= 0.0 || uMax3(fabs(roll), fabs(pitch), fabs(yaw)) < guessMinRotation_) && - (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && header.stamp.toSec()-previousStamp_ < guessMinTime_))) + (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_))) { // Ignore odometry update, we didn't move enough if(publishTf_) @@ -643,7 +671,16 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } guessPreviousPose_ = guessCurrentPose; return; @@ -658,7 +695,7 @@ void OdometryROS::mainLoop() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (header.stamp.toSec()-previousStamp_) > 1.0/minUpdateRate_; + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; // process data ros::WallTime time = ros::WallTime::now(); @@ -697,11 +734,29 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } else { - tfBroadcaster_.sendTransform(poseMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(poseMsg); + } + else { + ROS_WARN("TF %s->%s is not published because we detected a time jump in the past of %f sec.", + poseMsg.header.frame_id.c_str(), + poseMsg.child_frame_id.c_str(), + (previousClockTime_ - time_now).toSec()); + } } } @@ -893,7 +948,18 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_.sendTransform(correctionMsg); + ros::Time time_now = ros::Time::now(); + if(time_now >= previousClockTime_) { + tfBroadcaster_.sendTransform(correctionMsg); + } + else { + ROS_WARN("TF %s->%s is not published because its stamp (%f) is greater " + "than current time (%f), possible time jump happened!", + correctionMsg.header.frame_id.c_str(), + correctionMsg.child_frame_id.c_str(), + correctionMsg.header.stamp.toSec(), + time_now.toSec()); + } } } @@ -903,7 +969,7 @@ void OdometryROS::mainLoop() { NODELET_WARN( "Odometry lost! Odometry will be reset because last update " "is %fs too old (>%fs, min_update_rate = %f Hz). Previous data stamp is %f while new data stamp is %f.", - header.stamp.toSec() - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, header.stamp.toSec()); + (header.stamp - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); } else if(--resetCurrentCount_>0) { @@ -1098,10 +1164,10 @@ void OdometryROS::mainLoop() syncDiagnostic_->tick(header.stamp, maxUpdateRate_>0 ? maxUpdateRate_: expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_: - previousStamp_ == 0.0 || header.stamp.toSec() - previousStamp_ > 1.0/curentRate?0:curentRate); + previousStamp_.toSec() == 0.0 || (header.stamp - previousStamp_).toSec() > 1.0/curentRate?0:curentRate); } - previousStamp_ = header.stamp.toSec(); + previousStamp_ = header.stamp; } bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&) @@ -1125,7 +1191,8 @@ void OdometryROS::reset(const Transform & pose) odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); - previousStamp_ = 0.0; + previousStamp_ = ros::Time(); + previousClockTime_ = ros::Time(); resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); @@ -1135,6 +1202,7 @@ void OdometryROS::reset(const Transform & pose) imus_.clear(); imuMutex_.unlock(); this->flushCallbacks(); + this->tfListener().clear(); } bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&) diff --git a/rtabmap_odom/src/nodelets/icp_odometry.cpp b/rtabmap_odom/src/nodelets/icp_odometry.cpp index 23bb280e..1c712e1c 100644 --- a/rtabmap_odom/src/nodelets/icp_odometry.cpp +++ b/rtabmap_odom/src/nodelets/icp_odometry.cpp @@ -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; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 6a3ab207..ff930a44 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1023,6 +1023,15 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + Transform odom = rtabmap_conversions::transformFromPoseMsg(odomMsg->pose.pose); if(!odom.isNull()) { @@ -1134,6 +1143,15 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) { if(!paused_) { + // Check time jump in the past + if(stamp < previousStamp_) { + ROS_WARN("Detected time jump in the past of %f sec (previous stamp=%f, current stamp=%f). Resetting internal stamps and abort!", + previousStamp_.toSec() - stamp.toSec(), previousStamp_.toSec(), stamp.toSec()); + previousStamp_ = ros::Time(); + tfListener_.clear(); + return false; + } + // Odom TF ready? Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, tfListener_, waitForTransform_?waitForTransformDuration_:0.0); if(odom.isNull()) diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index dfc3e384..59f9181f 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -19,7 +19,9 @@ class SyncDiagnostic { compositeTask_("Sync status"), lastCallbackCalledStamp_(ros::Time::now().toSec()-1), targetFrequency_(0.0), - windowSize_(windowSize) + windowSize_(windowSize), + lastTickTime_(0.0), + nodeName_(nodeName) { UASSERT(windowSize_ >= 1); } @@ -46,7 +48,7 @@ class SyncDiagnostic { } diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/")); diagnosticUpdater_.force_update(); - diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this); + diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(5), &SyncDiagnostic::diagnosticTimerCallback, this); } void tick(const ros::Time & stamp, double targetFrequency = 0) @@ -79,16 +81,29 @@ class SyncDiagnostic { targetFrequency_ = targetFrequency; } lastCallbackCalledStamp_ = stamp.toSec(); + + double clockNow = ros::Time::now().toSec(); + if(lastTickTime_ > clockNow) + { + ROS_WARN("%s: Detected time jump in the past of %f sec, forcing diagnostic update.", + nodeName_.c_str(), lastTickTime_ - clockNow); + frequencyStatus_.clear(); + diagnosticUpdater_.force_update(); + lastCallbackCalledStamp_ = clockNow; + } + else + { + diagnosticUpdater_.update(); + } + lastTickTime_ = clockNow; } private: void diagnosticTimerCallback(const ros::TimerEvent& event) { - diagnosticUpdater_.update(); - if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty()) { - ROS_WARN_THROTTLE(5, "%s", topicsNotReceivedWarningMsg_.c_str()); + ROS_WARN("%s", topicsNotReceivedWarningMsg_.c_str()); } } @@ -103,6 +118,8 @@ private: double targetFrequency_; int windowSize_; std::deque window_; + double lastTickTime_; + std::string nodeName_; };