Feat static tree

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:
@@ -308,7 +312,7 @@ def parse_args(self, argv):
         parser.add_argument("-l", "--load", action="append",
                       dest="file",
                       help="Load a file at startup. [rospack filepath/file]")
-        parser.add_argument("-r", "--rate", type=int)
+        parser.add_argument("-r", "--rate", type=int, help="Rate for broadcasting. Does not involve tf frames.")
 
         args, unknowns = parser.parse_known_args(argv)
         print('arguments: {}'.format(args))

frame_editor/src/frame_editor/interface_tf.py

@@ -13,13 +13,21 @@ def __init__(self, frame_editor):
         self.editor = frame_editor
         self.editor.observers.append(self)
 
-    def broadcast(self, editor):
-        #print "> Broadcasting"
+    def update(self, editor, level, elements):
         now = rospy.Time.now()
-        transforms = [
-            ToTransformStamped(
-                f.position, f.orientation, now, f.name, f.parent)
-            for f in editor.frames.values()]
-        Frame.tf_broadcaster.sendTransform(transforms)
+
+        # change if there is a pose change
+        change = False
+        for element in elements:
+            if element is not None and (level & 1 == 1 or level & 4 == 4):
+                change = True
+
+        # publish all transforms if any changed (required for tf2 static)
+        if change:
+            transforms = [
+                ToTransformStamped(
+                    f.position, f.orientation, now, f.name, f.parent)
+                for f in editor.frames.values()]
+            Frame.tf_broadcaster.sendTransform(transforms)
 
 # eof

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 *
@@ -41,10 +43,20 @@ def __init__(self, name, position=(0,0,0), orientation=(0,0,0,1), parent="world"
     @staticmethod
     def init_tf():
         if Frame.tf_buffer is None:
-            Frame.tf_broadcaster = tf2_ros.TransformBroadcaster()
+            Frame.tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
             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))