i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-10 19:49:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
(cherry picked from commit 097cab0667)
This commit is contained in:
Borong Yuan
2024-09-01 21:38:25 -07:00
committed by GitHub
parent a97efff760
commit 3eb0b47a55
2 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/src/MsgConversion.cpp
+2
View File
@@ -1402,6 +1402,7 @@ rtabmap::Signature nodeFromROS(const rtabmap_msgs::Node & msg)
std::multimap<int, int> words; std::multimap<int, int> words;
std::vector<cv::KeyPoint> wordsKpts; std::vector<cv::KeyPoint> wordsKpts;
std::vector<cv::Point3f> words3D; std::vector<cv::Point3f> words3D;
cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors); cv::Mat wordsDescriptors = rtabmap::uncompressData(msg.word_descriptors);
if(msg.word_id_keys.size() != msg.word_id_values.size()) 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.setWords(words, wordsKpts, words3D, wordsDescriptors);
s.sensorData() = sensorDataFromROS(msg.data); s.sensorData() = sensorDataFromROS(msg.data);
s.sensorData().setId(msg.id);
return s; return s;
} }
void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg) void nodeToROS(const rtabmap::Signature & signature, rtabmap_msgs::Node & msg)
rtabmap_slam/src/CoreWrapper.cpp
+2 -5
View File
@@ -1778,10 +1778,7 @@ void CoreWrapper::commonSensorDataCallback(
} }
SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
if(lastPoseIntermediate_) data.setId(lastPoseIntermediate_?-1:0);
{
data.setId(-1);
}
OdometryInfo odomInfo; OdometryInfo odomInfo;
if(odomInfoMsg.get()) if(odomInfoMsg.get())
@@ -2282,7 +2279,7 @@ void CoreWrapper::process(
} }
// If not intermediate node // If not intermediate node
if(data.id() > 0) if(data.id() >= 0)
{ {
localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_); localizationDiagnostic_.updateStatus(rtabmap_.getStatistics().localizationCovariance(), twoDMapping_);
tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps)); tick(stamp, rate_>0?rate_:1000.0/(timeMsgConversion+timeRtabmap+timeUpdateMaps+timePublishMaps));