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

Added MapGraph message to avoid MapGraphDisplay to subscribe to MapData (which can contain node data not used by MapGraphDisplay, that also limit the bandwidth used when MapCloudDisplay and MapGraphDisplay are added to RVIZ)

Browse Source
This commit is contained in:
matlabbe
2015-09-15 16:40:07 -04:00
parent 10a508c12c
commit 5edae82d98
17 changed files with 138 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+26 -26
View File
@@ -298,7 +298,7 @@ void mapDataFromROS(
rtabmap::Transform & mapToOdom)
{
//optimized graph
mapDataFromROS(msg, poses, links, mapToOdom);
mapGraphFromROS(msg.graph, poses, links, mapToOdom);
//Data
for(unsigned int i=0; i<msg.nodes.size(); ++i)
@@ -306,8 +306,29 @@ void mapDataFromROS(
signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
}
}
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, rtabmap::Link> & links,
const std::map<int, rtabmap::Signature> & signatures,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::MapData & msg)
{
//Optimized graph
mapGraphToROS(poses, links, mapToOdom, msg.graph);
//Data
msg.nodes.resize(signatures.size());
int index=0;
for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
iter!=signatures.end();
++iter)
{
nodeDataToROS(iter->second, msg.nodes[index++]);
}
}
void mapGraphFromROS(
const rtabmap_ros::MapGraph & msg,
std::map<int, rtabmap::Transform> & poses,
std::multimap<int, rtabmap::Link> & links,
rtabmap::Transform & mapToOdom)
@@ -325,32 +346,11 @@ void mapDataFromROS(
}
mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
}
void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, rtabmap::Link> & links,
const std::map<int, rtabmap::Signature> & signatures,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::MapData & msg)
{
//Optimized graph
mapDataToROS(poses, links, mapToOdom, msg);
//Data
msg.nodes.resize(signatures.size());
int index=0;
for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
iter!=signatures.end();
++iter)
{
nodeDataToROS(iter->second, msg.nodes[index++]);
}
}
void mapDataToROS(
void mapGraphToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, rtabmap::Link> & links,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::MapData & msg)
rtabmap_ros::MapGraph & msg)
{
//Optimized graph
msg.posesId.resize(poses.size());