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

Map Path: removed last id on localization mode

Browse Source
This commit is contained in:
matlabbe
2022-11-11 15:44:56 -08:00
parent a9497f3a39
commit 4294706189
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+3 -2
View File
@@ -4117,7 +4117,8 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
nav_msgs::Path path;
if(pubPath)
{
path.poses.resize(stats.poses().size());
// Ignore pose of current location in Localization mode
path.poses.resize(stats.poses().size()-(rtabmap_.getMemory()->isIncremental()?0:1));
}
int oi = 0;
for(std::map<int, Transform>::const_iterator poseIter=stats.poses().begin();
@@ -4185,7 +4186,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
markers.markers.push_back(marker);
}
if(pubPath)
if(pubPath && (rtabmap_.getMemory()->isIncremental() || poseIter->first != stats.poses().rbegin()->first))
{
rtabmap_ros::transformToPoseMsg(poseIter->second, path.poses.at(oi).pose);
path.poses.at(oi).header.frame_id = mapFrameId_;