Fixed node ID=0 issues as msgs may not have seq. (#1202)

(cherry picked from commit 097cab0)
introlab · Sep 2, 2024 · 3eb0b47 · 3eb0b47
1 parent a97efff
commit 3eb0b47
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
@@ -1402,6 +1402,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::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())
@@ -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
@@ -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));