diff --git a/frame_editor/src/frame_editor/editor.py b/frame_editor/src/frame_editor/editor.py index 6d8b648..a5d2a5c 100755 --- a/frame_editor/src/frame_editor/editor.py +++ b/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: diff --git a/frame_editor/src/frame_editor/objects.py b/frame_editor/src/frame_editor/objects.py index a2b81fb..6db152a 100644 --- a/frame_editor/src/frame_editor/objects.py +++ b/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 diff --git a/frame_editor/src/frame_editor/rqt_editor.py b/frame_editor/src/frame_editor/rqt_editor.py index e5b66db..b6f622b 100755 --- a/frame_editor/src/frame_editor/rqt_editor.py +++ b/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))