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' ], }, )