From 097cab06677e4618ef0e4132cc544bc319d1d9a6 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sun, 19 Nov 2023 18:57:01 -0800
Subject: [PATCH] Fixed node ID=0 issues on ros2 as msgs don't have seq.

---
 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 b4d55cf5..97fd6c13 100644
--- a/rtabmap_conversions/src/MsgConversion.cpp
+++ b/rtabmap_conversions/src/MsgConversion.cpp
@@ -1413,6 +1413,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::Node & msg)
 	std::multimap<int, int> words;
 	std::vector<cv::KeyPoint> wordsKpts;
 	std::vector<cv::Point3f> words3D;
+
 	cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors);
 
 	if(msg.word_id_keys.size() != msg.word_id_values.size())
@@ -1468,6 +1469,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::msg::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::msg::Node & msg)
diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp
index d99f4b82..93c8391d 100644
--- a/rtabmap_slam/src/CoreWrapper.cpp
+++ b/rtabmap_slam/src/CoreWrapper.cpp
@@ -1753,10 +1753,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())
@@ -2256,7 +2253,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));