From 3eb0b47a55bd56ea6282e5fff157880dcdca753a Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 2 Sep 2024 12:38:25 +0800 Subject: [PATCH 01/20] Fixed node ID=0 issues as msgs may not have seq. (#1202) (cherry picked from commit 097cab06677e4618ef0e4132cc544bc319d1d9a6) --- rtabmap_conversions/src/MsgConversion.cpp | 2 ++ rtabmap_slam/src/CoreWrapper.cpp | 7 ++----- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 3903fc9c..5f59758e 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1402,6 +1402,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg) std::multimap words; std::vector wordsKpts; std::vector words3D; + cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors); if(msg.word_id_keys.size() != msg.word_id_values.size()) @@ -1456,6 +1457,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg) } s.setWords(words, wordsKpts, words3D, wordsDescriptors); s.sensorData() = sensorDataFromROS(msg.data); + s.sensorData().setId(msg.id); return s; } void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 7d21e5f1..08b56b4b 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -1778,10 +1778,7 @@ void CoreWrapper::commonSensorDataCallback( } SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); - if(lastPoseIntermediate_) - { - data.setId(-1); - } + data.setId(lastPoseIntermediate_?-1:0); OdometryInfo odomInfo; if(odomInfoMsg.get()) @@ -2282,7 +2279,7 @@ void CoreWrapper::process( } // If not intermediate node - if(data.id() > 0) + if(data.id() >= 0) { localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_); tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)); From fc1387f7843aab6ee814bbea879a472d5f7c8e2c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 17:18:06 -0700 Subject: [PATCH 02/20] Odom: removed long processing from ros callbacks (decreasing delay, also making delay independent of the message filters topic_queue_size and sync_queue_size parameters) --- .../include/rtabmap_odom/OdometryROS.h | 14 ++- rtabmap_odom/src/OdometryROS.cpp | 110 ++++++++++++------ 2 files changed, 86 insertions(+), 38 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index d36a18d9..fe632fd4 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -55,7 +56,7 @@ class Odometry; namespace rtabmap_odom { -class OdometryROS : public nodelet::Nodelet +class OdometryROS : public nodelet::Nodelet, public UThread { public: @@ -94,6 +95,9 @@ private: virtual void onOdomInit() = 0; virtual void updateParameters(rtabmap::ParametersMap & parameters) {} + virtual void mainLoop(); + virtual void mainLoopKill(); + void callbackIMU(const sensor_msgs::ImuConstPtr& msg); void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity()); @@ -138,6 +142,13 @@ private: tf::TransformListener tfListener_; ros::Subscriber imuSub_; + // Safe-threading + UMutex imuMutex_; + UMutex dataMutex_; + USemaphore dataReady_; + rtabmap::SensorData dataToProcess_; + std_msgs::Header dataHeaderToProcess_; + bool paused_; int resetCountdown_; int resetCurrentCount_; @@ -156,7 +167,6 @@ private: bool waitIMUToinit_; bool imuProcessed_; std::map imus_; - std::pair bufferedData_; rtabmap_util::ULogToRosout ulogToRosout_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 1219a87a..d9150749 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -93,6 +93,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) : OdometryROS::~OdometryROS() { + this->join(true); delete odometry_; } @@ -379,6 +380,8 @@ void OdometryROS::onInit() NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str()); } + this->start(); + onOdomInit(); } @@ -433,17 +436,12 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); + UScopeMutex m(imuMutex_); + imus_.insert(std::make_pair(stamp, imu)); - - if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp()) - { - SensorData data = bufferedData_.first; - bufferedData_.first = SensorData(); - processData(data, bufferedData_.second); - } - if(imus_.size() > 1000) { + NODELET_WARN("Dropping imu data!"); imus_.erase(imus_.begin()); } } @@ -451,40 +449,75 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) void OdometryROS::processData(SensorData & data, const std_msgs::Header & header) { - if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) + //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); + if(dataMutex_.lockTry() == 0) { - NODELET_WARN("odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_.getTopic().c_str()); + dataToProcess_ = data; + dataHeaderToProcess_ = header; + dataReady_.release(); + dataMutex_.unlock(); + } + else + { + NODELET_INFO("Dropping image/scan data"); + } +} + +void OdometryROS::mainLoopKill() +{ + // in case we were waiting, unblock thread + dataReady_.release(); +} + +void OdometryROS::mainLoop() +{ + dataReady_.acquire(); + + if(!this->isRunning()) + { + // thread killed return; } - if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) - { - //NODELET_WARN("No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", stamp.toSec()); + UScopeMutex lock(dataMutex_); - // keep in cache to process later when we will receive imu msgs - if(bufferedData_.first.isValid()) + // aliases + SensorData & data = dataToProcess_; + std_msgs::Header & header = dataHeaderToProcess_; + + std::vector > imus; + { + UScopeMutex m(imuMutex_); + + if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty()) { - NODELET_ERROR("Overwriting previous data! Make sure IMU is " - "published faster than data rate. (last image stamp " - "buffered=%f and new one is %f, last imu stamp received=%f)", - bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + NODELET_WARN("odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_.getTopic().c_str()); + return; + } + + if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) + { + NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + return; + } + // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) + std::map::iterator iterEnd = imus_.lower_bound(header.stamp.toSec()); + if(iterEnd!= imus_.end()) + { + ++iterEnd; + } + for(std::map::iterator iter=imus_.begin(); iter!=iterEnd;) + { + imus.push_back(*iter); + imus_.erase(iter++); } - bufferedData_.first = data; - bufferedData_.second = header; - return; } - // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) - std::map::iterator iterEnd = imus_.lower_bound(header.stamp.toSec()); - if(iterEnd!= imus_.end()) + + for(size_t i=0; i::iterator iter=imus_.begin(); iter!=iterEnd;) - { - //NODELET_WARN("img callback: process imu %f", iter->first); - SensorData dataIMU(iter->second, 0, iter->first); + SensorData dataIMU(imus[i].second, 0, imus[i].first); odometry_->process(dataIMU); - imus_.erase(iter++); imuProcessed_ = true; } @@ -1020,20 +1053,21 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header odomSensorDataCompressedPub_.publish(msg); } + double delay = (ros::Time::now() - header.stamp).toSec(); if(visParams_) { if(icpParams_) { - NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, 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()); + NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, 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 { - NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, 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()); + NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.inliers, 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(icpParams_) { - NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, 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()); + NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs, delay=%fs", info.reg.icpInliersRatio, 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); } statusDiagnostic_.setStatus(pose.isNull()); @@ -1066,14 +1100,18 @@ bool OdometryROS::resetToPose(rtabmap_msgs::ResetPose::Request& req, rtabmap_msg void OdometryROS::reset(const Transform & pose) { + UScopeMutex lock(dataMutex_); odometry_->reset(pose); guess_.setNull(); guessPreviousPose_.setNull(); previousStamp_ = 0.0; resetCurrentCount_ = resetCountdown_; imuProcessed_ = false; - bufferedData_.first= SensorData(); + dataToProcess_ = SensorData(); + dataHeaderToProcess_ = std_msgs::Header(); + imuMutex_.lock(); imus_.clear(); + imuMutex_.unlock(); this->flushCallbacks(); } From 1e7df7e276975819934760ee5538c1f9e8224894 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 17:23:11 -0700 Subject: [PATCH 03/20] Changed a log from info->debug --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index d9150749..133de117 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -457,10 +457,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header dataReady_.release(); dataMutex_.unlock(); } - else - { - NODELET_INFO("Dropping image/scan data"); - } + //else + //{ + // NODELET_INFO("Dropping image/scan data"); + //} } void OdometryROS::mainLoopKill() From 76f1c50d19d7ba6ccb35abba402238a47a312a6a Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 3 Sep 2024 18:02:54 -0700 Subject: [PATCH 04/20] Changed a log from info->debug --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 133de117..a46d4fb2 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -457,10 +457,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header dataReady_.release(); dataMutex_.unlock(); } - //else - //{ - // NODELET_INFO("Dropping image/scan data"); - //} + else + { + NODELET_DEBUG("Dropping image/scan data"); + } } void OdometryROS::mainLoopKill() From e0542c7e4b315d3e309f95ea047ac4a2583efd31 Mon Sep 17 00:00:00 2001 From: ChristopherBilberg Date: Sun, 22 Sep 2024 00:51:13 +0200 Subject: [PATCH 05/20] Make sure to use syncQueueSize for sync policies in (#1209) PointCloudAggregator Queue size parameter was split into sync_queue_size and topic_queue_size parameters in ae44e1a2157b84e67ade530f2b8e713eac9650e8, however syncQueueSize was never used in PointCloudAggregator. Co-authored-by: Christopher Prinds Bilberg --- rtabmap_util/src/nodelets/point_cloud_aggregator.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp index 80493365..19ad1886 100644 --- a/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_aggregator.cpp @@ -135,14 +135,14 @@ private: cloudSub_4_.subscribe(nh, "cloud4", queueSize); if(approx) { - approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + approxSync4_ = new message_filters::Synchronizer(ApproxSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); if(approxSyncMaxInterval > 0.0) approxSync4_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } else { - exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); + exactSync4_ = new message_filters::Synchronizer(ExactSync4Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_, cloudSub_4_); exactSync4_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds4_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s,\n %s", @@ -159,14 +159,14 @@ private: cloudSub_3_.subscribe(nh, "cloud3", queueSize); if(approx) { - approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + approxSync3_ = new message_filters::Synchronizer(ApproxSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); if(approxSyncMaxInterval > 0.0) approxSync3_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } else { - exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(queueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); + exactSync3_ = new message_filters::Synchronizer(ExactSync3Policy(syncQueueSize), cloudSub_1_, cloudSub_2_, cloudSub_3_); exactSync3_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds3_callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s", @@ -181,14 +181,14 @@ private: { if(approx) { - approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + approxSync2_ = new message_filters::Synchronizer(ApproxSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); if(approxSyncMaxInterval > 0.0) approxSync2_->setMaxIntervalDuration(ros::Duration(approxSyncMaxInterval)); approxSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2)); } else { - exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(queueSize), cloudSub_1_, cloudSub_2_); + exactSync2_ = new message_filters::Synchronizer(ExactSync2Policy(syncQueueSize), cloudSub_1_, cloudSub_2_); exactSync2_->registerCallback(boost::bind(&rtabmap_util::PointCloudAggregator::clouds2_callback, this, boost::placeholders::_1, boost::placeholders::_2)); } subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s", From c3f7cfeaca8963c0dc11394e914c378839f583e4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 8 Oct 2024 20:18:42 -0700 Subject: [PATCH 06/20] fixed build with RTAB-Map 0.21.8 --- rtabmap_util/src/MapsManager.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 614b5f80..1ee902f2 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -1485,7 +1485,11 @@ void MapsManager::publishMaps( (elevationMapPub_.getNumSubscribers() && !latched_.at(&elevationMapPub_))) { grid_map_msgs::GridMap msg; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>21) || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR==21 && RTABMAP_VERSION_PATCH>=8) + grid_map::GridMapRosConverter::toMessage(*elevationMap_->gridMap(), msg); +#else grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap(), msg); +#endif msg.info.header.frame_id = mapFrameId; msg.info.header.stamp = stamp; elevationMapPub_.publish(msg); From 817417e11609a372be9ebafe6edc0e7cd6ed5723 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 19 Oct 2024 21:07:51 -0700 Subject: [PATCH 07/20] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ce68bdb0..48c48fd3 100644 --- a/README.md +++ b/README.md @@ -59,9 +59,9 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa # Installation ## ROS2 distribution -**Under construction**: see [ros2 branch](https://github.com/introlab/rtabmap_ros/tree/ros2#rtabmap_ros). +See [ros2 branch](https://github.com/introlab/rtabmap_ros/tree/ros2#rtabmap_ros). -## ROS distribution +## ROS1 distribution RTAB-Map is released as binaries in the ROS distribution. ```bash From ddd5e195cac0e1c597ed3f5c11c15289936ca4cc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 19 Oct 2024 21:10:08 -0700 Subject: [PATCH 08/20] Update README.md --- README.md | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 48c48fd3..bf16e219 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Build Status - ROS 2 + ROS 2 Humble Build Status @@ -42,6 +42,10 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Iron Build Status + + Jazzy + Build Status + Rolling Build Status From debfb0759ed97666012e8159a13b19e500ae8704 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 31 Oct 2024 20:37:38 -0700 Subject: [PATCH 09/20] Applying https://github.com/introlab/rtabmap_ros/pull/1233 to ros1 --- rtabmap_util/src/nodelets/point_cloud_assembler.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp index ead6cb0e..a60779a8 100644 --- a/rtabmap_util/src/nodelets/point_cloud_assembler.cpp +++ b/rtabmap_util/src/nodelets/point_cloud_assembler.cpp @@ -302,6 +302,7 @@ private: void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg) { + callbackCalled_ = true; if(cloudPub_.getNumSubscribers()) { UASSERT_MSG(cloudMsg->data.size() == cloudMsg->row_step*cloudMsg->height, From d12f73af6a119f6b760ec9ba880a65e4926de637 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:46:41 -0800 Subject: [PATCH 10/20] bump 0.21.9 --- rtabmap_conversions/package.xml | 2 +- rtabmap_costmap_plugins/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_legacy/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- 10 files changed, 10 insertions(+), 10 deletions(-) diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index ed4abf29..1c6718d1 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.5 + 0.21.9 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index 1b89fd21..e81526de 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.5 + 0.21.9 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index b2806f86..2ba3f4bb 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.5 + 0.21.9 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 47d37539..1e178c53 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.5 + 0.21.9 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 8ee6de39..3a0fce7f 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.5 + 0.21.9 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index fb6e6d06..7273799d 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.5 + 0.21.9 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index a4236524..61dbdcae 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.5 + 0.21.9 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 22d65080..837afdc9 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.5 + 0.21.9 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 782efd83..8a0d0b23 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.5 + 0.21.9 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index 85d968e2..caebced3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.5 + 0.21.9 RTAB-Map Stack From 7ca3a95899625b1fa48e5252be874a101e3eb4e0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:50:39 -0800 Subject: [PATCH 11/20] bump 0.21.9 remaining packages --- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index bf996032..a9030076 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.5 + 0.21.9 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 08e2fc06..2299ac09 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.5 + 0.21.9 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 83c43128..0f7c2625 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.5 + 0.21.9 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 13ef3227..33a58e81 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.5 + 0.21.9 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe From 6f81a8f70fac4eaa5e822edc32b82024ecf29189 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 10:55:09 -0800 Subject: [PATCH 12/20] missing 0.21.9 bump --- rtabmap_rviz_plugins/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index d604c7e9..48bab9cb 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.5 + 0.21.9 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe From e5667b280a516e1b610438d7985db67678016b33 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 14:36:14 -0800 Subject: [PATCH 13/20] Deskewing: add support for timestamp float64 field in nanoseconds (Livox) --- rtabmap_conversions/src/MsgConversion.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 5f59758e..9753bd88 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3058,6 +3058,25 @@ bool deskew_impl( } } + if(secFirst > 1.e18) + { + // convert nanoseconds to seconds + secFirst /= 1.e9; + secLast /= 1.e9; + } + else if(secFirst > 1.e15) + { + // convert microseconds to seconds + secFirst /= 1.e6; + secLast /= 1.e6; + } + else if(secFirst > 1.e12) + { + // convert milliseconds to seconds + secFirst /= 1.e3; + secLast /= 1.e3; + } + firstStamp = ros::Time(secFirst); lastStamp = ros::Time(secLast); } From 3bdbcdf3933d08abe258f12d7adf2b14ded1488c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 16:14:47 -0800 Subject: [PATCH 14/20] Follow-up of previous commit to convert all float64 timestamps correctly --- rtabmap_conversions/src/MsgConversion.cpp | 30 +++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 9753bd88..b41c7ae5 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3217,6 +3217,21 @@ bool deskew_impl( else if(timeDatatype == 8) //float64 { double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } @@ -3296,6 +3311,21 @@ bool deskew_impl( else if(timeDatatype == 8) { double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } From 42b0b49f5bcc182c6ccb604bdc4c076fb5bfaf16 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 17:45:11 -0800 Subject: [PATCH 15/20] Update README.md --- rtabmap_demos/README.md | 115 +++++++++++++++------------------------- 1 file changed, 43 insertions(+), 72 deletions(-) diff --git a/rtabmap_demos/README.md b/rtabmap_demos/README.md index 8ab7a988..5e0b350d 100644 --- a/rtabmap_demos/README.md +++ b/rtabmap_demos/README.md @@ -1,101 +1,72 @@ # rtabmap_demos - -- [rtabmap_demos](#rtabmap-demos) - + [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) - + [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) - + [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) - + [Find-Object with SLAM](#find-object-with-slam) - + [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2--2d-lidar-and-rgb-d-slam) - + [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) - + [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) - + [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2--2d-lidar-and-rgb-d-slam) - + [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2--elevation-map-and-vslam) - + [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--2d-lidar-and-rgb-d-slam) - + [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-and-rgb-d-slam) - + [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2--3d-lidar-assembling-and-rgb-d-slam) - + [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) - + [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) ++ [Outdoor Stereo VSLAM](#outdoor-stereo-vslam) ++ [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam) ++ [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam) ++ [Find-Object with SLAM](#find-object-with-slam) ++ [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2-2d-lidar-and-rgb-d-slam) ++ [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam) ++ [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam) ++ [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam) ++ [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam) ++ [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam) ++ [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-assembling-and-rgb-d-slam) ++ [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam) ++ [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam) ### Outdoor Stereo VSLAM -``` -ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py -``` +[stereo_outdoor_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/stereo_outdoor_demo.launch.py) + ![Peek 2024-11-29 10-52](https://github.com/user-attachments/assets/b6dd4a1c-5bd5-4cfa-936d-e8e707bbcb23) - ### Indoor 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true -``` +[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py) + ![Peek 2024-11-29 11-07](https://github.com/user-attachments/assets/b02beeea-28ed-4fde-932d-c89bef1a046d) - ### Multi-Session Indoor 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos multisession_mapping_demo.launch.py -``` +[multisession_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/multisession_mapping_demo.launch.py) + ![Peek 2024-11-29 11-48](https://github.com/user-attachments/assets/b130e5ab-618f-4c8b-840f-f926b65ab53b) - ### Find-Object with SLAM -``` -ros2 launch rtabmap_demos find_object_demo.launch.py -``` +[find_object_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/find_object_demo.launch.py) + ![Peek 2024-11-29 12-01](https://github.com/user-attachments/assets/b3cc0c67-517a-4f69-b4cc-35d288e96165) - ### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py -``` +[turtlebot4_sim_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py) + ![Peek 2024-11-29 12-19](https://github.com/user-attachments/assets/5914e34c-19f1-4b7c-b4df-2e7084946888) - ### Turtlebot3 Nav2 and 2D LiDAR SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py -``` +[turtlebot3_sim_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py) + ![Peek 2024-11-29 12-23](https://github.com/user-attachments/assets/e3c31c5a-5c46-4370-ad17-38c795db7917) - ### Turtlebot3 Nav2 and RGB-D SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py -``` +[turtlebot3_sim_rgbd_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py) + ![Peek 2024-11-29 14-22](https://github.com/user-attachments/assets/5088be17-0875-42cc-b863-d14468c67f26) - ### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py -``` +[turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py) + ![Peek 2024-11-29 13-41](https://github.com/user-attachments/assets/2e878158-b1b6-48a4-801c-72cdb41b4783) - ### Champ Quadruped Nav2, Elevation Map and VSLAM -``` -ros2 launch rtabmap_demos champ_sim_vslam.launch.py -``` +[champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py) + ![Peek 2024-11-29 15-00](https://github.com/user-attachments/assets/d1a27c78-27bc-4901-82a7-59b5d24e6454) - ### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py -``` +[husky_sim_scan2d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py) + ![Peek 2024-11-29 15-30](https://github.com/user-attachments/assets/c8f79b86-253e-4c8e-ac7a-c26584f43fa4) - ### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py -``` +[husky_sim_scan3d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py) + ![Peek 2024-11-29 15-36](https://github.com/user-attachments/assets/a4b6e6ae-38ed-44da-bbfb-d3c30a301f9c) - ### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM -``` -ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py -``` +[husky_sim_scan3d_assemble_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py) + ![Peek 2024-11-29 16-16](https://github.com/user-attachments/assets/b2235bd2-33d2-4c44-b6e9-9923a524632b) - ### Isaac Sim Nav2 and Stereo SLAM -``` -ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py -``` -![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) +![Peek 2024-11-29 17-49](https://github.com/user-attachments/assets/54cd0c82-aaed-47e5-911a-f286b6d2cc17) ### Isaac Sim Nav2 and RGB-D VSLAM -``` -ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py stereo:=false vo:=rtabmap -``` -![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) \ No newline at end of file +[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) stereo:=false vo:=rtabmap + +![Peek 2024-11-30 13-22](https://github.com/user-attachments/assets/240820c6-4dea-4cbf-9431-b4b3af695d51) From 9a4c585d5ef406d222e727645095a764163d8ca1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Dec 2024 17:26:46 -0800 Subject: [PATCH 16/20] Fixed https://github.com/introlab/rtabmap/issues/1399 --- .../include/rtabmap_odom/OdometryROS.h | 1 + rtabmap_odom/src/OdometryROS.cpp | 35 ++++++++++++++----- 2 files changed, 28 insertions(+), 8 deletions(-) diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index fe632fd4..c5672d36 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -148,6 +148,7 @@ private: USemaphore dataReady_; rtabmap::SensorData dataToProcess_; std_msgs::Header dataHeaderToProcess_; + bool bufferedDataToProcess_; bool paused_; int resetCountdown_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index a46d4fb2..a75ab336 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -436,13 +436,24 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(), localTransform); - UScopeMutex m(imuMutex_); - - imus_.insert(std::make_pair(stamp, imu)); - if(imus_.size() > 1000) { - NODELET_WARN("Dropping imu data!"); - imus_.erase(imus_.begin()); + UScopeMutex m(imuMutex_); + + imus_.insert(std::make_pair(stamp, imu)); + if(imus_.size() > 1000) + { + NODELET_WARN("Dropping imu data!"); + imus_.erase(imus_.begin()); + } + } + if(dataMutex_.lockTry() == 0) + { + if(bufferedDataToProcess_ && dataHeaderToProcess_.stamp.toSec() <= stamp) + { + bufferedDataToProcess_ = false; + dataReady_.release(); + } + dataMutex_.unlock(); } } } @@ -454,6 +465,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header { dataToProcess_ = data; dataHeaderToProcess_ = header; + bufferedDataToProcess_ = false; dataReady_.release(); dataMutex_.unlock(); } @@ -497,8 +509,15 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + if(bufferedDataToProcess_) { + NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + } + else { + NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; + } return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) From 01b8ab80ee122bca574269c97fc73994c8cdec59 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Dec 2024 17:47:36 -0800 Subject: [PATCH 17/20] refactored last commit --- rtabmap_odom/src/OdometryROS.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index a75ab336..0155b86e 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -463,6 +463,10 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); if(dataMutex_.lockTry() == 0) { + if(bufferedDataToProcess_) { + NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!", + dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec()); + } dataToProcess_ = data; dataHeaderToProcess_ = header; bufferedDataToProcess_ = false; @@ -509,15 +513,9 @@ void OdometryROS::mainLoop() if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec())) { - if(bufferedDataToProcess_) { - NODELET_ERROR("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Previous image is dropped, buffering the new image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); - } - else { - NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", - data.stamp(), imus_.empty()?0:imus_.rbegin()->first); - bufferedDataToProcess_ = true; - } + NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.", + data.stamp(), imus_.empty()?0:imus_.rbegin()->first); + bufferedDataToProcess_ = true; return; } // process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp) @@ -1128,6 +1126,7 @@ void OdometryROS::reset(const Transform & pose) imuProcessed_ = false; dataToProcess_ = SensorData(); dataHeaderToProcess_ = std_msgs::Header(); + bufferedDataToProcess_ = false; imuMutex_.lock(); imus_.clear(); imuMutex_.unlock(); From ef8ec4357d76fc83351acfaff0fc31c778214f46 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 13 Dec 2024 14:33:35 -0800 Subject: [PATCH 18/20] Fixed vo reset from guess (after being lost) not correctly updated if it is still lost after auto reset countdown --- rtabmap_odom/src/OdometryROS.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 0155b86e..f79fca44 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -905,10 +905,9 @@ void OdometryROS::mainLoop() "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()); } - else + else if(--resetCurrentCount_>0) { NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_); - --resetCurrentCount_; } if(resetCurrentCount_ == 0 || tooOldPreviousData) @@ -936,6 +935,11 @@ void OdometryROS::mainLoop() odometry_->reset(tfPose); } } + // Keep resetting if the odometry cannot initialize in next updates (e.g., lack of features). + // This will make sure we keep updating to latest guess pose. + if(resetCurrentCount_ == 0) { + ++resetCurrentCount_; + } } } From 856b2340b99809c19d86918e953c429253d9b201 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 3 Feb 2025 20:59:28 -0800 Subject: [PATCH 19/20] Added delete_db_on_start parameter to be usable with ComposableNode (#1058). --- rtabmap_slam/src/CoreWrapper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index 73424801..3816cc41 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -346,7 +346,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } // declare parameters - this->declare_parameter("is_rtabmap_paused", paused_); + paused_ = this->declare_parameter("is_rtabmap_paused", paused_); if(paused_) { RCLCPP_WARN(get_logger(), "Node paused... don't forget to call service \"resume\" to start rtabmap."); @@ -388,6 +388,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : char ** argv = new char*[argList.size()]; bool deleteDbOnStart = false; + deleteDbOnStart = this->declare_parameter("delete_db_on_start", deleteDbOnStart); for(unsigned int i=0; i Date: Wed, 12 Feb 2025 19:25:32 -0800 Subject: [PATCH 20/20] bump 0.21.10 --- rtabmap_conversions/package.xml | 2 +- rtabmap_costmap_plugins/package.xml | 2 +- rtabmap_demos/package.xml | 2 +- rtabmap_examples/package.xml | 2 +- rtabmap_launch/package.xml | 2 +- rtabmap_legacy/package.xml | 2 +- rtabmap_msgs/package.xml | 2 +- rtabmap_odom/package.xml | 2 +- rtabmap_python/package.xml | 2 +- rtabmap_ros/package.xml | 2 +- rtabmap_rviz_plugins/package.xml | 2 +- rtabmap_slam/package.xml | 2 +- rtabmap_sync/package.xml | 2 +- rtabmap_util/package.xml | 2 +- rtabmap_viz/package.xml | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/rtabmap_conversions/package.xml b/rtabmap_conversions/package.xml index 1c6718d1..e0454f93 100644 --- a/rtabmap_conversions/package.xml +++ b/rtabmap_conversions/package.xml @@ -1,7 +1,7 @@ rtabmap_conversions - 0.21.9 + 0.21.10 RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_costmap_plugins/package.xml b/rtabmap_costmap_plugins/package.xml index e81526de..70f746dd 100644 --- a/rtabmap_costmap_plugins/package.xml +++ b/rtabmap_costmap_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_costmap_plugins - 0.21.9 + 0.21.10 RTAB-Map's costmap_2d plugins Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 2ba3f4bb..560d8ad7 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -1,7 +1,7 @@ rtabmap_demos - 0.21.9 + 0.21.10 RTAB-Map's demo launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 1e178c53..ddb8cc10 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -1,7 +1,7 @@ rtabmap_examples - 0.21.9 + 0.21.10 RTAB-Map's example launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_launch/package.xml b/rtabmap_launch/package.xml index 3a0fce7f..4f9914a1 100644 --- a/rtabmap_launch/package.xml +++ b/rtabmap_launch/package.xml @@ -1,7 +1,7 @@ rtabmap_launch - 0.21.9 + 0.21.10 RTAB-Map's main launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 7273799d..883e4bb2 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -1,7 +1,7 @@ rtabmap_legacy - 0.21.9 + 0.21.10 RTAB-Map's legacy launch files. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_msgs/package.xml b/rtabmap_msgs/package.xml index 61dbdcae..3371b25a 100644 --- a/rtabmap_msgs/package.xml +++ b/rtabmap_msgs/package.xml @@ -1,7 +1,7 @@ rtabmap_msgs - 0.21.9 + 0.21.10 RTAB-Map's msgs package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_odom/package.xml b/rtabmap_odom/package.xml index 837afdc9..6b828b33 100644 --- a/rtabmap_odom/package.xml +++ b/rtabmap_odom/package.xml @@ -1,7 +1,7 @@ rtabmap_odom - 0.21.9 + 0.21.10 RTAB-Map's odometry package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_python/package.xml b/rtabmap_python/package.xml index 8a0d0b23..07cad0da 100644 --- a/rtabmap_python/package.xml +++ b/rtabmap_python/package.xml @@ -1,7 +1,7 @@ rtabmap_python - 0.21.9 + 0.21.10 RTAB-Map's python package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_ros/package.xml b/rtabmap_ros/package.xml index caebced3..0d0a21d3 100644 --- a/rtabmap_ros/package.xml +++ b/rtabmap_ros/package.xml @@ -1,7 +1,7 @@ rtabmap_ros - 0.21.9 + 0.21.10 RTAB-Map Stack diff --git a/rtabmap_rviz_plugins/package.xml b/rtabmap_rviz_plugins/package.xml index 48bab9cb..7ff1e174 100644 --- a/rtabmap_rviz_plugins/package.xml +++ b/rtabmap_rviz_plugins/package.xml @@ -1,7 +1,7 @@ rtabmap_rviz_plugins - 0.21.9 + 0.21.10 RTAB-Map's rviz plugins. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_slam/package.xml b/rtabmap_slam/package.xml index a9030076..4427bd1c 100644 --- a/rtabmap_slam/package.xml +++ b/rtabmap_slam/package.xml @@ -1,7 +1,7 @@ rtabmap_slam - 0.21.9 + 0.21.10 RTAB-Map's SLAM package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_sync/package.xml b/rtabmap_sync/package.xml index 2299ac09..6bdc9e28 100644 --- a/rtabmap_sync/package.xml +++ b/rtabmap_sync/package.xml @@ -1,7 +1,7 @@ rtabmap_sync - 0.21.9 + 0.21.10 RTAB-Map's synchronization package. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 0f7c2625..2286a923 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -1,7 +1,7 @@ rtabmap_util - 0.21.9 + 0.21.10 RTAB-Map's various useful nodes and nodelets. Mathieu Labbe Mathieu Labbe diff --git a/rtabmap_viz/package.xml b/rtabmap_viz/package.xml index 33a58e81..4bcba5fd 100644 --- a/rtabmap_viz/package.xml +++ b/rtabmap_viz/package.xml @@ -1,7 +1,7 @@ rtabmap_viz - 0.21.9 + 0.21.10 RTAB-Map's visualization package. Mathieu Labbe Mathieu Labbe