From e661abb884018b05a0951c3c2d20d8c53149fc20 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Mar 2025 14:15:09 -0700 Subject: [PATCH] GPS and global pose async topics buffered to get more accuratly the closest value to current update stamp in case there is significant delay. --- .../rtabmap_conversions/MsgConversion.h | 36 ++++++ rtabmap_conversions/src/MsgConversion.cpp | 2 +- .../include/rtabmap_slam/CoreWrapper.h | 9 +- rtabmap_slam/src/CoreWrapper.cpp | 107 +++++++++++++----- 4 files changed, 125 insertions(+), 29 deletions(-) diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 904731c2..3d9f5daa 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -323,6 +323,42 @@ inline int sizeOfPointField(int datatype) } return -1; } + +template +typename std::map::const_iterator getClosestIterator( + const std::map & buffer, + const K & key) +{ + UASSERT(!buffer.empty()); + typename std::map::const_iterator iterB = buffer.lower_bound(key); + typename std::map::const_iterator iterA = iterB; + if(iterA != buffer.begin()) + { + iterA = --iterA; + } + if(iterB == buffer.end()) + { + iterB = --iterB; + } + if(iterA == iterB) + { + return iterA; + } + if(iterA->first > key) + { + return iterA; + } + else if(iterB->first < key) + { + return iterB; + } + else if(key - iterA->first < iterB->first - key) + { + return iterA; + } + return iterB; +} + } #endif /* MSGCONVERSION_H_ */ diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index dc94f69d..be80ce78 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3386,7 +3386,7 @@ bool deskew_impl( } } } - UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed()); + UDEBUG("Lidar deskewing time=%fs (slerp=%s waitForTransform=%f)", processingTime.elapsed(), slerp?"true":"false", waitForTransform); return true; } diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index e0146aac..a0eb6e73 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -405,10 +405,15 @@ private: cv::Mat userData_; UMutex userDataMutex_; + rclcpp::CallbackGroup::SharedPtr globalPoseAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr globalPoseAsyncSub_; - geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; + std::map globalPoses_; + UMutex globalPoseMutex_; + + rclcpp::CallbackGroup::SharedPtr gpsAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; - rtabmap::GPS gps_; + std::map gps_; + UMutex gpsMutex_; rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 80a16b7c..95944bf9 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -145,7 +145,6 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : char * rosHomePath = getenv("ROS_HOME"); std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros"; databasePath_ = workingDir+"/"+rtabmap::Parameters::getDefaultDatabaseName(); - globalPose_.header.stamp = rclcpp::Time(0); mapsManager_.init(*this, this->get_name(), true); @@ -844,12 +843,18 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : // Setup callback groups for any subscriptions that should not be affected by main processing thread. userDataAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + globalPoseAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); rclcpp::SubscriptionOptions userDataAsyncSubOptions; + rclcpp::SubscriptionOptions globalPoseAsyncSubOptions; + rclcpp::SubscriptionOptions gpsAsyncSubOptions; rclcpp::SubscriptionOptions landmarkSubOptions; rclcpp::SubscriptionOptions imuSubOptions; userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_; + globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_; + gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_; landmarkSubOptions.callback_group = imuCallbackGroup_; imuSubOptions.callback_group = imuCallbackGroup_; @@ -858,8 +863,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions); - globalPoseAsyncSub_ = this->create_subscription("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions); - gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), subOptions); + globalPoseAsyncSub_ = this->create_subscription("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions); + gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions); landmarkDetectionSub_ = this->create_subscription("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS @@ -2059,18 +2064,40 @@ void CoreWrapper::process( data.setGroundTruth(groundTruthPose); //global pose - if(globalPose_.header.stamp.sec != 0 || globalPose_.header.stamp.nanosec != 0) + geometry_msgs::msg::PoseWithCovarianceStamped globalPoseMsg; + globalPoseMsg.header.stamp = rclcpp::Time(0); + { + UScopeMutex lock(globalPoseMutex_); + if(!globalPoses_.empty()) + { + auto iter = rtabmap_conversions::getClosestIterator(globalPoses_, data.stamp()); + // Check if it is not too old + if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_) + { + globalPoseMsg = iter->second; + } + else + { + RCLCPP_WARN(this->get_logger(), "Ignoring global pose with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).", + iter->first, + 1.0/rate_, + data.stamp()); + } + globalPoses_.clear(); + } + } + if(globalPoseMsg.header.stamp.sec != 0 || globalPoseMsg.header.stamp.nanosec != 0) { // assume sensor is fixed Transform sensorToBase = rtabmap_conversions::getTransform( - globalPose_.header.frame_id, + globalPoseMsg.header.frame_id, frameId_, stamp, *tfBuffer_, waitForTransform_); if(!sensorToBase.isNull()) { - Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPose_.pose.pose); + Transform globalPose = rtabmap_conversions::transformFromPoseMsg(globalPoseMsg.pose.pose); globalPose *= sensorToBase; // transform global pose from sensor frame to robot base frame // Correction of the global pose accounting the odometry movement since we received it @@ -2078,7 +2105,7 @@ void CoreWrapper::process( frameId_, odomFrameId, stamp, - rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec), + rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec), *tfBuffer_, waitForTransform_); if(!correction.isNull()) @@ -2091,17 +2118,32 @@ void CoreWrapper::process( "If odometry is small since it received the global pose and " "covariance is large, this should not be a problem."); } - cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPose_.pose.covariance.data()).clone(); + cv::Mat globalPoseCovariance = cv::Mat(6,6, CV_64FC1, (void*)globalPoseMsg.pose.covariance.data()).clone(); data.setGlobalPose(globalPose, globalPoseCovariance); } } - globalPose_.header.stamp = rclcpp::Time(0); - if(gps_.stamp() > 0.0) { - data.setGPS(gps_); + UScopeMutex lock(gpsMutex_); + if(!gps_.empty()) + { + std::map::const_iterator iter = rtabmap_conversions::getClosestIterator(gps_, data.stamp()); + // Check if it is not too old + if(rate_ == 0 || fabs(iter->first - data.stamp()) < 1.0/rate_) + { + data.setGPS(iter->second); + } + else + { + RCLCPP_WARN(this->get_logger(), "Ignoring GPS with stamp %f because it should be inside the update period (%f) of the current data stamp (%f).", + iter->first, + 1.0/rate_, + data.stamp()); + } + gps_.clear(); + } } - gps_ = rtabmap::GPS(); + //tag detections landmarksMutex_.lock(); @@ -2520,7 +2562,12 @@ void CoreWrapper::globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCova { if(!paused_) { - globalPose_ = *globalPoseMsg; + UScopeMutex lock(globalPoseMutex_); + globalPoses_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(globalPoseMsg->header.stamp), *globalPoseMsg)); + if(globalPoses_.size() > 1000) + { + globalPoses_.erase(globalPoses_.begin()); + } } } @@ -2537,13 +2584,21 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP error = sqrt(variance); } } - gps_ = rtabmap::GPS( - rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp), - gpsFixMsg->longitude, - gpsFixMsg->latitude, - gpsFixMsg->altitude, - error, - 0); + + rtabmap::GPS gps( + rtabmap_conversions::timestampFromROS(gpsFixMsg->header.stamp), + gpsFixMsg->longitude, + gpsFixMsg->latitude, + gpsFixMsg->altitude, + error, + 0); + + UScopeMutex lock(gpsMutex_); + gps_.insert(std::make_pair(gps.stamp(), gps)); + if(gps_.size() > 1000) + { + gps_.erase(gps_.begin()); + } } } @@ -3003,8 +3058,8 @@ void CoreWrapper::resetRtabmapCallback( graphLatched_ = false; mapsManager_.clear(); previousStamp_ = rclcpp::Time(0); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); landmarksMutex_.lock(); landmarks_.clear(); landmarksMutex_.unlock(); @@ -3105,8 +3160,8 @@ void CoreWrapper::loadDatabaseCallback( graphLatched_ = false; mapsManager_.clear(); previousStamp_ = rclcpp::Time(0); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); landmarksMutex_.lock(); landmarks_.clear(); landmarksMutex_.unlock(); @@ -3241,8 +3296,8 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); - globalPose_.header.stamp = rclcpp::Time(0); - gps_ = rtabmap::GPS(); + globalPoses_.clear(); + gps_.clear(); landmarksMutex_.lock(); landmarks_.clear(); landmarksMutex_.unlock();