i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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/rviz/MapCloudDisplay.cpp
+6 -6
View File
@@ -299,9 +299,9 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
// Update graph
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.posesId.size() && i<map.poses.size(); ++i)
for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.posesId[i], rtabmap_ros::transformFromPoseMsg(map.poses[i])));
poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -482,12 +482,12 @@ void MapCloudDisplay::downloadMap()
else
{
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QApplication::processEvents();
this->reset();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
@@ -539,10 +539,10 @@ void MapCloudDisplay::downloadGraph()
}
else
{
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
QApplication::processEvents();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
src/rviz/MapGraphDisplay.cpp
+2 -2
View File
@@ -93,7 +93,7 @@ void MapGraphDisplay::destroyObjects()
manual_objects_.clear();
}
void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg )
{
if(!(msg->poses.size() == msg->posesId.size()))
{
@@ -105,7 +105,7 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapDataFromROS(*msg, poses, links, mapToOdom);
rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom);
destroyObjects();
src/rviz/MapGraphDisplay.h
+4 -4
View File
@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef MAP_GRAPH_DISPLAY_H
#define MAP_GRAPH_DISPLAY_H
#include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/MapGraph.h>
#include <rviz/message_filter_display.h>
@@ -51,9 +51,9 @@ namespace rtabmap_ros
/**
* \class MapGraphDisplay
* \brief Displays the graph in rtabmap::MapData message
* \brief Displays the graph of rtabmap::MapGraph message
*/
class MapGraphDisplay: public MessageFilterDisplay<rtabmap_ros::MapData>
class MapGraphDisplay: public MessageFilterDisplay<rtabmap_ros::MapGraph>
{
Q_OBJECT
public:
@@ -68,7 +68,7 @@ protected:
virtual void onInitialize();
/** @brief Overridden from MessageFilterDisplay. */
void processMessage( const rtabmap_ros::MapData::ConstPtr& msg );
void processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg );
private:
void destroyObjects();