added gui

franka_logistics/GUI.py

@@ -0,0 +1,647 @@
+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.create_subscription(PoseStamped, 'franka_grasp_moveit/current_pose', self.pose_callback, 10)
+        self.moveit_action_client = ActionClient(self, GraspSequence, 'franka_grasp_moveit/grasp')
+
+        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
+
+    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 = 'Learn'  # Starts in Learn mode
+
+        # Menu Bar for switching modes
+        self.menubar = tk.Menu(self.root)
+        self.root.config(menu=self.menubar)
+
+        # Toggle button for mode switch
+        self.menubar.add_command(label="Toggle Mode", command=self.toggle_mode)
+
+        # Initialize frames for Learn and Run modes
+        self.learn_frame = ttk.Frame(root)
+        self.perform_frame = ttk.Frame(root)
+
+        # range of grasp width
+        self.width_lower = 0.0
+        self.width_upper = 0.1
+
+        # Pose Submenu attributes
+        self.pose_name = ''
+        self.pose_entries = {
+            '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 = 1
+
+        # Grasp Submenu attributes
+        self.grasp_name = ''
+        self.idle = ''
+        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 = ''
+        self.grasp_db = ''
+
+        # Grasp Run Submenu attributes
+        self.grasp_run = ''
+        self.grasp_entries = {
+            'idle_pose': 'Idle',
+            'hower_pose': 'Hower',
+            'grasp_pose': 'Grasp',
+            'drop_pose': 'Drop',
+            'grasp_width': 0.0,
+            'idle_width': 0.0,
+        }
+
+        # 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
+
+        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_learn_mode()
+
+    # <editor-fold desc="Menu Methods">
+
+    def toggle_mode(self):
+        if self.current_mode == 'Learn':
+            self.switch_to_run()
+        else:
+            self.switch_to_learn()
+
+    def switch_to_learn(self):
+        # Logic to switch to Learn mode
+        self.perform_frame.pack_forget()
+        self.grasp_frame.pack_forget()
+        self.separator_1.pack_forget()
+        self.db_frame.pack_forget()
+        self.separator_2.pack_forget()
+        self.run_frame.pack_forget()
+
+        self.learn_frame.pack()
+
+        self.setup_learn_mode()
+        self.current_mode = 'Learn'
+
+    def switch_to_run(self):
+        # Logic to switch to Run mode
+        self.learn_frame.pack_forget()
+
+        self.grasp_frame.pack_forget()
+        self.separator_1.pack_forget()
+        self.db_frame.pack_forget()
+        self.separator_2.pack_forget()
+        self.pose_frame.pack_forget()
+
+        self.perform_frame.pack()
+
+        self.setup_run_mode()
+        self.current_mode = 'Run'
+
+    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()
+
+    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="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, width_grasp, width_idle"
+            " 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]
+            self.grasp_entries['width_grasp'] = result[4]
+            self.grasp_entries['width_idle'] = result[5]
+        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, position_x, position_y, position_z,
+             orientation_x, orientation_y, orientation_z, orientation_w))
+
+        self.conn.commit()
+
+    def add_new_grasp(self, sequence_name, idle, hower, grasp, drop, width_grasp, width_idle):
+        self.cursor.execute("INSERT INTO sequence_table"
+                            """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop, width_grasp, width_idle)
+                             VALUES (?, ?, ?, ?, ?, ?, ?)""",
+                            (sequence_name, idle, hower, grasp, drop, width_grasp, width_idle))
+
+        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}")
+                self.pose_entries[key].config(state='readonly')  # Set back to read-only
+
+    def update_grasp_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_grasp_selectors(db_grasps)
+        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 = 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'],
+                          self.pose_entries['position_y'], self.pose_entries['position_z'],
+                          self.pose_entries['orientation_x'], self.pose_entries['orientation_y'],
+                          self.pose_entries['orientation_z'], self.pose_entries['orientation_w'])
+        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.width_idle = float(self.width_idle_str.get())
+        self.width_grasp = float(self.width_grasp_str.get())
+        print("Grasp Button Clicked! Grasp Name:", self.grasp_name, 'Grasp Width:', self.width_grasp, 'Idle Width:',
+              self.width_idle, '')
+
+    # 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, self.width_grasp, self.width_idle)
+
+        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=6)
+            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=["1", "2", "3"], 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=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')
+
+        # 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', '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)
+            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_pose_values()
+        root.update()
+
+
+    gui_node.destroy_node()
+    rclpy.shutdown()
+    app.conn.close()
+
+
+if __name__ == '__main__':
+    main()

franka_logistics/pose_learn_node.py

@@ -16,9 +16,9 @@ class PoseLearnNode(Node):
         self.pose_sub = self.create_subscription(PoseStamped, 'moveit_interface/current_pose',
                                                  self.pose_callback, 10)
 
-        self.save_sub = self.create_subscription(String, '/logistic/save_pose', self.save_callback, 10)
+        self.save_sub = self.create_subscription(String, '/logistic/save_pose', self.save_pose_callback, 10)
 
-        self.seq_sub = self.create_subscription(String, '/logistic/save_seq', self.seq_callback, 10)
+        self.seq_sub = self.create_subscription(String, '/logistic/save_seq', self.save_sequence_callback, 10)
 
         # Connect to Database
         self.conn = None
@@ -45,22 +45,28 @@ class PoseLearnNode(Node):
             orientation_w REAL NOT NULL)''')
 
         self.cursor.execute('''
-            CREATE TABLE IF NOT EXISTS pose_sequence_table (
+            CREATE TABLE IF NOT EXISTS sequence_table (
                 sequence_id INTEGER PRIMARY KEY AUTOINCREMENT,
                 sequence_name TEXT UNIQUE NOT NULL,
-                pose_1 INTEGER NOT NULL,
-                pose_2 INTEGER NOT NULL,
-                width REAL NOT NULL,
-                FOREIGN KEY (pose_1) REFERENCES pose_table (pose_id),
-                FOREIGN KEY (pose_2) REFERENCES pose_table (pose_id)
+                pose_idle TEXT NOT NULL,
+                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),
+                FOREIGN KEY (pose_drop) REFERENCES pose_table (pose_name)
             )
         ''')
         self.conn.commit()
+        self.get_logger().info('Initializing PoseLearnNode')
 
     def pose_callback(self, msg):
         self.current_pose = msg
 
-    def save_callback(self, msg):
+    def save_pose_callback(self, msg):
         pose_name = msg.data
         position = self.current_pose.pose.position
         orientation = self.current_pose.pose.orientation
@@ -76,11 +82,13 @@ class PoseLearnNode(Node):
         except sqlite3.IntegrityError:
             self.get_logger().info(f"Pose with name '{pose_name}' already exists.")
 
-    def save_seq(self, msg):
+    def save_sequence_callback(self, msg):
         try:
             self.cursor.execute('''INSERT INTO sequence_table
-            (sequence_name, pose_1, pose_2, width) VALUES (?, ?, ?, ?)''',
-                                (msg.sequence_name, msg.pose_1, msg.pose_2, msg.width))
+            (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop, width_grasp, width_idle)
+             VALUES (?, ?, ?, ?, ?, ?, ?)''',
+                                (msg.sequence_name, msg.pose_idle, msg.pose_hower, msg.pose_grasp,
+                                 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.")

package.xml

@@ -11,6 +11,7 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>franka_cps_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

setup.py

@@ -1,4 +1,6 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'franka_logistics'
 
@@ -9,6 +11,7 @@ setup(
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
+        (os.path.join('share', package_name), glob('launch/*.launch.py')),
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
@@ -20,6 +23,9 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
+            'franka_logistics = franka_logistics.logistics_node:main',
+            'pose_learn = franka_logistics.pose_learn_node:main',
+            'franka_grasp_gui = franka_logistics.GUI:main'
         ],
     },
 )