Rviz config saving and loading fixed

ctu-mrs · Nov 16, 2023 · 95a4e6b · 95a4e6b
1 parent cd2cb8d
commit 95a4e6b
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 3 deletions.
diff --git a/include/control/control.h b/include/control/control.h
@@ -26,6 +26,8 @@
 #include "control/overlay_picker_tool.h"
 #include "control/im_server.h"
 
+#include <rviz/display_group.h>
+
 namespace mrs_rviz_plugins{
 
 class ControlTool : public rviz::SelectionTool{
@@ -39,6 +41,7 @@ class ControlTool : public rviz::SelectionTool{
   void deactivate() override;
   int  processMouseEvent(rviz::ViewportMouseEvent& event) override;
   int  processKeyEvent(QKeyEvent* event, rviz::RenderPanel* panel) override;
+  rviz::InteractiveMarkerDisplay* findDisplay(rviz::Property* property);
 
 protected:
   // Check if the mouse has moved from one object to another,

diff --git a/src/control/control.cpp b/src/control/control.cpp
@@ -32,13 +32,36 @@ void ControlTool::onInitialize(){
   rviz::SelectionTool::onInitialize();
   overlay_picker_tool->initialize(context_);
 
-  dis = new rviz::InteractiveMarkerDisplay();
-  dynamic_cast<rviz::VisualizationManager*>(context_)->addDisplay(dis, true);
+  dis = findDisplay(context_->getRootDisplayGroup());
+  if(dis == NULL){
+    dis = new rviz::InteractiveMarkerDisplay();
+    dynamic_cast<rviz::VisualizationManager*>(context_)->addDisplay(dis, true);
+    dis->setName(QString("Control Display"));
+    dis->setClassId("rviz/InteractiveMarkers");
+  }
 
-  dis->setName(QString("Control Display"));
   dis->setTopic(QString("control/update"), QString("visualization_msgs/InteractiveMarkerUpdate"));
 }
 
+rviz::InteractiveMarkerDisplay* ControlTool::findDisplay(rviz::Property* property){
+  if(overlay_picker_tool->isPropertyType<rviz::DisplayGroup>(property)){
+    rviz::DisplayGroup* group_property = overlay_picker_tool->isPropertyType<rviz::DisplayGroup>(property);
+    for (int i = 0; i < group_property->numChildren(); i++) {
+      rviz::InteractiveMarkerDisplay* res = findDisplay(group_property->childAt(i));
+      if (res) {
+        return res;
+      }
+    }
+  }
+  else{
+    rviz::InteractiveMarkerDisplay* im_display = overlay_picker_tool->isPropertyType<rviz::InteractiveMarkerDisplay>(property);
+    if(im_display && im_display->getName() == "Control Display"){
+      return im_display;
+    }
+  }
+  return NULL;
+}
+
 void ControlTool::activate(){
   rviz::SelectionTool::activate();
   setStatus(DEFAULT_MODE_MESSAGE);