From 3eb0b47a55bd56ea6282e5fff157880dcdca753a Mon Sep 17 00:00:00 2001 From: Borong Yuan Date: Mon, 2 Sep 2024 12:38:25 +0800 Subject: [PATCH 01/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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/13] 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); }