Allow to recreate deleted frames in case they are frameeditor frames

frame_editor/src/frame_editor/editor.py

@@ -110,6 +110,10 @@ def all_frame_ids(include_temp=True):
         return [f for f in FrameEditor.tf_dict() if
                 not FrameEditor.frame_is_temporary(f) or include_temp]
 
+    def all_editor_frame_ids(self, include_temp=True):
+        return [f for f in self.frames.keys() if
+                not FrameEditor.frame_is_temporary(f) or include_temp]
+
     def iter_frames(self, include_temp=True):
         for f in self.frames.values():
             if not self.frame_is_temporary(f.name) or include_temp:

frame_editor/src/frame_editor/objects.py

@@ -8,6 +8,8 @@
 import tf.transformations as tft
 import tf2_ros
 
+import yaml
+
 from frame_editor.constructors_geometry import *
 from frame_editor.constructors_std import *
 from frame_editor.srv import *
@@ -45,6 +47,16 @@ def init_tf():
             Frame.tf_buffer = tf2_ros.Buffer()
             Frame.tf_listener = tf2_ros.TransformListener(Frame.tf_buffer)
 
+    @staticmethod
+    def was_published_by_frameeditor(name):
+        tf2_structure_in_yaml = Frame.tf_buffer.all_frames_as_yaml()
+        tf2_structure = yaml.load(tf2_structure_in_yaml, Loader=yaml.Loader)
+        try:
+            bc = tf2_structure[name]["broadcaster"]
+            return bc == rospy.get_name()
+        except KeyError:
+            return False
+
     @classmethod
     def create_new_id(cls):
         cls.__id_counter = cls.__id_counter + 1

frame_editor/src/frame_editor/rqt_editor.py

@@ -257,24 +257,33 @@ def write_file(self, file_name):
     def clear_all(self):
         self.editor.command(Command_ClearAll(self.editor))
 
-    @Slot(bool)
-    def btn_add_clicked(self, checked):
-        # Get a unique frame name
-        existing_frames = set(self.editor.all_frame_ids())
+    def get_valid_frame_name(self, window_title, default_name="my_frame"):
+
+        existing_tf_frames = set(self.editor.all_frame_ids())
+        existing_editor_frames = set(self.editor.all_editor_frame_ids())
 
-        name, ok = QtWidgets.QInputDialog.getText(self.widget, "Add New Frame", "Name:", QtWidgets.QLineEdit.Normal, "my_frame");
+        name, ok = QtWidgets.QInputDialog.getText(self.widget, window_title, "Name:", QtWidgets.QLineEdit.Normal, default_name);
 
-        while ok and name in existing_frames:
-            name, ok = QtWidgets.QInputDialog.getText(self.widget, "Add New Frame", "Name (must be unique):", QtWidgets.QLineEdit.Normal, "my_frame")
+        # allow recreating if frame was published by frameditor node originally
+        while ok and name in existing_editor_frames or (name in existing_tf_frames and not Frame.was_published_by_frameeditor(name)):
+            name, ok = QtWidgets.QInputDialog.getText(self.widget, window_title, "Name (must be unique):", QtWidgets.QLineEdit.Normal, default_name)
         if not ok:
+            return None
+        return name
+
+
+    @Slot(bool)
+    def btn_add_clicked(self, checked):
+
+        name = self.get_valid_frame_name("Add New Frame")
+        if not name:
             return
 
-        if not existing_frames:
+        available_parents = self.editor.all_frame_ids(include_temp=False)
+        if not available_parents:
             available_parents = ["world"]
-        else:
-            available_parents = self.editor.all_frame_ids(include_temp=False)
-        parent, ok = QtWidgets.QInputDialog.getItem(self.widget, "Add New Frame", "Parent Name:", sorted(available_parents))
 
+        parent, ok = QtWidgets.QInputDialog.getItem(self.widget, "Add New Frame", "Parent Name:", sorted(available_parents))
 
         if not ok or parent == "":
             return
@@ -291,14 +300,8 @@ def btn_duplicate_clicked(self, checked):
         source_name = item.text()
         parent_name = self.editor.frames[source_name].parent
 
-        # Get a unique frame name
-        existing_frames = set(self.editor.all_frame_ids())
-
-        name, ok = QtWidgets.QInputDialog.getText(self.widget, "Duplicate Frame", "Name:", QtWidgets.QLineEdit.Normal, source_name);
-
-        while ok and name in existing_frames:
-            name, ok = QtWidgets.QInputDialog.getText(self.widget, "Duplicate Frame", "Name (must be unique):", QtWidgets.QLineEdit.Normal, source_name)
-        if not ok:
+        name = self.get_valid_frame_name("Duplicate Frame", default_name=source_name)
+        if not name:
             return
 
         self.editor.command(Command_CopyElement(self.editor, name, source_name, parent_name))