From 618ecc7a26df7f7a5f53259aea7f4bd3f8867008 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 1 Feb 2024 19:54:30 +0100 Subject: [PATCH] task not working --- franka_logistics/GUI.py | 94 +++-- franka_logistics/Pose_Learn_GUI.py | 518 ++++++++++++++++++++++++++++ franka_logistics/Run_GUI.py | 436 +++++++++++++++++++++++ franka_logistics/logistics_node.py | 58 ++-- franka_logistics/pose_learn_node.py | 10 +- setup.py | 4 +- 6 files changed, 1026 insertions(+), 94 deletions(-) create mode 100644 franka_logistics/Pose_Learn_GUI.py create mode 100644 franka_logistics/Run_GUI.py diff --git a/franka_logistics/GUI.py b/franka_logistics/GUI.py index b1e69a8..3d8c0f0 100755 --- a/franka_logistics/GUI.py +++ b/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, - orientation_x, orientation_y, orientation_z, orientation_w)) + (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, 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) - VALUES (?, ?, ?, ?, ?, ?, ?)""", - (sequence_name, idle, hower, grasp, drop, width_grasp, width_idle)) + """ (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop) + VALUES (?, ?, ?, ?, ?)""", + (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) # @@ -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'], - 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(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 @@ -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.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, '') + self.add_new_grasp(self.grasp_name, + self.grasp_entries['idle_pose'], + self.grasp_entries['hower_pose'], + 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, - drop_pose, self.width_grasp, self.width_idle) + feedback = self.node.send_grasp_sequence_goal(idle_pose, hower_pose, grasp_pose, drop_pose) 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('<>', self.decimal_selector_event) @@ -518,23 +524,6 @@ class MyGUI: self.drop_selector.grid(row=8, column=1) self.drop_selector.bind('<>', 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', - 'Drop Pose', 'Grasp Width', 'Idle Width'] + 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) @@ -637,7 +626,6 @@ def main(args=None): app.update_pose_values() root.update() - gui_node.destroy_node() rclpy.shutdown() app.conn.close() diff --git a/franka_logistics/Pose_Learn_GUI.py b/franka_logistics/Pose_Learn_GUI.py new file mode 100644 index 0000000..e325c56 --- /dev/null +++ b/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() + + # + + 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 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 + + # + + # + 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() + + # + + # + 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) + + # + + # + # 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!') + + # + + # + 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 + + # + + # + 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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 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() diff --git a/franka_logistics/Run_GUI.py b/franka_logistics/Run_GUI.py new file mode 100644 index 0000000..5db2564 --- /dev/null +++ b/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() + + # + 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() + + # + + # + 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() + + # + + # + 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 + + # + + # + # 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!') + + # + + # + 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 + + # + + # + 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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('<>', 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) + + # + + +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() diff --git a/franka_logistics/logistics_node.py b/franka_logistics/logistics_node.py index dfd92b2..f1adeea 100755 --- a/franka_logistics/logistics_node.py +++ b/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(''' - SELECT p1.position_x, p1.position_y, p1.position_z, - 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() + raise(NotImplementedError) + # result = self.cursor.fetchone() - if result: - pose_array = PoseArray() - # Pose 1 - pose1 = Pose() - 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] - pose_array.poses.append(pose1) - - # Pose 2 - pose2 = Pose() - 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] - pose_array.poses.append(pose2) - - # Width of the object - width = result[-1] - - # Publish or process the PoseArray as needed - # ... - - else: - self.get_logger().info('Sequence not found') + # if result: + # pose_array = PoseArray() + # # Pose 1 + # pose1 = Pose() + # 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] + # pose_array.poses.append(pose1) + # + # # Pose 2 + # pose2 = Pose() + # 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] + # pose_array.poses.append(pose2) + # + # # Width of the object + # width = result[-1] + # + # # Publish or process the PoseArray as needed + # # ... + # + # else: + # self.get_logger().info('Sequence not found') def main(args=None): diff --git a/franka_logistics/pose_learn_node.py b/franka_logistics/pose_learn_node.py index e1a9b2a..ad3a813 100755 --- a/franka_logistics/pose_learn_node.py +++ b/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) - VALUES (?, ?, ?, ?, ?, ?, ?)''', - (msg.sequence_name, msg.pose_idle, msg.pose_hower, msg.pose_grasp, - msg.pose_drop, msg.width_grasp, msg.width_idle)) + (sequence_name, pose_idle, pose_hower, pose_grasp, pose_drop) + VALUES (?, ?, ?, ?, ?)''', + (msg.sequence_name, msg.pose_idle, msg.pose_hower, msg.pose_grasp, msg.pose_drop)) self.conn.commit() except sqlite3.IntegrityError: self.get_logger().info(f"Sequence with name '{msg.sequence_name}' already exists.") diff --git a/setup.py b/setup.py index fddb267..7f2742d 100644 --- a/setup.py +++ b/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' ], }, )