diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index f21262b2..82063cb4 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -114,6 +114,8 @@ private: double guessMinTranslation_; double guessMinRotation_; double guessMinTime_; + double guessLinearVariance_; + double guessAngularVariance_; bool publishTf_; bool waitForTransform_; double waitForTransformDuration_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 40b2a5e9..9843afeb 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -67,6 +67,8 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : guessMinTranslation_(0.0), guessMinRotation_(0.0), guessMinTime_(0.0), + guessLinearVariance_(0.001), + guessAngularVariance_(0.001), publishTf_(true), waitForTransform_(true), waitForTransformDuration_(0.1), // 100 ms @@ -147,6 +149,8 @@ void OdometryROS::onInit() pnh.param("guess_min_translation", guessMinTranslation_, guessMinTranslation_); pnh.param("guess_min_rotation", guessMinRotation_, guessMinRotation_); pnh.param("guess_min_time", guessMinTime_, guessMinTime_); + pnh.param("guess_linear_variance", guessLinearVariance_, guessLinearVariance_); + pnh.param("guess_angular_variance", guessAngularVariance_, guessAngularVariance_); pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_); @@ -186,6 +190,8 @@ void OdometryROS::onInit() NODELET_INFO("Odometry: guess_min_translation = %f", guessMinTranslation_); NODELET_INFO("Odometry: guess_min_rotation = %f", guessMinRotation_); NODELET_INFO("Odometry: guess_min_time = %f", guessMinTime_); + NODELET_INFO("Odometry: guess_linear_variance = %f", guessLinearVariance_); + NODELET_INFO("Odometry: guess_angular_variance = %f", guessAngularVariance_); NODELET_INFO("Odometry: expected_update_rate = %f Hz", expectedUpdateRate_); NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_); NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_); @@ -672,6 +678,44 @@ void OdometryROS::processData() } } + bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; + if(tooOldPreviousData) + { + 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 - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); + + if(!guess_.isNull()) + { + 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(); + 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, this->tfListener(), this->waitForTransformDuration()); + 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(tfPose); + } + } + } + + bool skipOdometryUpdate = false; + + rtabmap::Transform pose; + rtabmap::OdometryInfo info; + rtabmap::Transform guessVelocity; Transform guessCurrentPose; if(!guessFrameId_.empty()) @@ -708,27 +752,22 @@ void OdometryROS::processData() (guessMinTime_ <= 0.0 || (previousStamp_.toSec()>0.0 && (header.stamp-previousStamp_).toSec() < guessMinTime_))) { // Ignore odometry update, we didn't move enough - if(publishTf_) - { - geometry_msgs::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); - 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; + 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 + + //set velocity + double dt = (header.stamp-previousStamp_).toSec(); + 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; @@ -740,23 +779,21 @@ void OdometryROS::processData() } } - bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_.toSec() > 0 && (header.stamp-previousStamp_).toSec() > 1.0/minUpdateRate_; - // process data ros::WallTime time = ros::WallTime::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_; //********************* @@ -829,11 +866,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; @@ -878,7 +920,7 @@ void OdometryROS::processData() odomLocalMap_.publish(cloudMsg); } - if(odomLastFrame_.getNumSubscribers()) + if(!skipOdometryUpdate && odomLastFrame_.getNumSubscribers()) { // check which type of Odometry is using if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry @@ -1008,20 +1050,14 @@ void OdometryROS::processData() } } - if(pose.isNull() && (resetCurrentCount_ > 0 || tooOldPreviousData)) + if(pose.isNull() && resetCurrentCount_ > 0) { - if(tooOldPreviousData) - { - 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 - previousStamp_).toSec(), 1.0/minUpdateRate_, minUpdateRate_, previousStamp_.toSec(), header.stamp.toSec()); - } - else if(--resetCurrentCount_>0) + if(--resetCurrentCount_>0) { NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); } - if(resetCurrentCount_ == 0 || tooOldPreviousData) + if(resetCurrentCount_ == 0) { if(!guess_.isNull()) { @@ -1184,9 +1220,11 @@ void OdometryROS::processData() msg.header.stamp = header.stamp; // use corresponding time stamp to image odomSensorDataCompressedPub_.publish(msg); } - double delay = (ros::Time::now() - header.stamp).toSec(); - if(visParams_) + if(skipOdometryUpdate) { + NODELET_INFO( "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)), (ros::WallTime::now()-time).toSec(), 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 37a8bfc1..2f5d7c72 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -278,6 +278,7 @@ private: double landmarkDefaultLinVariance_; bool waitForTransform_; double waitForTransformDuration_; + double stalenessFactor_; bool useActionForGoal_; bool useSavedMap_; bool genScan_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 6a3ab207..0a96897a 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -104,6 +104,7 @@ CoreWrapper::CoreWrapper() : landmarkDefaultLinVariance_(0.001), waitForTransform_(true), waitForTransformDuration_(0.2), // 200 ms + stalenessFactor_(0.0), useActionForGoal_(false), useSavedMap_(true), genScan_(false), @@ -197,6 +198,7 @@ void CoreWrapper::onInit() pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); + pnh.param("staleness_factor", stalenessFactor_, stalenessFactor_); pnh.param("initial_pose", initialPoseStr, initialPoseStr); pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_); pnh.param("use_saved_map", useSavedMap_, useSavedMap_); @@ -248,6 +250,14 @@ void CoreWrapper::onInit() NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); + NODELET_INFO("rtabmap: wait_for_transform = %s", waitForTransform_?"true":"false"); + NODELET_INFO("rtabmap: wait_for_transform_duration = %f", waitForTransformDuration_); + if(stalenessFactor_!=0.0 && stalenessFactor_ < 1.0) { + NODELET_ERROR("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; + } + NODELET_INFO("rtabmap: staleness_factor = %f", stalenessFactor_); bool subscribeStereo = false; pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeStereo) @@ -1054,6 +1064,26 @@ bool CoreWrapper::odomUpdate(const nav_msgs::OdometryConstPtr & odomMsg, ros::Ti UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg->pose.covariance[0], odomMsg->twist.covariance[0])); rtabmap_.triggerNewMap(); covariance_ = cv::Mat(); + } + else if(stalenessFactor_>0.0 && + previousStamp_.toSec() > 0.0 && + rate_>0.0f && + stamp.toSec() - previousStamp_.toSec() > 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.toSec() - previousStamp_.toSec(), + stamp.toSec(), + previousStamp_.toSec(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + rtabmap_.triggerNewMap(); + covariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1147,6 +1177,26 @@ bool CoreWrapper::odomTFUpdate(const ros::Time & stamp) rtabmap_.triggerNewMap(); covariance_ = cv::Mat(); } + else if(stalenessFactor_>0.0 && + previousStamp_.toSec() > 0.0 && + rate_>0.0f && + stamp.toSec() - previousStamp_.toSec() > 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.toSec() - previousStamp_.toSec(), + stamp.toSec(), + previousStamp_.toSec(), + Parameters::kRtabmapDetectionRate().c_str(), + rate_, + 1.0f/rate_, + stalenessFactor_, + stalenessFactor_/rate_); + rtabmap_.triggerNewMap(); + covariance_ = cv::Mat(); + } lastPoseIntermediate_ = false; lastPose_ = odom;