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

merged master -> ros2

Browse Source
This commit is contained in:
matlabbe
2021-12-20 17:03:13 -05:00
parent 67de34b6a8
commit 07951eee08
9 changed files with 182 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+8
View File
@@ -513,6 +513,13 @@ void infoFromROS(const rtabmap_ros::msg::Info & info, rtabmap::Statistics & stat
stat.setLocalPath(info.local_path);
stat.setCurrentGoalId(info.current_goal_id);
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.stats_keys.size() && i<info.stats_values.size(); i++)
{
@@ -548,6 +555,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::msg::Info & info)
info.labels_values = uValues(stats.labels());
info.local_path = stats.localPath();
info.current_goal_id = stats.currentGoalId();
mapGraphToROS(stats.odomCachePoses(), stats.odomCacheConstraints(), stats.mapCorrection(), info.odom_cache);
// Statistics data
info.stats_keys = uKeys(stats.data());