From 740050f1be1cb0cf538cdbd4701ce8712f8f52f1 Mon Sep 17 00:00:00 2001
From: Roelof Oomen <rj.oomen@isohorti.com>
Date: Tue, 19 Nov 2024 15:42:01 +0100
Subject: [PATCH] Fix "Plot Scene Graph" function

---
 common/src/utils.cpp                          |  2 +-
 .../environment/widgets/environment_widget.h  |  3 ---
 .../src/widgets/environment_widget.cpp        | 12 ----------
 scene_graph/src/models/scene_graph_model.cpp  | 24 +++++++++++++++++++
 4 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/common/src/utils.cpp b/common/src/utils.cpp
index fe274009..ab4ad6d8 100644
--- a/common/src/utils.cpp
+++ b/common/src/utils.cpp
@@ -67,7 +67,7 @@ bool saveDotImage(const tesseract_common::fs::path& dot_path,
   gvc = gvContext();
   fp = fopen(dot_path.c_str(), "r");
   g = agread(fp, 0);
-  agsafeset(g, const_cast<char*>("dpi"), const_cast<char*>("300"), const_cast<char*>("300"));
+  agsafeset(g, const_cast<char*>("dpi"), const_cast<char*>("1000"), const_cast<char*>("1000"));
   gvLayout(gvc, g, "dot");
   gvRender(gvc, g, format.c_str(), fopen(save_path.c_str(), "w"));
   gvFreeLayout(gvc, g);
diff --git a/environment/include/tesseract_qt/environment/widgets/environment_widget.h b/environment/include/tesseract_qt/environment/widgets/environment_widget.h
index 2cf14631..fcd02fe6 100644
--- a/environment/include/tesseract_qt/environment/widgets/environment_widget.h
+++ b/environment/include/tesseract_qt/environment/widgets/environment_widget.h
@@ -52,9 +52,6 @@ class EnvironmentWidget : public QWidget
   void setComponentInfo(std::shared_ptr<const ComponentInfo> component_info);
   std::shared_ptr<const ComponentInfo> getComponentInfo() const;
 
-public Q_SLOTS:
-  virtual void onPlotSceneGraph();
-
 private:
   struct Implementation;
   std::unique_ptr<Ui::EnvironmentWidget> ui;
diff --git a/environment/src/widgets/environment_widget.cpp b/environment/src/widgets/environment_widget.cpp
index 17f23a8c..94918463 100644
--- a/environment/src/widgets/environment_widget.cpp
+++ b/environment/src/widgets/environment_widget.cpp
@@ -62,16 +62,4 @@ void EnvironmentWidget::setComponentInfo(std::shared_ptr<const ComponentInfo> co
 
 std::shared_ptr<const ComponentInfo> EnvironmentWidget::getComponentInfo() const { return data_->component_info; }
 
-void EnvironmentWidget::onPlotSceneGraph()
-{
-  //  tesseract_common::fs::path dot_path("/tmp/environment_widget_scene_graph.dot");
-  //  tesseract_common::fs::path image_path("/tmp/environment_widget_scene_graph.png");
-  //  data_->config->getEnvironment()->getSceneGraph()->saveDOT(dot_path.c_str());
-  //  saveDotImage(dot_path, image_path, "png");
-
-  //  auto* image_viewer = new ImageViewerWidget();
-  //  image_viewer->loadImage(image_path.c_str());
-  //  image_viewer->show();
-}
-
 }  // namespace tesseract_gui
diff --git a/scene_graph/src/models/scene_graph_model.cpp b/scene_graph/src/models/scene_graph_model.cpp
index f4bf4040..39dc563f 100644
--- a/scene_graph/src/models/scene_graph_model.cpp
+++ b/scene_graph/src/models/scene_graph_model.cpp
@@ -32,7 +32,10 @@
 #include <tesseract_qt/common/component_info.h>
 #include <tesseract_qt/common/environment_manager.h>
 #include <tesseract_qt/common/environment_wrapper.h>
+#include <tesseract_qt/common/utils.h>
+#include <tesseract_qt/common/widgets/image_viewer_widget.h>
 
+#include <tesseract_common/utils.h>
 #include <tesseract_scene_graph/graph.h>
 #include <tesseract_scene_graph/link.h>
 #include <tesseract_scene_graph/joint.h>
@@ -273,6 +276,27 @@ bool SceneGraphModel::eventFilter(QObject* obj, QEvent* event)
       }
     }
   }
+  else if (event->type() == events::SceneGraphPlot::kType)
+  {
+    assert(dynamic_cast<events::SceneGraphPlot*>(event) != nullptr);
+    auto* e = static_cast<events::SceneGraphPlot*>(event);
+    if (e->getComponentInfo() == data_->component_info)
+    {
+      auto env_wrapper = EnvironmentManager::get(data_->component_info);
+      if (env_wrapper != nullptr && env_wrapper->getEnvironment()->isInitialized())
+      {
+        tesseract_common::fs::path dot_path(tesseract_common::getTempPath() + "environment_widget_scene_graph.dot");
+        tesseract_common::fs::path image_path(tesseract_common::getTempPath() + "environment_widget_scene_graph.png");
+
+        env_wrapper->getEnvironment()->getSceneGraph()->saveDOT(dot_path.c_str());
+        saveDotImage(dot_path, image_path, "png");
+
+        auto* image_viewer = new ImageViewerWidget();
+        image_viewer->loadImage(image_path.c_str());
+        image_viewer->show();
+      }
+    }
+  }
 
   // Standard event processing
   return QObject::eventFilter(obj, event);