From 9e86faa4addd72a311fd0c894b303bc034f6ad15 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 7 Sep 2023 09:10:04 -0700
Subject: [PATCH] GraphView: added right-click option to export 2d grid map
 directly from the view

---
 guilib/src/GraphViewer.cpp | 44 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)

diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp
index 0953e93d34..4e80da439e 100644
--- a/guilib/src/GraphViewer.cpp
+++ b/guilib/src/GraphViewer.cpp
@@ -1798,6 +1798,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QMenu menu;
 	QAction * aScreenShot = menu.addAction(tr("Take a screenshot..."));
+	QAction * aExportGridMap = menu.addAction(tr("Export grid map..."));
+	aExportGridMap->setEnabled(!_gridMap->pixmap().isNull());
 	menu.addSeparator();
 
 	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
@@ -2095,6 +2097,48 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 		}
 		return; // without emitting configChanged
 	}
+	else if(r == aExportGridMap)
+	{
+		float xMin, yMin, cellSize;
+
+		cellSize = _gridCellSize;
+		xMin = -_gridMap->y()/100.0f;
+		yMin = -_gridMap->x()/100.0f;
+
+		QString path = QFileDialog::getSaveFileName(
+				this,
+				tr("Save File"),
+				"map.pgm",
+				tr("Map (*.pgm)"));
+
+		if(!path.isEmpty())
+		{
+			if(QFileInfo(path).suffix() == "")
+			{
+				path += ".pgm";
+			}
+			_gridMap->pixmap().save(path);
+
+			QFileInfo info(path);
+			QString yaml = info.absolutePath() + "/" +  info.baseName() + ".yaml";
+
+			float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+
+			std::ofstream file;
+			file.open (yaml.toStdString());
+			file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl;
+			file << "resolution: " << cellSize << std::endl;
+			file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl;
+			file << "negate: 0" << std::endl;
+			file << "occupied_thresh: " << occupancyThr << std::endl;
+			file << "free_thresh: 0.196" << std::endl;
+			file << std::endl;
+			file.close();
+
+
+			QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml));
+		}
+	}
 	else if(r == aSetIntraInterSessionColors)
 	{
 		setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked());