task not working

franka_logistics/GUI.py

@@ -16,7 +16,7 @@ class GUINode(Node):
     def __init__(self):
         super().__init__('grasp_gui_node')
 
-        self.create_subscription(PoseStamped, 'franka_grasp_moveit/current_pose', self.pose_callback, 10)
+        self.create_subscription(PoseStamped, 'pose_recoder/current_pose', self.pose_callback, 10)
         self.moveit_action_client = ActionClient(self, GraspSequence, 'franka_grasp_moveit/grasp')
 
         self.current_pose = {
@@ -93,7 +93,7 @@ class MyGUI:
             'orientation_z': 0.0,
             'orientation_w': 0.0
         }
-        self.decimal_select = 1
+        self.decimal_select = 2
 
         # Grasp Submenu attributes
         self.grasp_name = ''
@@ -101,10 +101,6 @@ class MyGUI:
         self.hower = ''
         self.grasp = ''
         self.drop = ''
-        self.width_grasp_str = tk.StringVar(value='0.0')
-        self.width_grasp = 0.0
-        self.width_idle_str = tk.StringVar(value='0.1')
-        self.width_idle = 0.0
 
         # Database Submenu attributes
         self.pose_db = ''
@@ -117,8 +113,6 @@ class MyGUI:
             'hower_pose': 'Hower',
             'grasp_pose': 'Grasp',
             'drop_pose': 'Drop',
-            'grasp_width': 0.0,
-            'idle_width': 0.0,
         }
 
         # GUI Subcomponents
@@ -256,7 +250,7 @@ class MyGUI:
 
     def get_grasp_sequence_by_name(self, sequence_name):
         self.cursor.execute(
-            "SELECT pose_idle, pose_hower, pose_grasp, pose_drop, width_grasp, width_idle"
+            "SELECT pose_idle, pose_hower, pose_grasp, pose_drop"
             " FROM sequence_table WHERE sequence_name = ?",
             (sequence_name,))
         result = self.cursor.fetchone()
@@ -265,8 +259,6 @@ class MyGUI:
             self.grasp_entries['hower_pose'] = result[1]
             self.grasp_entries['grasp_pose'] = result[2]
             self.grasp_entries['drop_pose'] = result[3]
-            self.grasp_entries['width_grasp'] = result[4]
-            self.grasp_entries['width_idle'] = result[5]
         else:
             print("Sequence not found")
 
@@ -276,16 +268,23 @@ class MyGUI:
             "INSERT INTO pose_table (pose_name, position_x, position_y, position_z,"
             " orientation_x, orientation_y, orientation_z, orientation_w)"
             " VALUES (?, ?, ?, ?, ?, ?, ?, ?)",
-            (pose_name, position_x, position_y, position_z,
+            (pose_name,
-             orientation_x, orientation_y, orientation_z, orientation_w))
+             round(position_x, self.decimal_select),
+             round(position_y, self.decimal_select),
+             round(position_z, self.decimal_select),
+             round(orientation_x, self.decimal_select),
+             round(orientation_y, self.decimal_select),
+             round(orientation_z, self.decimal_select),
+             round(orientation_w, self.decimal_select)))
 
         self.conn.commit()
 
-    def add_new_grasp(self, sequence_name, idle, hower, grasp, drop, width_grasp, width_idle):
+    def add_new_grasp(self, sequence_name, idle, hower, grasp, drop):
+        print(sequence_name, idle, hower, grasp, drop)
         self.cursor.execute("INSERT INTO sequence_table"
-                            """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop, width_grasp, width_idle)
+                            """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop)
-                             VALUES (?, ?, ?, ?, ?, ?, ?)""",
+                             VALUES (?, ?, ?, ?, ?)""",
-                            (sequence_name, idle, hower, grasp, drop, width_grasp, width_idle))
+                            (sequence_name, idle, hower, grasp, drop))
 
         self.conn.commit()
 
@@ -307,10 +306,10 @@ class MyGUI:
             if key in self.pose_entries:
                 self.pose_entries[key].config(state='normal')  # Enable writing
                 self.pose_entries[key].delete(0, tk.END)
-                self.pose_entries[key].insert(0, f"{value}")
+                self.pose_entries[key].insert(0, f"{value:.5f}")
                 self.pose_entries[key].config(state='readonly')  # Set back to read-only
 
-    def update_grasp_selectors(self, db_poses):
+    def update_pose_selectors(self, db_poses):
         self.idle_selector['values'] = db_poses
         self.hower_selector['values'] = db_poses
         self.grasp_selector['values'] = db_poses
@@ -323,7 +322,7 @@ class MyGUI:
     def update_from_database(self):
         db_poses = self.get_pose_names()
         db_grasps = self.get_grasp_names()
-        self.update_grasp_selectors(db_grasps)
+        self.update_pose_selectors(db_poses)
         self.update_db_selectors(db_poses, db_grasps)
 
     # </editor-fold>
@@ -332,15 +331,21 @@ class MyGUI:
     # Pose Events
     def decimal_selector_event(self, event):
         # Get the selected value from the decimal selector
-        self.decimal_select = self.decimal_selector.get()
+        self.decimal_select = int(self.decimal_selector.get())
         print("Selected Decimal Value:", self.decimal_select)
 
     def pose_set_event(self):
         self.pose_name = self.pose_name_entry.get()
-        self.add_new_pose(self.pose_name, self.pose_entries['position_x'],
+        print(type(float(self.pose_entries['position_x'].get())))
-                          self.pose_entries['position_y'], self.pose_entries['position_z'],
+
-                          self.pose_entries['orientation_x'], self.pose_entries['orientation_y'],
+        self.add_new_pose(self.pose_name,
-                          self.pose_entries['orientation_z'], self.pose_entries['orientation_w'])
+                          float(self.pose_entries['position_x'].get()),
+                          float(self.pose_entries['position_y'].get()),
+                          float(self.pose_entries['position_z'].get()),
+                          float(self.pose_entries['orientation_x'].get()),
+                          float(self.pose_entries['orientation_y'].get()),
+                          float(self.pose_entries['orientation_z'].get()),
+                          float(self.pose_entries['orientation_w'].get()))
         print("Pose Button Clicked! Pose Name:", self.pose_name)
 
     # Grasp Events
@@ -358,10 +363,12 @@ class MyGUI:
 
     def grasp_set_event(self):
         self.grasp_name = self.grasp_name_entry.get()
-        self.width_idle = float(self.width_idle_str.get())
+        self.add_new_grasp(self.grasp_name,
-        self.width_grasp = float(self.width_grasp_str.get())
+                           self.grasp_entries['idle_pose'],
-        print("Grasp Button Clicked! Grasp Name:", self.grasp_name, 'Grasp Width:', self.width_grasp, 'Idle Width:',
+                           self.grasp_entries['hower_pose'],
-              self.width_idle, '')
+                           self.grasp_entries['grasp_pose'],
+                           self.grasp_entries['drop_pose'])
+        print("Grasp Button Clicked! Grasp Name:", self.grasp_name)
 
     # Database Events
     def pose_db_selector_event(self, event):
@@ -389,8 +396,7 @@ class MyGUI:
         grasp_pose = self.get_pose_by_name(self.grasp)
         drop_pose = self.get_pose_by_name(self.drop)
 
-        feedback = self.node.send_grasp_sequence_goal(idle_pose, hower_pose, grasp_pose,
+        feedback = self.node.send_grasp_sequence_goal(idle_pose, hower_pose, grasp_pose, drop_pose)
-                                                      drop_pose, self.width_grasp, self.width_idle)
 
         print("Grasp: ", self.grasp_run, ' is performed!')
 
@@ -439,7 +445,7 @@ class MyGUI:
                        'Orientation X', 'Orientation Y', 'Orientation Z', 'Orientation W']
         for i, label in enumerate(pose_labels, start=5):
             tk.Label(self.pose_frame, text=label).grid(row=i, column=0)
-            entry = tk.Entry(self.pose_frame, state='readonly', width=6)
+            entry = tk.Entry(self.pose_frame, state='readonly', width=10)
             entry.grid(row=i, column=1)
             self.pose_entries[label.replace(' ', '_').lower()] = entry
 
@@ -452,7 +458,7 @@ class MyGUI:
         # Decimal Selector
         decimal_label = tk.Label(self.pose_frame, text="Nr of Dec Pts", font="Arial")
         decimal_label.grid(row=13, column=0)
-        self.decimal_selector = ttk.Combobox(self.pose_frame, values=["1", "2", "3"], width=6)
+        self.decimal_selector = ttk.Combobox(self.pose_frame, values=["2", "3", "4"], width=6)
         self.decimal_selector.grid(row=13, column=1)
         self.decimal_selector.bind('<<ComboboxSelected>>', self.decimal_selector_event)
 
@@ -518,23 +524,6 @@ class MyGUI:
         self.drop_selector.grid(row=8, column=1)
         self.drop_selector.bind('<<ComboboxSelected>>', self.drop_selector_event)
 
-        # Separator
-        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
-        separator.grid(row=9, column=0, columnspan=2, sticky='ew')
-
-        # Width Input
-        float_validate = self.root.register(self.validate_float)
-
-        tk.Label(self.grasp_frame, text="Grasp Width").grid(row=10, column=0)
-        self.width_grasp_entry = tk.Entry(self.grasp_frame, textvariable=self.width_grasp_str, validate='key', width=8)
-        self.width_grasp_entry.config(validatecommand=(float_validate, '%P'))
-        self.width_grasp_entry.grid(row=10, column=1)
-
-        tk.Label(self.grasp_frame, text="Idle Width").grid(row=11, column=0)
-        self.width_idle_entry = tk.Entry(self.grasp_frame, textvariable=self.width_idle_str, validate='key', width=8)
-        self.width_idle_entry.config(validatecommand=(float_validate, '%P'))
-        self.width_idle_entry.grid(row=11, column=1)
-
         # Separator
         separator = ttk.Separator(self.grasp_frame, orient='horizontal')
         separator.grid(row=12, column=0, columnspan=2, sticky='ew')
@@ -602,8 +591,8 @@ class MyGUI:
         separator = ttk.Separator(self.run_frame, orient='horizontal')
         separator.grid(row=3, column=0, columnspan=2, sticky='ew')
 
-        grasp_labels = ['Idle Pose', 'Hower Pose', 'Grasp Pose',
+        grasp_labels = ['Idle Pose', 'Hower Pose', 'Grasp Pose', 'Drop Pose']
-                        'Drop Pose', 'Grasp Width', 'Idle Width']
+
         for i, label in enumerate(grasp_labels, start=4):
             tk.Label(self.run_frame, text=label).grid(row=i, column=0)
             entry = tk.Entry(self.run_frame, state='readonly', width=8)
@@ -637,7 +626,6 @@ def main(args=None):
         app.update_pose_values()
         root.update()
 
-
     gui_node.destroy_node()
     rclpy.shutdown()
     app.conn.close()

franka_logistics/Pose_Learn_GUI.py

@@ -0,0 +1,518 @@
+import threading
+import tkinter as tk
+from tkinter import ttk
+
+import sqlite3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+
+from geometry_msgs.msg import PoseStamped, Pose
+
+
+class GUINode(Node):
+    def __init__(self):
+        super().__init__('grasp_gui_node')
+
+        self.create_subscription(PoseStamped, 'pose_recoder/current_pose', self.pose_callback, 10)
+
+        self.current_pose = {
+            'position_x': 0.0,
+            'position_y': 0.0,
+            'position_z': 0.0,
+            'orientation_x': 0.0,
+            'orientation_y': 0.0,
+            'orientation_z': 0.0,
+            'orientation_w': 0.0
+        }
+
+    def pose_callback(self, msg):
+        self.current_pose['position_x'] = msg.pose.position.x
+        self.current_pose['position_y'] = msg.pose.position.y
+        self.current_pose['position_z'] = msg.pose.position.z
+        self.current_pose['orientation_x'] = msg.pose.orientation.x
+        self.current_pose['orientation_y'] = msg.pose.orientation.y
+        self.current_pose['orientation_z'] = msg.pose.orientation.z
+        self.current_pose['orientation_w'] = msg.pose.orientation.w
+
+
+class MyGUI:
+    def __init__(self, root, node):
+        self.root = root
+        self.root.title("Franka Grasp Interface")
+
+        # ROS2 Node
+        self.node = node
+
+        # Connect to your SQLite database
+        self.conn = sqlite3.connect('/home/niko/sqlite_dbs/logistics_db')
+        self.cursor = self.conn.cursor()
+
+        # Track the current mode
+        self.current_mode = 'Learn'  # Starts in Learn mode
+
+        # Initialize frames for Learn and Run modes
+        self.learn_frame = ttk.Frame(root)
+
+        # Pose Submenu attributes
+        self.pose_name = ''
+        self.pose_entries = {}
+        self.pose_entries_values = {
+            'position_x': 0.0,
+            'position_y': 0.0,
+            'position_z': 0.0,
+            'orientation_x': 0.0,
+            'orientation_y': 0.0,
+            'orientation_z': 0.0,
+            'orientation_w': 0.0
+        }
+        self.decimal_select = 2
+
+        # Grasp Submenu attributes
+        self.grasp_name = ''
+        self.idle = ''
+        self.hower = ''
+        self.grasp = ''
+        self.drop = ''
+
+        # Database Submenu attributes
+        self.pose_db = ''
+        self.grasp_db = ''
+
+        # Grasp Run Submenu attributes
+        self.grasp_run = ''
+        self.grasp_entries = {}
+
+        # GUI Subcomponents
+        self.pose_name_entry = None
+        self.decimal_selector = None
+
+        self.grasp_name_entry = None
+        self.idle_selector = None
+        self.hower_selector = None
+        self.grasp_selector = None
+        self.drop_selector = None
+        self.width_grasp_entry = None
+        self.width_idle_entry = None
+
+        self.pose_db_selector = None
+        self.grasp_db_selector = None
+
+        # Frames
+        self.pose_frame = None
+        self.grasp_frame = None
+        self.db_frame = None
+        self.separator_1 = None
+        self.separator_2 = None
+
+        # Initially set up Learn mode
+        self.setup_learn_mode()
+
+    # <editor-fold desc="Menu Methods">
+
+    def setup_learn_mode(self):
+        self.create_pose_column()
+        self.separator_1 = tk.Canvas(self.root, width=1, bg="grey")
+        self.separator_1.pack(side=tk.LEFT, fill=tk.Y)
+        self.create_grasp_column()
+        self.separator_2 = tk.Canvas(self.root, width=1, bg="grey")
+        self.separator_2.pack(side=tk.LEFT, fill=tk.Y)
+        self.create_db_edit_column()
+
+    # </editor-fold>
+
+    # <editor-fold desc="Initialize Methods">
+    def set_initial_pose_values(self):
+        initial_values = {
+            'position_x': 0.0,
+            'position_y': 0.0,
+            'position_z': 0.0,
+            'orientation_x': 0.0,
+            'orientation_y': 0.0,
+            'orientation_z': 0.0,
+            'orientation_w': 0.0
+        }
+        for key, value in initial_values.items():
+            if key in self.pose_entries:
+                self.pose_entries[key].config(state='normal')  # Enable writing
+                self.pose_entries[key].delete(0, tk.END)
+                self.pose_entries[key].insert(0, str(value))
+                self.pose_entries[key].config(state='readonly')  # Set back to read-only
+
+    # </editor-fold>
+
+    # <editor-fold desc="SQLite Methods">
+    def get_pose_names(self):
+        self.cursor.execute("SELECT pose_name FROM pose_table")
+        return [row[0] for row in self.cursor.fetchall()]
+
+    def get_grasp_names(self):
+        self.cursor.execute("SELECT sequence_name FROM sequence_table")
+        return [row[0] for row in self.cursor.fetchall()]
+
+    def get_pose_by_name(self, pose_name):
+        self.cursor.execute(
+            "SELECT position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w FROM pose_table WHERE pose_name = ?",
+            (pose_name,))
+        result = self.cursor.fetchone()
+        if result:
+            pose = Pose()
+            pose.position.x = result[0]
+            pose.position.y = result[1]
+            pose.position.z = result[2]
+            pose.orientation.x = result[3]
+            pose.orientation.y = result[4]
+            pose.orientation.z = result[5]
+            pose.orientation.w = result[6]
+            return pose
+        else:
+            print("Pose not found")
+            return None
+
+    def get_grasp_sequence_by_name(self, sequence_name):
+        self.cursor.execute(
+            "SELECT pose_idle, pose_hower, pose_grasp, pose_drop"
+            " FROM sequence_table WHERE sequence_name = ?",
+            (sequence_name,))
+        result = self.cursor.fetchone()
+        if result:
+            self.grasp_entries['idle_pose'] = result[0]
+            self.grasp_entries['hower_pose'] = result[1]
+            self.grasp_entries['grasp_pose'] = result[2]
+            self.grasp_entries['drop_pose'] = result[3]
+        else:
+            print("Sequence not found")
+
+    def add_new_pose(self, pose_name, position_x, position_y, position_z, orientation_x, orientation_y, orientation_z,
+                     orientation_w):
+        self.cursor.execute(
+            "INSERT INTO pose_table (pose_name, position_x, position_y, position_z,"
+            " orientation_x, orientation_y, orientation_z, orientation_w)"
+            " VALUES (?, ?, ?, ?, ?, ?, ?, ?)",
+            (pose_name,
+             round(position_x, self.decimal_select),
+             round(position_y, self.decimal_select),
+             round(position_z, self.decimal_select),
+             round(orientation_x, self.decimal_select),
+             round(orientation_y, self.decimal_select),
+             round(orientation_z, self.decimal_select),
+             round(orientation_w, self.decimal_select)))
+
+        self.conn.commit()
+
+    def add_new_grasp(self, sequence_name, idle, hower, grasp, drop):
+        print(sequence_name, idle, hower, grasp, drop)
+        self.cursor.execute("INSERT INTO sequence_table"
+                            """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop)
+                             VALUES (?, ?, ?, ?, ?)""",
+                            (sequence_name, idle, hower, grasp, drop))
+
+        self.conn.commit()
+
+    def delete_pose_by_name(self, pose_name):
+        self.cursor.execute("DELETE FROM pose_table WHERE pose_name = ?", (pose_name,))
+
+        self.conn.commit()
+
+    def delete_grasp_by_name(self, sequence_name):
+        self.cursor.execute("DELETE FROM sequence_table WHERE sequence_name = ?", (sequence_name,))
+
+        self.conn.commit()
+
+    # </editor-fold>
+
+    # <editor-fold desc="Update Methods">
+    def update_pose_values(self):
+        for key, value in self.node.current_pose.items():
+            if key in self.pose_entries:
+                self.pose_entries[key].config(state='normal')  # Enable writing
+                self.pose_entries[key].delete(0, tk.END)
+                self.pose_entries[key].insert(0, f"{value:.5f}")
+                self.pose_entries[key].config(state='readonly')  # Set back to read-only
+
+    def update_pose_selectors(self, db_poses):
+        self.idle_selector['values'] = db_poses
+        self.hower_selector['values'] = db_poses
+        self.grasp_selector['values'] = db_poses
+        self.drop_selector['values'] = db_poses
+
+    def update_db_selectors(self, db_poses, db_grasp):
+        self.pose_db_selector['values'] = db_poses
+        self.grasp_db_selector['values'] = db_grasp
+
+    def update_from_database(self):
+        db_poses = self.get_pose_names()
+        db_grasps = self.get_grasp_names()
+        self.update_pose_selectors(db_poses)
+        self.update_db_selectors(db_poses, db_grasps)
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Events">
+    # Pose Events
+    def decimal_selector_event(self, event):
+        # Get the selected value from the decimal selector
+        self.decimal_select = int(self.decimal_selector.get())
+        print("Selected Decimal Value:", self.decimal_select)
+
+    def pose_set_event(self):
+        self.pose_name = self.pose_name_entry.get()
+        print(type(float(self.pose_entries['position_x'].get())))
+
+        self.add_new_pose(self.pose_name,
+                          float(self.pose_entries['position_x'].get()),
+                          float(self.pose_entries['position_y'].get()),
+                          float(self.pose_entries['position_z'].get()),
+                          float(self.pose_entries['orientation_x'].get()),
+                          float(self.pose_entries['orientation_y'].get()),
+                          float(self.pose_entries['orientation_z'].get()),
+                          float(self.pose_entries['orientation_w'].get()))
+        print("Pose Button Clicked! Pose Name:", self.pose_name)
+
+    # Grasp Events
+    def idle_selector_event(self, event):
+        self.idle = self.idle_selector.get()
+
+    def hower_selector_event(self, event):
+        self.hower = self.hower_selector.get()
+
+    def grasp_selector_event(self, event):
+        self.grasp = self.grasp_selector.get()
+
+    def drop_selector_event(self, event):
+        self.drop = self.drop_selector.get()
+
+    def grasp_set_event(self):
+        self.grasp_name = self.grasp_name_entry.get()
+        self.add_new_grasp(self.grasp_name,
+                           self.idle,
+                           self.hower,
+                           self.grasp,
+                           self.drop)
+        print("Grasp Button Clicked! Grasp Name:", self.grasp_name)
+
+    # Database Events
+    def pose_db_selector_event(self, event):
+        self.pose_db = self.pose_db_selector.get()
+
+    def grasp_db_selector_event(self, event):
+        self.grasp_db = self.grasp_db_selector.get()
+
+    def pose_delete_event(self):
+        self.delete_pose_by_name(self.pose_db)
+        print("Pose: ", self.pose_db, ' was deleted!')
+
+    def grasp_delete_event(self):
+        self.delete_grasp_by_name(self.grasp_db)
+        print("Grasp: ", self.grasp_db, ' was deleted!')
+
+    # Run Events
+    def grasp_run_selector_event(self, event):
+        self.grasp_run = self.grasp_run_selector.get()
+        self.get_grasp_sequence_by_name(self.grasp_run)
+
+    def grasp_perform_event(self):
+        idle_pose = self.get_pose_by_name(self.idle)
+        hower_pose = self.get_pose_by_name(self.hower)
+        grasp_pose = self.get_pose_by_name(self.grasp)
+        drop_pose = self.get_pose_by_name(self.drop)
+
+        feedback = self.node.send_grasp_sequence_goal(idle_pose, hower_pose, grasp_pose, drop_pose)
+
+        print("Grasp: ", self.grasp_run, ' is performed!')
+
+    # </editor-fold>
+
+    # <editor-fold desc="Validate methods">
+    def validate_float(self, P):
+        try:
+            value = float(P)
+            if self.width_lower <= value <= self.width_upper:
+                return True
+            else:
+                return False
+        except ValueError:
+            return False
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Create Methods">
+    def create_pose_column(self):
+        self.pose_frame = tk.Frame(self.root)
+        self.pose_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.pose_frame, text="New Pose", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Name Entry
+        tk.Label(self.pose_frame, text="Pose Name").grid(row=1, column=0)
+        self.pose_name_entry = tk.Entry(self.pose_frame)
+        self.pose_name_entry.grid(row=1, column=1)
+
+        # Separator
+        separator = ttk.Separator(self.pose_frame, orient='horizontal')
+        separator.grid(row=2, column=0, columnspan=2, sticky='ew')
+
+        # Title for Current Pose
+        current_pose_label = tk.Label(self.pose_frame, text="Current Pose", font=("Arial", 10, "bold"))
+        current_pose_label.grid(row=3, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.pose_frame, orient='horizontal')
+        separator.grid(row=4, column=0, columnspan=2, sticky='ew')
+
+        # Pose Values Entries
+        pose_labels = ['Position X', 'Position Y', 'Position Z',
+                       'Orientation X', 'Orientation Y', 'Orientation Z', 'Orientation W']
+        for i, label in enumerate(pose_labels, start=5):
+            tk.Label(self.pose_frame, text=label).grid(row=i, column=0)
+            entry = tk.Entry(self.pose_frame, state='readonly', width=10)
+            entry.grid(row=i, column=1)
+            self.pose_entries[label.replace(' ', '_').lower()] = entry
+
+        self.set_initial_pose_values()
+
+        # Separator
+        separator = ttk.Separator(self.pose_frame, orient='horizontal')
+        separator.grid(row=12, column=0, columnspan=2, sticky='ew')
+
+        # Decimal Selector
+        decimal_label = tk.Label(self.pose_frame, text="Nr of Dec Pts", font="Arial")
+        decimal_label.grid(row=13, column=0)
+        self.decimal_selector = ttk.Combobox(self.pose_frame, values=["2", "3", "4"], width=6)
+        self.decimal_selector.grid(row=13, column=1)
+        self.decimal_selector.bind('<<ComboboxSelected>>', self.decimal_selector_event)
+
+        # Separator
+        separator = ttk.Separator(self.pose_frame, orient='horizontal')
+        separator.grid(row=14, column=0, columnspan=2, sticky='ew')
+
+        # Set Button
+        set_button = tk.Button(self.pose_frame, text="Set New Pose", command=self.pose_set_event)
+        set_button.grid(row=15, column=0, columnspan=2)
+
+        self.pose_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+    def create_grasp_column(self):
+        self.grasp_frame = tk.Frame(self.root)
+        self.grasp_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.grasp_frame, text="New Grasp", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Name Entry
+        tk.Label(self.grasp_frame, text="Grasp Name").grid(row=1, column=0)
+        self.grasp_name_entry = tk.Entry(self.grasp_frame)
+        self.grasp_name_entry.grid(row=1, column=1)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=2, column=0, columnspan=2, sticky='ew')
+
+        # Title for Current Pose
+        current_pose_label = tk.Label(self.grasp_frame, text="Grasp Poses", font=("Arial", 10, "bold"))
+        current_pose_label.grid(row=3, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=4, column=0, columnspan=2, sticky='ew')
+
+        # Idle Pose Selector
+        idle_label = tk.Label(self.grasp_frame, text="Idle Pose", font="Arial")
+        idle_label.grid(row=5, column=0)
+        self.idle_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.idle_selector.grid(row=5, column=1)
+        self.idle_selector.bind('<<ComboboxSelected>>', self.idle_selector_event)
+
+        # Hower Pose Selector
+        hower_label = tk.Label(self.grasp_frame, text="Hower Pose", font="Arial")
+        hower_label.grid(row=6, column=0)
+        self.hower_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.hower_selector.grid(row=6, column=1)
+        self.hower_selector.bind('<<ComboboxSelected>>', self.hower_selector_event)
+
+        # Grasp Pose Selector
+        grasp_label = tk.Label(self.grasp_frame, text="Grasp Pose", font="Arial")
+        grasp_label.grid(row=7, column=0)
+        self.grasp_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.grasp_selector.grid(row=7, column=1)
+        self.grasp_selector.bind('<<ComboboxSelected>>', self.grasp_selector_event)
+
+        # Drop Pose Selector
+        drop_label = tk.Label(self.grasp_frame, text="Drop Pose", font="Arial")
+        drop_label.grid(row=8, column=0)
+        self.drop_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.drop_selector.grid(row=8, column=1)
+        self.drop_selector.bind('<<ComboboxSelected>>', self.drop_selector_event)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=12, column=0, columnspan=2, sticky='ew')
+
+        # Set Button
+        set_button = tk.Button(self.grasp_frame, text="Set New Grasp", command=self.grasp_set_event)
+        set_button.grid(row=13, column=0, columnspan=2)
+
+    def create_db_edit_column(self):
+        self.db_frame = tk.Frame(self.root)
+        self.db_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.db_frame, text="Edit Database", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.db_frame, orient='horizontal')
+        separator.grid(row=1, column=0, columnspan=2, sticky='ew')
+
+        # Pose Database Selector
+        pose_db_label = tk.Label(self.db_frame, text="Poses", font="Arial")
+        pose_db_label.grid(row=2, column=0)
+        self.pose_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8)
+        self.pose_db_selector.grid(row=2, column=1)
+        self.pose_db_selector.bind('<<ComboboxSelected>>', self.pose_db_selector_event)
+
+        # Pose Delete Button
+        pose_delete_button = tk.Button(self.db_frame, text="Delete Pose", command=self.pose_delete_event)
+        pose_delete_button.grid(row=3, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.db_frame, orient='horizontal')
+        separator.grid(row=4, column=0, columnspan=2, sticky='ew')
+
+        # Grasp Database Selector
+        grasp_db_label = tk.Label(self.db_frame, text="Grasp", font="Arial")
+        grasp_db_label.grid(row=5, column=0)
+        self.grasp_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8)
+        self.grasp_db_selector.grid(row=5, column=1)
+        self.grasp_db_selector.bind('<<ComboboxSelected>>', self.grasp_db_selector_event)
+
+        # Grasp Delete Button
+        grasp_delete_button = tk.Button(self.db_frame, text="Delete Grasp", command=self.grasp_delete_event)
+        grasp_delete_button.grid(row=6, column=0, columnspan=2)
+    # </editor-fold>
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    gui_node = GUINode()
+
+    thread_spin = threading.Thread(target=rclpy.spin, args=(gui_node,))
+    thread_spin.start()
+
+    root = tk.Tk()
+
+    # app = MyGUI(root)
+    app = MyGUI(root, gui_node)
+    while True:
+        app.update_from_database()
+        app.update_pose_values()
+        root.update()
+
+    gui_node.destroy_node()
+    rclpy.shutdown()
+    app.conn.close()
+
+
+if __name__ == '__main__':
+    main()

franka_logistics/Run_GUI.py

@@ -0,0 +1,436 @@
+import threading
+import tkinter as tk
+from tkinter import ttk
+
+import sqlite3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+
+from geometry_msgs.msg import PoseStamped, Pose
+from franka_cps_msgs.action import GraspSequence
+
+
+class GUINode(Node):
+    def __init__(self):
+        super().__init__('grasp_gui_node')
+        self.moveit_action_client = ActionClient(self, GraspSequence, 'franka_grasp_moveit/grasp')
+
+
+    def send_grasp_sequence_goal(self, idle, hower, grasp, drop, width_grasp, width_idle):
+        goal_msg = GraspSequence.Goal()
+        goal_msg.idle = idle
+        goal_msg.hower = hower
+        goal_msg.grasp = grasp
+        goal_msg.drop = drop
+        goal_msg.width_grasp = width_grasp
+        goal_msg.width_idle = width_idle
+
+        self.moveit_action_client.wait_for_server()
+
+        return self.moveit_action_client.send_goal(goal_msg)
+
+
+class MyGUI:
+    def __init__(self, root, node):
+        self.root = root
+        self.root.title("Franka Grasp Interface")
+
+        # ROS2 Node
+        self.node = node
+
+        # Connect to your SQLite database
+        self.conn = sqlite3.connect('/home/niko/sqlite_dbs/logistics_db')
+        self.cursor = self.conn.cursor()
+
+        # Track the current mode
+        self.current_mode = 'Run'  # Starts in Learn mode
+
+        # Initialize frames for Learn and Run modes
+        self.perform_frame = ttk.Frame(root)
+
+        # Grasp Submenu attributes
+        self.grasp_name = ''
+        self.idle = ''
+        self.hower = ''
+        self.grasp = ''
+        self.drop = ''
+
+        # Database Submenu attributes
+        self.pose_db = ''
+        self.grasp_db = ''
+
+        # Grasp Run Submenu attributes
+        self.grasp_run = ''
+        self.grasp_entries = {}
+        self.grasp_values = {
+            'idle_pose': 'idle',
+            'hower_pose': 'hower',
+            'grasp_pose': 'grasp',
+            'drop_pose': 'drop'
+        }
+
+        # GUI Subcomponents
+        self.grasp_name_entry = None
+        self.idle_selector = None
+        self.hower_selector = None
+        self.grasp_selector = None
+        self.drop_selector = None
+        self.width_grasp_entry = None
+        self.width_idle_entry = None
+
+        self.pose_db_selector = None
+        self.grasp_db_selector = None
+
+        self.grasp_run_selector = None
+
+        # Frames
+        self.pose_frame = None
+        self.grasp_frame = None
+        self.db_frame = None
+        self.run_frame = None
+        self.separator_1 = None
+        self.separator_2 = None
+
+        # Initially set up Learn mode
+        self.setup_run_mode()
+
+    # <editor-fold desc="Menu Methods">
+    def setup_run_mode(self):
+        self.create_run_column()
+        self.separator_1 = tk.Canvas(self.root, width=1, bg="grey")
+        self.separator_1.pack(side=tk.LEFT, fill=tk.Y)
+        self.create_grasp_column()
+        self.separator_2 = tk.Canvas(self.root, width=1, bg="grey")
+        self.separator_2.pack(side=tk.LEFT, fill=tk.Y)
+        self.create_db_edit_column()
+
+    # </editor-fold>
+
+    # <editor-fold desc="SQLite Methods">
+    def get_pose_names(self):
+        self.cursor.execute("SELECT pose_name FROM pose_table")
+        return [row[0] for row in self.cursor.fetchall()]
+
+    def get_grasp_names(self):
+        self.cursor.execute("SELECT sequence_name FROM sequence_table")
+        return [row[0] for row in self.cursor.fetchall()]
+
+    def get_pose_by_name(self, pose_name):
+        self.cursor.execute(
+            "SELECT position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w FROM pose_table WHERE pose_name = ?",
+            (pose_name,))
+        result = self.cursor.fetchone()
+        if result:
+            pose = Pose()
+            pose.position.x = result[0]
+            pose.position.y = result[1]
+            pose.position.z = result[2]
+            pose.orientation.x = result[3]
+            pose.orientation.y = result[4]
+            pose.orientation.z = result[5]
+            pose.orientation.w = result[6]
+            return pose
+        else:
+            print("Pose not found")
+            return None
+
+    def get_grasp_sequence_by_name(self, sequence_name):
+        self.cursor.execute(
+            "SELECT pose_idle, pose_hower, pose_grasp, pose_drop"
+            " FROM sequence_table WHERE sequence_name = ?",
+            (sequence_name,))
+        result = self.cursor.fetchone()
+        if result:
+            self.grasp_values['idle_pose'] = result[0]
+            self.grasp_values['hower_pose'] = result[1]
+            self.grasp_values['grasp_pose'] = result[2]
+            self.grasp_values['drop_pose'] = result[3]
+        else:
+            print("Sequence not found")
+
+    def add_new_grasp(self, sequence_name, idle, hower, grasp, drop):
+        print(sequence_name, idle, hower, grasp, drop)
+        self.cursor.execute("INSERT INTO sequence_table"
+                            """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop)
+                             VALUES (?, ?, ?, ?, ?)""",
+                            (sequence_name, idle, hower, grasp, drop))
+
+        self.conn.commit()
+
+    def delete_pose_by_name(self, pose_name):
+        self.cursor.execute("DELETE FROM pose_table WHERE pose_name = ?", (pose_name,))
+
+        self.conn.commit()
+
+    def delete_grasp_by_name(self, sequence_name):
+        self.cursor.execute("DELETE FROM sequence_table WHERE sequence_name = ?", (sequence_name,))
+
+        self.conn.commit()
+
+    # </editor-fold>
+
+    # <editor-fold desc="Update Methods">
+    def update_grasp_values(self):
+        for key, value in self.grasp_values.items():
+            if key in self.grasp_entries:
+                self.grasp_entries[key].config(state='normal')
+                self.grasp_entries[key].delete(0, tk.END)
+                self.grasp_entries[key].insert(0, f"{value}")
+                self.grasp_entries[key].config(state='readonly')
+    def update_pose_selectors(self, db_poses):
+        self.idle_selector['values'] = db_poses
+        self.hower_selector['values'] = db_poses
+        self.grasp_selector['values'] = db_poses
+        self.drop_selector['values'] = db_poses
+
+    def update_db_selectors(self, db_poses, db_grasp):
+        self.pose_db_selector['values'] = db_poses
+        self.grasp_db_selector['values'] = db_grasp
+
+    def update_from_database(self):
+        db_poses = self.get_pose_names()
+        db_grasps = self.get_grasp_names()
+        self.update_pose_selectors(db_poses)
+        self.update_db_selectors(db_poses, db_grasps)
+        self.grasp_run_selector['values'] = db_grasps
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Events">
+    # Pose Events
+    def decimal_selector_event(self, event):
+        # Get the selected value from the decimal selector
+        self.decimal_select = int(self.decimal_selector.get())
+        print("Selected Decimal Value:", self.decimal_select)
+
+    # Grasp Events
+    def idle_selector_event(self, event):
+        self.idle = self.idle_selector.get()
+
+    def hower_selector_event(self, event):
+        self.hower = self.hower_selector.get()
+
+    def grasp_selector_event(self, event):
+        self.grasp = self.grasp_selector.get()
+
+    def drop_selector_event(self, event):
+        self.drop = self.drop_selector.get()
+
+    def grasp_set_event(self):
+        self.grasp_name = self.grasp_name_entry.get()
+        self.add_new_grasp(self.grasp_name,
+                           self.idle,
+                           self.hower,
+                           self.grasp,
+                           self.drop)
+        print("Grasp Button Clicked! Grasp Name:", self.grasp_name)
+
+    # Database Events
+    def pose_db_selector_event(self, event):
+        self.pose_db = self.pose_db_selector.get()
+
+    def grasp_db_selector_event(self, event):
+        self.grasp_db = self.grasp_db_selector.get()
+
+    def pose_delete_event(self):
+        self.delete_pose_by_name(self.pose_db)
+        print("Pose: ", self.pose_db, ' was deleted!')
+
+    def grasp_delete_event(self):
+        self.delete_grasp_by_name(self.grasp_db)
+        print("Grasp: ", self.grasp_db, ' was deleted!')
+
+    # Run Events
+    def grasp_run_selector_event(self, event):
+        self.grasp_run = self.grasp_run_selector.get()
+        self.get_grasp_sequence_by_name(self.grasp_run)
+
+    def grasp_perform_event(self):
+        idle_pose = self.get_pose_by_name(self.idle)
+        hower_pose = self.get_pose_by_name(self.hower)
+        grasp_pose = self.get_pose_by_name(self.grasp)
+        drop_pose = self.get_pose_by_name(self.drop)
+
+        feedback = self.node.send_grasp_sequence_goal(idle_pose, hower_pose, grasp_pose, drop_pose)
+
+        print("Grasp: ", self.grasp_run, ' is performed!')
+
+    # </editor-fold>
+
+    # <editor-fold desc="Validate methods">
+    def validate_float(self, P):
+        try:
+            value = float(P)
+            if self.width_lower <= value <= self.width_upper:
+                return True
+            else:
+                return False
+        except ValueError:
+            return False
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Create Methods">
+    def create_grasp_column(self):
+        self.grasp_frame = tk.Frame(self.root)
+        self.grasp_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.grasp_frame, text="New Grasp", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Name Entry
+        tk.Label(self.grasp_frame, text="Grasp Name").grid(row=1, column=0)
+        self.grasp_name_entry = tk.Entry(self.grasp_frame)
+        self.grasp_name_entry.grid(row=1, column=1)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=2, column=0, columnspan=2, sticky='ew')
+
+        # Title for Current Pose
+        current_pose_label = tk.Label(self.grasp_frame, text="Grasp Poses", font=("Arial", 10, "bold"))
+        current_pose_label.grid(row=3, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=4, column=0, columnspan=2, sticky='ew')
+
+        # Idle Pose Selector
+        idle_label = tk.Label(self.grasp_frame, text="Idle Pose", font="Arial")
+        idle_label.grid(row=5, column=0)
+        self.idle_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.idle_selector.grid(row=5, column=1)
+        self.idle_selector.bind('<<ComboboxSelected>>', self.idle_selector_event)
+
+        # Hower Pose Selector
+        hower_label = tk.Label(self.grasp_frame, text="Hower Pose", font="Arial")
+        hower_label.grid(row=6, column=0)
+        self.hower_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.hower_selector.grid(row=6, column=1)
+        self.hower_selector.bind('<<ComboboxSelected>>', self.hower_selector_event)
+
+        # Grasp Pose Selector
+        grasp_label = tk.Label(self.grasp_frame, text="Grasp Pose", font="Arial")
+        grasp_label.grid(row=7, column=0)
+        self.grasp_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.grasp_selector.grid(row=7, column=1)
+        self.grasp_selector.bind('<<ComboboxSelected>>', self.grasp_selector_event)
+
+        # Drop Pose Selector
+        drop_label = tk.Label(self.grasp_frame, text="Drop Pose", font="Arial")
+        drop_label.grid(row=8, column=0)
+        self.drop_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10)
+        self.drop_selector.grid(row=8, column=1)
+        self.drop_selector.bind('<<ComboboxSelected>>', self.drop_selector_event)
+
+        # Separator
+        separator = ttk.Separator(self.grasp_frame, orient='horizontal')
+        separator.grid(row=12, column=0, columnspan=2, sticky='ew')
+
+        # Set Button
+        set_button = tk.Button(self.grasp_frame, text="Set New Grasp", command=self.grasp_set_event)
+        set_button.grid(row=13, column=0, columnspan=2)
+
+    def create_db_edit_column(self):
+        self.db_frame = tk.Frame(self.root)
+        self.db_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.db_frame, text="Edit Database", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.db_frame, orient='horizontal')
+        separator.grid(row=1, column=0, columnspan=2, sticky='ew')
+
+        # Pose Database Selector
+        pose_db_label = tk.Label(self.db_frame, text="Poses", font="Arial")
+        pose_db_label.grid(row=2, column=0)
+        self.pose_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8)
+        self.pose_db_selector.grid(row=2, column=1)
+        self.pose_db_selector.bind('<<ComboboxSelected>>', self.pose_db_selector_event)
+
+        # Pose Delete Button
+        pose_delete_button = tk.Button(self.db_frame, text="Delete Pose", command=self.pose_delete_event)
+        pose_delete_button.grid(row=3, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.db_frame, orient='horizontal')
+        separator.grid(row=4, column=0, columnspan=2, sticky='ew')
+
+        # Grasp Database Selector
+        grasp_db_label = tk.Label(self.db_frame, text="Grasp", font="Arial")
+        grasp_db_label.grid(row=5, column=0)
+        self.grasp_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8)
+        self.grasp_db_selector.grid(row=5, column=1)
+        self.grasp_db_selector.bind('<<ComboboxSelected>>', self.grasp_db_selector_event)
+
+        # Grasp Delete Button
+        grasp_delete_button = tk.Button(self.db_frame, text="Delete Grasp", command=self.grasp_delete_event)
+        grasp_delete_button.grid(row=6, column=0, columnspan=2)
+
+    def create_run_column(self):
+        self.run_frame = tk.Frame(self.root)
+        self.run_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+
+        submenu_label = tk.Label(self.run_frame, text="Run Grasp", font=("Arial", 12, "bold"))
+        submenu_label.grid(row=0, column=0, columnspan=2)
+
+        # Separator
+        separator = ttk.Separator(self.run_frame, orient='horizontal')
+        separator.grid(row=1, column=0, columnspan=2, sticky='ew')
+
+        # Pose Database Selector
+        grasp_label = tk.Label(self.run_frame, text="Grasp", font="Arial")
+        grasp_label.grid(row=2, column=0)
+        self.grasp_run_selector = ttk.Combobox(self.run_frame, values=[""], width=8)
+        self.grasp_run_selector.grid(row=2, column=1)
+        self.grasp_run_selector.bind('<<ComboboxSelected>>', self.grasp_run_selector_event)
+
+        # Separator
+        separator = ttk.Separator(self.run_frame, orient='horizontal')
+        separator.grid(row=3, column=0, columnspan=2, sticky='ew')
+
+        grasp_labels = ['Idle Pose', 'Hower Pose', 'Grasp Pose', 'Drop Pose']
+
+        for i, label in enumerate(grasp_labels, start=4):
+            tk.Label(self.run_frame, text=label).grid(row=i, column=0)
+            entry = tk.Entry(self.run_frame, state='readonly', width=8)
+            entry.grid(row=i, column=1)
+            self.grasp_entries[label.replace(' ', '_').lower()] = entry
+
+        # Separator
+        separator = ttk.Separator(self.run_frame, orient='horizontal')
+        separator.grid(row=10, column=0, columnspan=2, sticky='ew')
+
+        # Grasp Delete Button
+        grasp_delete_button = tk.Button(self.run_frame, text="Perform Grasp", command=self.grasp_perform_event)
+        grasp_delete_button.grid(row=11, column=0, columnspan=2)
+
+    # </editor-fold>
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    gui_node = GUINode()
+
+    thread_spin = threading.Thread(target=rclpy.spin, args=(gui_node,))
+    thread_spin.start()
+
+    root = tk.Tk()
+
+    # app = MyGUI(root)
+    app = MyGUI(root, gui_node)
+    while True:
+        app.update_from_database()
+        app.update_grasp_values()
+        root.update()
+
+    gui_node.destroy_node()
+    rclpy.shutdown()
+    app.conn.close()
+
+
+if __name__ == '__main__':
+    main()

franka_logistics/logistics_node.py

@@ -51,41 +51,31 @@ class LogisticsNode(Node):
 
     def process_sequence(self, sequence_name):
         # Query the database for the sequence
-        self.cursor.execute('''
+        raise(NotImplementedError)
-             SELECT p1.position_x, p1.position_y, p1.position_z,
+        # result = self.cursor.fetchone()
-                    p1.orientation_x, p1.orientation_y, p1.orientation_z, p1.orientation_w,
-                    p2.position_x, p2.position_y, p2.position_z,
-                    p2.orientation_x, p2.orientation_y, p2.orientation_z, p2.orientation_w,
-                    sequence_table.width
-             FROM sequence_table
-             INNER JOIN pose_table AS p1 ON pose_sequence_table.pose_1 = p1.pose_id
-             INNER JOIN pose_table AS p2 ON pose_sequence_table.pose_2 = p2.pose_id
-             WHERE pose_sequence_table.sequence_name = ?
-         ''', (sequence_name,))
-        result = self.cursor.fetchone()
 
-        if result:
+        # if result:
-            pose_array = PoseArray()
+        #     pose_array = PoseArray()
-            # Pose 1
+        #     # Pose 1
-            pose1 = Pose()
+        #     pose1 = Pose()
-            pose1.position.x, pose1.position.y, pose1.position.z = result[0:3]
+        #     pose1.position.x, pose1.position.y, pose1.position.z = result[0:3]
-            pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w = result[3:7]
+        #     pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w = result[3:7]
-            pose_array.poses.append(pose1)
+        #     pose_array.poses.append(pose1)
-
+        #
-            # Pose 2
+        #     # Pose 2
-            pose2 = Pose()
+        #     pose2 = Pose()
-            pose2.position.x, pose2.position.y, pose2.position.z = result[7:10]
+        #     pose2.position.x, pose2.position.y, pose2.position.z = result[7:10]
-            pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w = result[10:14]
+        #     pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w = result[10:14]
-            pose_array.poses.append(pose2)
+        #     pose_array.poses.append(pose2)
-
+        #
-            # Width of the object
+        #     # Width of the object
-            width = result[-1]
+        #     width = result[-1]
-
+        #
-            # Publish or process the PoseArray as needed
+        #     # Publish or process the PoseArray as needed
-            # ...
+        #     # ...
-
+        #
-        else:
+        # else:
-            self.get_logger().info('Sequence not found')
+        #     self.get_logger().info('Sequence not found')
 
 
 def main(args=None):

franka_logistics/pose_learn_node.py

@@ -52,8 +52,6 @@ class PoseLearnNode(Node):
                 pose_hower TEXT NOT NULL,
                 pose_grasp TEXT NOT NULL,
                 pose_drop TEXT NOT NULL,
-                width_grasp REAL NOT NULL,
-                width_idle REAL NOT NULL,
                 FOREIGN KEY (pose_idle) REFERENCES pose_table (pose_name),
                 FOREIGN KEY (pose_hower) REFERENCES pose_table (pose_name),
                 FOREIGN KEY (pose_grasp) REFERENCES pose_table (pose_name),
@@ -69,6 +67,7 @@ class PoseLearnNode(Node):
     def save_pose_callback(self, msg):
         pose_name = msg.data
         position = self.current_pose.pose.position
+        position = self.current_pose.pose.position
         orientation = self.current_pose.pose.orientation
         try:
 
@@ -85,10 +84,9 @@ class PoseLearnNode(Node):
     def save_sequence_callback(self, msg):
         try:
             self.cursor.execute('''INSERT INTO sequence_table
-            (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop, width_grasp, width_idle)
+            (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop)
-             VALUES (?, ?, ?, ?, ?, ?, ?)''',
+             VALUES (?, ?, ?, ?, ?)''',
-                                (msg.sequence_name, msg.pose_idle, msg.pose_hower, msg.pose_grasp,
+                                (msg.sequence_name, msg.pose_idle, msg.pose_hower, msg.pose_grasp, msg.pose_drop))
-                                 msg.pose_drop, msg.width_grasp, msg.width_idle))
             self.conn.commit()
         except sqlite3.IntegrityError:
             self.get_logger().info(f"Sequence with name '{msg.sequence_name}' already exists.")

setup.py

@@ -25,7 +25,9 @@ setup(
         'console_scripts': [
             'franka_logistics = franka_logistics.logistics_node:main',
             'pose_learn = franka_logistics.pose_learn_node:main',
-            'franka_grasp_gui = franka_logistics.GUI:main'
+            'franka_grasp_gui = franka_logistics.GUI:main',
+            'franka_pose_learn_gui = franka_logistics.Pose_Learn_GUI:main',
+            'franka_run_gui = franka_logistics.Run_GUI:main'
         ],
     },
 )