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

Increased required rtabmap version to 0.20.14. Added detect_more_loop_closures, global_bundle_adjustment and cleanup_local_grids services. rviz MapCloud: added "Download namespace" parameter.

Browse Source
This commit is contained in:
matlabbe
2021-09-11 11:39:11 -04:00
parent b2ac4b6ea7
commit a324562ee0
8 changed files with 359 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+62 -78
View File
@@ -186,6 +186,8 @@ MapCloudDisplay::MapCloudDisplay()
node_filtering_angle_->setMin( 0.0f );
node_filtering_angle_->setMax( 359.0f );
download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this);
download_map_ = new rviz::BoolProperty( "Download map", false,
"Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
this, SLOT( downloadMap() ), this );
@@ -527,50 +529,65 @@ void MapCloudDisplay::updateCloudParameters()
// do nothing... only take effect on next generated clouds
}
void MapCloudDisplay::downloadMap(bool graphOnly)
{
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = false;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = graphOnly;
std::string rtabmapNs = download_namespace->getStdString();
ros::NodeHandle nh;
std::string srvName = nh.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
QMessageBox * messageBox = new QMessageBox(
QMessageBox::NoIcon,
tr("Calling \"%1\" service...").arg(srvName.c_str()),
tr("Downloading the map... please wait (rviz could become gray!)"),
QMessageBox::NoButton);
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
messageBox->show();
QApplication::processEvents();
uSleep(100); // hack make sure the text in the QMessageBox is shown...
QApplication::processEvents();
if(!ros::service::call(srvName, getMapSrv))
{
ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
"Tip: if rtabmap node is not in \"%s\" namespace, you can "
"change the \"Download namespace\" option.",
srvName.c_str(),
rtabmapNs.c_str());
messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
"Tip: if rtabmap node is not in \"%2\" namespace, you can "
"change the \"Download namespace\" option.").
arg(srvName.c_str()).arg(rtabmapNs.c_str()));
}
else if(graphOnly)
{
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.graph.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
else
{
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.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.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
}
void MapCloudDisplay::downloadMap()
{
if(download_map_->getBool())
{
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = true;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = false;
ros::NodeHandle nh;
QMessageBox * messageBox = new QMessageBox(
QMessageBox::NoIcon,
tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
tr("Downloading the map... please wait (rviz could become gray!)"),
QMessageBox::NoButton);
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
messageBox->show();
QApplication::processEvents();
uSleep(100); // hack make sure the text in the QMessageBox is shown...
QApplication::processEvents();
if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
{
ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
"Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
"to \"get_map_data\" in the launch "
"file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
nh.resolveName("rtabmap/get_map_data").c_str());
messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
"Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
"to \"get_map_data\" in the launch "
"file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
arg(nh.resolveName("rtabmap/get_map_data").c_str()));
}
else
{
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.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.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
downloadMap(false);
download_map_->blockSignals(true);
download_map_->setBool(false);
download_map_->blockSignals(false);
@@ -589,43 +606,7 @@ void MapCloudDisplay::downloadGraph()
{
if(download_graph_->getBool())
{
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = true;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = true;
ros::NodeHandle nh;
QMessageBox * messageBox = new QMessageBox(
QMessageBox::NoIcon,
tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
tr("Downloading the graph... please wait (rviz could become gray!)"),
QMessageBox::NoButton);
messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
messageBox->show();
QApplication::processEvents();
uSleep(100); // hack make sure the text in the QMessageBox is shown...
QApplication::processEvents();
if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
{
ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
"Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
"to \"get_map_data\" in the launch "
"file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
nh.resolveName("rtabmap/get_map_data").c_str());
messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
"Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
"to \"get_map_data\" in the launch "
"file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
arg(nh.resolveName("rtabmap/get_map_data").c_str()));
}
else
{
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.graph.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
downloadMap(true);
download_graph_->blockSignals(true);
download_graph_->setBool(false);
download_graph_->blockSignals(false);
@@ -759,7 +740,10 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
}
else
{
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
it->first,
cloudInfoIt->second->message_->header.frame_id.c_str(),
cloudInfoIt->second->message_->header.frame_id.c_str());
}
}
src/rviz/MapCloudDisplay.h
+2
View File
@@ -119,6 +119,7 @@ public:
rviz::FloatProperty* cloud_filter_ceiling_height_;
rviz::FloatProperty* node_filtering_radius_;
rviz::FloatProperty* node_filtering_angle_;
rviz::StringProperty * download_namespace;
rviz::BoolProperty* download_map_;
rviz::BoolProperty* download_graph_;
@@ -145,6 +146,7 @@ protected:
virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );
private:
void downloadMap(bool graphOnly);
void processMapData(const rtabmap_ros::MapData& map);
/**