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

Added mapOdomCache topic. Updated rtabmap_ros/Info msg.

Browse Source
This commit is contained in:
matlabbe
2021-12-18 18:55:46 -05:00
parent 0ad3d7bd7f
commit 8ea0782e14
4 changed files with 48 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+8
View File
@@ -507,6 +507,13 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
stat.setLocalPath(info.localPath);
stat.setCurrentGoalId(info.currentGoalId);
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> constraints;
rtabmap::Transform t;
mapGraphFromROS(info.odom_cache, poses, constraints, t);
stat.setOdomCachePoses(poses);
stat.setOdomCacheConstraints(constraints);
// Statistics data
for(unsigned int i=0; i<info.statsKeys.size() && i<info.statsValues.size(); i++)
{
@@ -542,6 +549,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
info.labelsValues = uValues(stats.labels());
info.localPath = stats.localPath();
info.currentGoalId = stats.currentGoalId();
mapGraphToROS(stats.odomCachePoses(), stats.odomCacheConstraints(), stats.mapCorrection(), info.odom_cache);
// Statistics data
info.statsKeys = uKeys(stats.data());