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