diff --git a/franka_logistics/GUI.py b/franka_logistics/GUI.py
new file mode 100755
index 0000000..b1e69a8
--- /dev/null
+++ b/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()
+
+ #
+
+ 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()
+
+ #
+
+ #
+ 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, 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()
+
+ #
+
+ #
+ 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)
+
+ #
+
+ #
+ # 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!')
+
+ #
+
+ #
+ 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=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('<>', 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=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('<>', 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', '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)
+
+ #
+
+
+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/pose_learn_node.py b/franka_logistics/pose_learn_node.py
old mode 100644
new mode 100755
index 2f12734..e1a9b2a
--- a/franka_logistics/pose_learn_node.py
+++ b/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.")
diff --git a/package.xml b/package.xml
index 157e296..5288a37 100644
--- a/package.xml
+++ b/package.xml
@@ -11,6 +11,7 @@
geometry_msgs
moveit_msgs
std_msgs
+ franka_cps_msgs
ament_copyright
ament_flake8
diff --git a/setup.py b/setup.py
index 5c10335..fddb267 100644
--- a/setup.py
+++ b/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'
],
},
)