Fixed node ID=0 issues on ros2 as msgs don't have seq.

introlab · Nov 20, 2023 · 097cab0 · 097cab0
1 parent a399ee9
commit 097cab0
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 5 deletions.
diff --git 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
@@ -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));