From e5667b280a516e1b610438d7985db67678016b33 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 14:36:14 -0800 Subject: [PATCH 1/6] 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 2/6] 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 9a4c585d5ef406d222e727645095a764163d8ca1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 7 Dec 2024 17:26:46 -0800 Subject: [PATCH 3/6] 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 4/6] 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 5/6] 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 59390d8199520ba2571ea8cb3f045495f5b16d97 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 12 Feb 2025 19:25:32 -0800 Subject: [PATCH 6/6] 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