diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index fe515e05..4a1b7a81 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -173,6 +173,7 @@ private: rtabmap::Transform guess_; rtabmap::Transform guessPreviousPose_; double previousStamp_; + double previousClockTime_; double expectedUpdateRate_; double maxUpdateRate_; double minUpdateRate_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index cfbbf7b5..c9955d99 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -84,7 +84,11 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o paused_(false), resetCountdown_(0), resetCurrentCount_(0), + stereoParams_(false), + visParams_(false), + icpParams_(false), previousStamp_(0.0), + previousClockTime_(0.0), expectedUpdateRate_(0.0), maxUpdateRate_(0.0), minUpdateRate_(0.0), @@ -577,7 +581,36 @@ void OdometryROS::mainLoop() Transform groundTruth; if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) { - if(previousStamp_>0.0 && previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp)) + // Detect time jump in the past + double clockNow = now().seconds(); + if(previousClockTime_ > clockNow) + { + RCLCPP_WARN(this->get_logger(), "Odometry: Detected jump back in time of %f sec. Odometry is " + "automatically reset to latest computed pose!", + previousClockTime_ - clockNow); + SensorData dataCpy = dataToProcess_; + std_msgs::msg::Header headerCpy = dataHeaderToProcess_; + double previousCpy = previousClockTime_; + this->reset(odometry_->getPose()); + if(clockNow > rtabmap_conversions::timestampFromROS(headerCpy.stamp)) { + // new frame is using new clock, process it now + dataToProcess_ = dataCpy; + dataHeaderToProcess_ = headerCpy; + dataReady_.release(); + RCLCPP_WARN(this->get_logger(), "Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", + rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow); + } + else { + // skip that old frame + RCLCPP_WARN(this->get_logger(), "Odometry: skipping frame: %f (clock previous=%f, new=%f)", + rtabmap_conversions::timestampFromROS(headerCpy.stamp), previousCpy, clockNow); + } + previousClockTime_ = clockNow; + return; + } + previousClockTime_ = clockNow; + + if(previousStamp_ >= rtabmap_conversions::timestampFromROS(header.stamp)) { RCLCPP_WARN(this->get_logger(), "Odometry: Detected not valid consecutive stamps (previous=%fs new=%fs). " "New stamp should be always greater than previous stamp. This new data is ignored.", @@ -677,7 +710,17 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "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); + } } guessPreviousPose_ = guessCurrentPose; return; @@ -731,11 +774,30 @@ void OdometryROS::mainLoop() correctionMsg.header.stamp = header.stamp; Transform correction = pose * guessCurrentPose.inverse(); rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); - tfBroadcaster_->sendTransform(correctionMsg); + + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "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); + } } else { - tfBroadcaster_->sendTransform(poseMsg); + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(poseMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "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); + } } } @@ -927,7 +989,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); + double time_now = now().seconds(); + if(time_now >= previousClockTime_) { + tfBroadcaster_->sendTransform(correctionMsg); + } + else { + RCLCPP_WARN(this->get_logger(), "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(), + rtabmap_conversions::timestampFromROS(correctionMsg.header.stamp), + time_now); + } } } @@ -1167,6 +1240,7 @@ void OdometryROS::reset(const Transform & pose) guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; + previousClockTime_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; dataToProcess_ = SensorData(); diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index c6a20333..c214d91a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -718,12 +718,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : mapToOdomMutex_.lock(); if(!odomFrameId_.empty()) { - rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance); geometry_msgs::msg::TransformStamped msg; + rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); msg.child_frame_id = odomFrameId_; msg.header.frame_id = mapFrameId_; - msg.header.stamp = tfExpiration; - rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); + msg.header.stamp = now() + rclcpp::Duration::from_seconds(tfTolerance); tfBroadcaster_->sendTransform(msg); } mapToOdomMutex_.unlock(); diff --git a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h index 2cafd411..03b49348 100644 --- a/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h +++ b/rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h @@ -26,10 +26,10 @@ class SyncDiagnostic { inCompositeTask_("Input Status"), outCompositeTask_("Output Status"), lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), - lastTickOutputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1), inTargetFrequency_(0.0), outTargetFrequency_(0.0), - windowSize_(windowSize) + windowSize_(windowSize), + lastTickTime_(0.0) { UASSERT(windowSize_ >= 1); } @@ -76,6 +76,7 @@ class SyncDiagnostic { void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0) { + double lastTickOutputStamp; updateFrequency( stamp, expectedFrequency, @@ -83,7 +84,7 @@ class SyncDiagnostic { outTimeStampStatus_, outWindow_, outTargetFrequency_, - lastTickOutputStamp_); + lastTickOutputStamp); } private: @@ -140,6 +141,18 @@ private: } lastTickStamp = stampSec; + + double clockNow = rtabmap_conversions::timestampFromROS(node_->now()); + if(lastTickTime_ > clockNow) + { + RCLCPP_WARN(node_->get_logger(), "%s: Detected time jump in the past of %f sec, forcing diagnostic update.", + node_->get_name(), lastTickTime_ - clockNow); + inFrequencyStatus_.clear(); + outFrequencyStatus_.clear(); + diagnosticUpdater_.force_update(); + lastTickInputStamp_ = clockNow; + } + lastTickTime_ = clockNow; } private: @@ -154,13 +167,13 @@ private: diagnostic_updater::CompositeDiagnosticTask outCompositeTask_; rclcpp::TimerBase::SharedPtr diagnosticTimer_; double lastTickInputStamp_; - double lastTickOutputStamp_; double inTargetFrequency_; double outTargetFrequency_; int windowSize_; std::deque inWindow_; std::deque outWindow_; UMutex tickMutex_; + double lastTickTime_; };