diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 582d0bc5..5961eea4 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -123,6 +123,8 @@ private: double guessMinTranslation_; double guessMinRotation_; double guessMinTime_; + double guessLinearVariance_; + double guessAngularVariance_; bool publishTf_; double waitForTransform_; bool publishNullWhenLost_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 8a96e4f4..9f7ec9ad 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -76,6 +76,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o guessMinTranslation_(0.0), guessMinRotation_(0.0), guessMinTime_(0.0), + guessLinearVariance_(0.001), + guessAngularVariance_(0.001), publishTf_(true), waitForTransform_(0.1), // 100 ms publishNullWhenLost_(true), @@ -143,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o guessMinTranslation_ = this->declare_parameter("guess_min_translation", guessMinTranslation_); guessMinRotation_ = this->declare_parameter("guess_min_rotation", guessMinRotation_); guessMinTime_ = this->declare_parameter("guess_min_time", guessMinTime_); + guessLinearVariance_ = this->declare_parameter("guess_linear_variance", guessLinearVariance_); + guessAngularVariance_ = this->declare_parameter("guess_angular_variance", guessAngularVariance_); expectedUpdateRate_ = this->declare_parameter("expected_update_rate", expectedUpdateRate_); maxUpdateRate_ = this->declare_parameter("max_update_rate", maxUpdateRate_); @@ -205,6 +209,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_translation = %f", guessMinTranslation_); RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_rotation = %f", guessMinRotation_); RCLCPP_INFO(this->get_logger(), "Odometry: guess_min_time = %f", guessMinTime_); + RCLCPP_INFO(this->get_logger(), "Odometry: guess_linear_variance = %f", guessLinearVariance_); + RCLCPP_INFO(this->get_logger(), "Odometry: guess_angular_variance = %f", guessAngularVariance_); RCLCPP_INFO(this->get_logger(), "Odometry: expected_update_rate = %f Hz", expectedUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_); RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_); @@ -710,6 +716,44 @@ void OdometryROS::processData() } } + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ > 1.0/minUpdateRate_; + if(tooOldPreviousData) + { + RCLCPP_WARN(this->get_logger(), "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.", + rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); + + if(!guess_.isNull()) + { + RCLCPP_WARN(this->get_logger(), "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(); + guessPreviousPose_.setNull(); + } + else + { + // Check TF to see if sensor fusion is used (e.g., the output of robot_localization) + Transform tfPose = rtabmap_conversions::getTransform(odomFrameId_, frameId_, header.stamp, *tfBuffer_, waitForTransform_); + if(tfPose.isNull()) + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest computed pose!"); + odometry_->reset(odometry_->getPose()); + } + else + { + RCLCPP_WARN(this->get_logger(), "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!", + odomFrameId_.c_str(), frameId_.c_str()); + odometry_->reset(tfPose); + } + } + } + + bool skipOdometryUpdate = false; + + rtabmap::Transform pose; + rtabmap::OdometryInfo info; + rtabmap::Transform guessVelocity; Transform guessCurrentPose; if(!guessFrameId_.empty()) @@ -746,28 +790,22 @@ void OdometryROS::processData() (guessMinTime_ <= 0.0 || (previousStamp_>0.0 && rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_ < guessMinTime_))) { // Ignore odometry update, we didn't move enough - if(publishTf_) - { - geometry_msgs::msg::TransformStamped correctionMsg; - correctionMsg.child_frame_id = guessFrameId_; - correctionMsg.header.frame_id = odomFrameId_; - correctionMsg.header.stamp = header.stamp; - Transform correction = odometry_->getPose() * guess_ * guessCurrentPose.inverse(); - rtabmap_conversions::transformToGeometryMsg(correction, correctionMsg.transform); + pose = odometry_->getPose() * guess_; + info.reg.covariance = cv::Mat::zeros(6,6,CV_64FC1); + info.reg.covariance.at(0,0) = guessLinearVariance_; // xx + info.reg.covariance.at(1,1) = guessLinearVariance_; // yy + info.reg.covariance.at(2,2) = guessLinearVariance_; // zz + info.reg.covariance.at(3,3) = guessAngularVariance_; // rr + info.reg.covariance.at(4,4) = guessAngularVariance_; // pp + info.reg.covariance.at(5,5) = guessAngularVariance_; // yawyaw - 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; + //set velocity + double dt = rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_; + UASSERT(dt>0.0); + // use part of guess matching dt + (previousPose.inverse() * guessCurrentPose).getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + guessVelocity = rtabmap::Transform(x/dt, y/dt, z/dt, roll/dt, pitch/dt, yaw/dt); + skipOdometryUpdate = true; } } guessPreviousPose_ = guessCurrentPose; @@ -779,23 +817,21 @@ void OdometryROS::processData() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_; - // process data rclcpp::Time timeStart = rclcpp::Clock().now(); - rtabmap::OdometryInfo info; if(!groundTruth.isNull()) { data.setGroundTruth(groundTruth); } - rtabmap::Transform pose; - if(!tooOldPreviousData) + if(!skipOdometryUpdate) { pose = odometry_->process(data, guess_, &info); } if(!pose.isNull()) { - guess_.setNull(); + if(!skipOdometryUpdate) { + guess_.setNull(); + } resetCurrentCount_ = resetCountdown_; //********************* @@ -869,11 +905,16 @@ void OdometryROS::processData() odom.pose.covariance.at(35) = info.reg.covariance.at(5,5)*2; // yawyaw //set velocity - bool setTwist = !odometry_->getVelocityGuess().isNull(); + bool setTwist = !guessVelocity.isNull() || !odometry_->getVelocityGuess().isNull(); if(setTwist) { float x,y,z,roll,pitch,yaw; - odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + if(skipOdometryUpdate) { + UASSERT(!guessVelocity.isNull()); + guessVelocity.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } else { + odometry_->getVelocityGuess().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw); + } odom.twist.twist.linear.x = x; odom.twist.twist.linear.y = y; odom.twist.twist.linear.z = z; @@ -918,7 +959,7 @@ void OdometryROS::processData() odomLocalMap_->publish(cloudMsg); } - if(odomLastFrame_->get_subscription_count()) + if(!skipOdometryUpdate && odomLastFrame_->get_subscription_count()) { // check which type of Odometry is using if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry @@ -1049,20 +1090,14 @@ void OdometryROS::processData() } - if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData)) + if(pose.isNull() && resetCurrentCount_ > 0) { - if(tooOldPreviousData) - { - RCLCPP_WARN(this->get_logger(), "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.", - rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_, 1.0/minUpdateRate_, minUpdateRate_, previousStamp_, rtabmap_conversions::timestampFromROS(header.stamp)); - } - else if(--resetCurrentCount_>0) + if(--resetCurrentCount_>0) { RCLCPP_WARN(this->get_logger(), "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); } - if(resetCurrentCount_ == 0 || tooOldPreviousData) + if(resetCurrentCount_ == 0) { if(!guess_.isNull()) { @@ -1225,9 +1260,11 @@ void OdometryROS::processData() msg.header.stamp = header.stamp; // use corresponding time stamp to image odomSensorDataCompressedPub_->publish(msg); } - double delay = (now()-header.stamp).seconds(); - if(visParams_) + if(skipOdometryUpdate) { + RCLCPP_INFO(this->get_logger(), "Odom: , std dev=%fm|%frad, update time=%fs, delay=%fs", pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay); + } + else if(visParams_) { if(icpParams_) { diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 157a9b26..8660bf81 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -334,6 +334,7 @@ private: double landmarkDefaultAngVariance_; double landmarkDefaultLinVariance_; double waitForTransform_; + double stalenessFactor_; bool useActionForGoal_; bool useSavedMap_; bool genScan_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index c235b484..528b6065 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -121,6 +121,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : landmarkDefaultAngVariance_(0.001), landmarkDefaultLinVariance_(0.001), waitForTransform_(0.2),// 200 ms + stalenessFactor_(0.0), useActionForGoal_(false), useSavedMap_(true), genScan_(false), @@ -207,6 +208,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_); waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_); + stalenessFactor_ = this->declare_parameter("staleness_factor", stalenessFactor_); initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr); useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_); #ifndef WITH_NAV2_MSGS @@ -251,6 +253,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); RCLCPP_INFO(this->get_logger(), "rtabmap: wait_for_transform = %f", waitForTransform_); + if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) { + RCLCPP_ERROR(this->get_logger(), "rtabmap: staleness_factor should be 0 (disabled) or >= 1 (value that multiplies the detection update period). Current value is %f, setting it to 0...", + stalenessFactor_); + stalenessFactor_ = 0.0; + } + RCLCPP_INFO(this->get_logger(), "rtabmap: staleness_factor = %f", stalenessFactor_); if(this->isSubscribedToStereo()) { RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false"); @@ -1138,6 +1146,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0])); triggerNewMapBeforeNextUpdate_ = true; lastPoseCovariance_ = cv::Mat(); + } + else if(stalenessFactor_>0.0 && + previousStamp_.seconds() > 0.0 && + rate_>0.0f && + (stamp - previousStamp_).seconds() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + (stamp - previousStamp_).seconds(), + stamp.seconds(), + previousStamp_.seconds(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1229,6 +1257,26 @@ bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Ti triggerNewMapBeforeNextUpdate_ = true; lastPoseCovariance_ = cv::Mat(); } + else if(stalenessFactor_>0.0 && + previousStamp_.seconds() > 0.0 && + rate_>0.0f && + (stamp - previousStamp_).seconds() > stalenessFactor_/rate_) + { + UWARN("The time difference (%f s) between the new timestamp received (%f) and " + "the previous one (%f) is way over than the expected update period (%s=%f Hz) " + "%f x staleness_factor (%f) = %f s. Triggering a new map! Set staleness_factor to 0 " + "to avoid triggering a new map when this happens.", + (stamp - previousStamp_).seconds(), + stamp.seconds(), + previousStamp_.seconds(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); + } lastPoseIntermediate_ = false; lastPose_ = odom; diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 28209840..827d43c1 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -526,7 +526,9 @@ std::map MapsManager::updateMapCaches( if(!iter->second.isNull()) { rtabmap::SensorData data; - if(iter->first == 0 || (addedNodes->find(iter->first) == addedNodes->end() && !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || ( + (fullUpdateNeeded || addedNodes->find(iter->first) == addedNodes->end()) && + !uContains(localMaps_.localGrids(), iter->first))) { UDEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first);