From 2bc8283ba1a693a4902996cdc6bbafd2c7835ffc Mon Sep 17 00:00:00 2001 From: Niko Date: Fri, 2 Feb 2024 21:09:59 +0100 Subject: [PATCH] task not working --- franka_logistics/grasp_demo_node.py | 249 +++++++++++++ franka_logistics/position_learn_gui.py | 489 +++++++++++++++++++++++++ franka_logistics/position_run_gui.py | 439 ++++++++++++++++++++++ launch/position_learn.launch.py | 146 ++++++++ launch/position_run.launch.py | 146 ++++++++ package.xml | 3 + setup.py | 6 +- 7 files changed, 1477 insertions(+), 1 deletion(-) create mode 100644 franka_logistics/grasp_demo_node.py create mode 100644 franka_logistics/position_learn_gui.py create mode 100644 franka_logistics/position_run_gui.py create mode 100644 launch/position_learn.launch.py create mode 100644 launch/position_run.launch.py diff --git a/franka_logistics/grasp_demo_node.py b/franka_logistics/grasp_demo_node.py new file mode 100644 index 0000000..4726150 --- /dev/null +++ b/franka_logistics/grasp_demo_node.py @@ -0,0 +1,249 @@ +import rclpy +from rclpy.node import Node +from rclpy.action import ActionClient, ActionServer +from rclpy.duration import Duration + +from franka_msgs.action import Grasp +from franka_msgs.action import Move +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from franka_cps_msgs.action import GraspSequence + +from threading import Event + +import sqlite3 + +class GraspDemoNode(Node): + def __init__(self): + super().__init__('grasp_demo_node') + + # Event Handling + self.action_completed_event = Event() + self._action_result = None + # Grasp Sequence Service + self.execute_action = ActionServer(self, GraspSequence, + 'franka_grasp/grasp_sequence', self.execute_callback) + + # Franka Position Controller + self.position_client = ActionClient(self, FollowJointTrajectory, + 'panda_arm_controller/follow_joint_trajectory') + + self._send_position_future = None + self._position_result_future = None + + # Franka Gripper Action Server clients for Move and Grasp + self.franka_move_action_client = ActionClient(self, Move, 'panda_gripper/move') + self.franka_grasp_action_client = ActionClient(self, Grasp, 'panda_gripper/grasp') + + self._send_move_future = None + self._move_result_future = None + self._send_grasp_future = None + self._grasp_result_future = None + + # Sqlite Database + self.conn = None + self.cursor = None + self.database_name = '/home/niko/sqlite_dbs/logistics_db' + + self.init_db() + + def init_db(self): + # Connect to the sqlite db + self.conn = sqlite3.connect(self.database_name) + self.cursor = self.conn.cursor() + + self.cursor.execute( + '''CREATE TABLE IF NOT EXISTS position_table ( + position_id INTEGER PRIMARY KEY AUTOINCREMENT, + position_name TEXT UNIQUE NOT NULL, + joint_1 REAL NOT NULL, + joint_2 REAL NOT NULL, + joint_3 REAL NOT NULL, + joint_4 REAL NOT NULL, + joint_5 REAL NOT NULL, + joint_6 REAL NOT NULL, + joint_7 REAL NOT NULL)''') + + self.cursor.execute(''' + CREATE TABLE IF NOT EXISTS position_sequence_table ( + sequence_id INTEGER PRIMARY KEY AUTOINCREMENT, + sequence_name TEXT UNIQUE NOT NULL, + position_idle TEXT NOT NULL, + position_hower TEXT NOT NULL, + position_grasp TEXT NOT NULL, + position_drop TEXT NOT NULL, + FOREIGN KEY (position_idle) REFERENCES position_table (position_name), + FOREIGN KEY (position_hower) REFERENCES position_table (position_name), + FOREIGN KEY (position_grasp) REFERENCES position_table (position_name), + FOREIGN KEY (position_drop) REFERENCES position_table (position_name) + ) + ''') + self.conn.commit() + self.get_logger().info('Initializing Grasp Demo Node finished!') + + def execute_callback(self, goal_handle): + self.get_logger().info(' Executing Grasp Sequence') + feedback = GraspSequence.Feedback() + + sequence = [ + (self.arm_move_to_position, (goal_handle.request.hower, 10), "Moving to Hower Position"), + (self.arm_move_to_position, (goal_handle.request.grasp, 10), "Moving to Grasp Position"), + (self.gripper_grasp, None, "Grasping Object"), + (self.arm_move_to_position, (goal_handle.request.hower, 10), "Moving back to Hower Position"), + (self.arm_move_to_position, (goal_handle.request.drop, 10), "Moving back to Drop Position"), + (self.gripper_move, 0.08, "Opening Gripper"), + (self.arm_move_to_position, (goal_handle.request.idle, 5), "Moving to Idle Position") + ] + + for action, args, step_descrioption in sequence: + feedback.current_step = step_descrioption + goal_handle.publish_feedback(feedback) + # if goal_handle.is_cancle_requested(): + # goal_handle.canceled() + # return GraspSequence.Result(success=False, error_code="Canceled") + + success = action(*args) if args else action() + if not success: + goal_handle.abort() + return GraspSequence.Result(success=False, error_code="aborted") + + goal_handle.succeed() + return GraspSequence.Result(sucess=True) + + + # region Position Controller + def arm_move_to_position(self, joint_positions, duration): + self.action_completed_event.clear() # Reset the event before sending a new goal + + goal_msg = FollowJointTrajectory.Goal() + trajectory = JointTrajectory() + trajectory.joint_names = ['panda_joint1', 'panda_joint2', + 'panda_joint3', 'panda_joint4', + 'panda_joint5', 'panda_joint6', + 'panda_joint7'] + point = JointTrajectoryPoint() + point.positions = joint_positions.position + time = Duration(seconds=duration) + self.get_logger().error(f"Time: {time}") + point.time_from_start = time.to_msg() + self.get_logger().error(f"NEW Point{point}") + trajectory.points.append(point) + goal_msg.trajectory = trajectory + self.get_logger().error(f'Goal:{goal_msg}') + + self.position_client.wait_for_server() + self._send_position_future = self.position_client.send_goal_async(goal_msg) + self._send_position_future.add_done_callback(self.position_response_callback) + # Wait here until the action completes + self.action_completed_event.wait() + + # After the event is set in position_result_callback + if hasattr(self, '_action_result') and self._action_result: + self._action_result = None + return True + return False + + def position_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Positon goal rejected!') + self.action_completed_event.set() # Signal that the action is completed (with failure) + return + + self.get_logger().info('Position goal accepted!') + self._position_result_future = goal_handle.get_result_async() + self._position_result_future.add_done_callback(self.position_result_callback) + + def position_result_callback(self, future): + result = future.result().result + self.get_logger().info(f'Position result: Success: {result.success}, Error: {result.error}') + self._action_result = result.success # Store the result for checking after wait + self.action_completed_event.set() # Signal that the action is completed + # endregion + + # region Grasp Action + def gripper_grasp(self): + self.action_completed_event.clear() # Reset the event before sending a new goal + + goal_msg = Grasp() + goal_msg.width = 0.0 + goal_msg.speed = 0.1 + goal_msg.force = 50.0 + + self.franka_grasp_action_client.wait_for_server() + self._send_grasp_future = self.franka_grasp_action_client.send_goal_async(goal_msg, feedback_callback=self.action_fb_callback) + self._send_grasp_future.add_done_callback(self.grasp_response_callback) + self.action_completed_event.wait() + + # After the event is set in grasp_result_callback + if hasattr(self, '_action_result') and self._action_result: + self._action_result = None + return True + return False + + def action_fb_callback(self, msg): + feedback = msg.feedback + self.get_logger().info(f'Received feedback: Current Width: {feedback.current_width}') + + def grasp_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Grasp goal rejected!') + self.action_completed_event.set() # Signal that the action is completed (with failure) + return + + self.get_logger().info('Grasp goal accepted!') + self._grasp_result_future = goal_handle.get_result_async() + self._grasp_result_future.add_done_callback(self.grasp_result_callback) + + def grasp_result_callback(self, future): + result = future.result().result + self.get_logger().info(f'Grasp result: Success: {result.success}, Error: {result.error}') + self._action_result = result.success # Store the result for checking after wait + self.action_completed_event.set() # Signal that the action is completed + # endregion + + # region Move Action + def gripper_move(self, width): + goal_msg = Move() + goal_msg.width = width + goal_msg.speed = 0.1 + + self.franka_move_action_client.wait_for_server() + self._send_move_future = self.franka_move_action_client.send_goal_async(goal_msg, feedback_callback=self.action_fb_callback) + self._send_move_future.add_done_callback(self.move_response_callback) + self.action_completed_event.wait() + + # After the event is set in move_result_callback + if hasattr(self, '_action_result') and self._action_result: + self._action_result = None + return True + return False + + def move_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Move goal rejected!') + self.action_completed_event.set() # Signal that the action is completed (with failure) + return + + self.get_logger().info('Move goal accepted!') + self._move_result_future = goal_handle.get_result_async() + self._move_result_future.add_done_callback(self.move_result_callback) + + def move_result_callback(self, future): + result = future.result().result + self.get_logger().info(f'Move result: Success: {result.success}, Error: {result.error}') + self._action_result = result.success # Store the result for checking after wait + self.action_completed_event.set() # Signal that the action is completed + # endregion + + +def main(args=None): + rclpy.init(args=args) + node = GraspDemoNode() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/franka_logistics/position_learn_gui.py b/franka_logistics/position_learn_gui.py new file mode 100644 index 0000000..f751c6c --- /dev/null +++ b/franka_logistics/position_learn_gui.py @@ -0,0 +1,489 @@ +import threading +import tkinter as tk +from tkinter import ttk + +import sqlite3 + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import JointState + + +class GUINode(Node): + def __init__(self): + super().__init__('position_learn_gui_node') + + self.create_subscription(JointState, 'franka/joint_states', self.position_callback, 10) + + self.current_position = { + 'joint_1': 0.0, + 'joint_2': 0.0, + 'joint_3': 0.0, + 'joint_4': 0.0, + 'joint_5': 0.0, + 'joint_6': 0.0, + 'joint_7': 0.0 + } + + def position_callback(self, msg): + self.current_position['joint_1'] = msg.position[0] + self.current_position['joint_2'] = msg.position[1] + self.current_position['joint_3'] = msg.position[2] + self.current_position['joint_4'] = msg.position[3] + self.current_position['joint_5'] = msg.position[4] + self.current_position['joint_6'] = msg.position[5] + self.current_position['joint_7'] = msg.position[6] + + +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) + + # Position Submenu attributes + self.position_name = '' + self.position_entries = {} + self.position_entries_values = { + 'joint_1': 0.0, + 'joint_2': 0.0, + 'joint_3': 0.0, + 'joint_4': 0.0, + 'joint_5': 0.0, + 'joint_6': 0.0, + 'joint_7': 0.0 + } + + # Grasp Submenu attributes + self.grasp_name = '' + self.idle = '' + self.hower = '' + self.grasp = '' + self.drop = '' + + # Database Submenu attributes + self.position_db = '' + self.grasp_db = '' + + # Grasp Run Submenu attributes + self.grasp_run = '' + self.grasp_entries = {} + + # GUI Subcomponents + self.position_name_entry = 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.position_db_selector = None + self.grasp_db_selector = None + + # Frames + self.position_frame = None + self.grasp_frame = None + self.db_frame = None + self.separator_1 = None + self.separator_2 = None + + # Initially set up Learn mode + self.setup_learn_mode() + + # + + def setup_learn_mode(self): + self.create_position_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_position_values(self): + initial_values = { + 'joint_1': 0.0, + 'joint_2': 0.0, + 'joint_3': 0.0, + 'joint_4': 0.0, + 'joint_5': 0.0, + 'joint_6': 0.0, + 'joint_7': 0.0 + } + for key, value in initial_values.items(): + if key in self.position_entries: + self.position_entries[key].config(state='normal') # Enable writing + self.position_entries[key].delete(0, tk.END) + self.position_entries[key].insert(0, str(value)) + self.position_entries[key].config(state='readonly') # Set back to read-only + + # + + # + def get_position_names(self): + self.cursor.execute("SELECT position_name FROM position_table") + return [row[0] for row in self.cursor.fetchall()] + + def get_grasp_names(self): + self.cursor.execute("SELECT sequence_name FROM position_sequence_table") + return [row[0] for row in self.cursor.fetchall()] + + def get_position_by_name(self, position_name): + self.cursor.execute( + "SELECT joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7 FROM position_table WHERE position_name = ?", + (position_name,)) + result = self.cursor.fetchone() + if result: + position = JointState() + position.position = result + return position + else: + print("Position not found") + return None + + def get_grasp_sequence_by_name(self, sequence_name): + self.cursor.execute( + "SELECT position_idle, position_hower, position_grasp, position_drop" + " FROM position_sequence_table WHERE sequence_name = ?", + (sequence_name,)) + result = self.cursor.fetchone() + if result: + self.grasp_entries['idle_position'] = result[0] + self.grasp_entries['hower_position'] = result[1] + self.grasp_entries['grasp_position'] = result[2] + self.grasp_entries['drop_position'] = result[3] + else: + print("Sequence not found") + + def add_new_position(self, position_name, joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7): + self.cursor.execute( + "INSERT INTO position_table (position_name, joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7)" + " VALUES (?, ?, ?, ?, ?, ?, ?, ?)", + (position_name, + joint_1, + joint_2, + joint_3, + joint_4, + joint_5, + joint_6, + joint_7)) + + 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 position_sequence_table" + """ (sequence_name, position_idle, position_hower, position_grasp, position_drop) + VALUES (?, ?, ?, ?, ?)""", + (sequence_name, idle, hower, grasp, drop)) + + self.conn.commit() + + def delete_position_by_name(self, position_name): + self.cursor.execute("DELETE FROM position_table WHERE position_name = ?", (position_name,)) + + self.conn.commit() + + def delete_grasp_by_name(self, sequence_name): + self.cursor.execute("DELETE FROM position_sequence_table WHERE sequence_name = ?", (sequence_name,)) + + self.conn.commit() + + # + + # + def update_position_values(self): + for key, value in self.node.current_position.items(): + if key in self.position_entries: + self.position_entries[key].config(state='normal') # Enable writing + self.position_entries[key].delete(0, tk.END) + self.position_entries[key].insert(0, f"{value:.5f}") + self.position_entries[key].config(state='readonly') # Set back to read-only + + def update_position_selectors(self, db_positions): + self.idle_selector['values'] = db_positions + self.hower_selector['values'] = db_positions + self.grasp_selector['values'] = db_positions + self.drop_selector['values'] = db_positions + + def update_db_selectors(self, db_positions, db_grasp): + self.position_db_selector['values'] = db_positions + self.grasp_db_selector['values'] = db_grasp + + def update_from_database(self): + db_positions = self.get_position_names() + db_grasps = self.get_grasp_names() + self.update_position_selectors(db_positions) + self.update_db_selectors(db_positions, db_grasps) + + # + + # + # Position Events + def position_set_event(self): + self.position_name = self.position_name_entry.get() + + self.add_new_position(self.position_name, + float(self.position_entries['joint_1'].get()), + float(self.position_entries['joint_2'].get()), + float(self.position_entries['joint_3'].get()), + float(self.position_entries['joint_4'].get()), + float(self.position_entries['joint_5'].get()), + float(self.position_entries['joint_6'].get()), + float(self.position_entries['joint_7'].get())) + print("Position Button Clicked! Position Name:", self.position_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 position_db_selector_event(self, event): + self.position_db = self.position_db_selector.get() + + def grasp_db_selector_event(self, event): + self.grasp_db = self.grasp_db_selector.get() + + def position_delete_event(self): + self.delete_position_by_name(self.position_db) + print("Position: ", self.position_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 = self.get_position_by_name(self.idle) + hower = self.get_position_by_name(self.hower) + grasp = self.get_position_by_name(self.grasp) + drop = self.get_position_by_name(self.drop) + + feedback = self.node.send_grasp_sequence_goal(idle, hower, grasp, drop) + + 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_position_column(self): + self.position_frame = tk.Frame(self.root) + self.position_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + + submenu_label = tk.Label(self.position_frame, text="New Position", font=("Arial", 12, "bold")) + submenu_label.grid(row=0, column=0, columnspan=2) + + # Name Entry + tk.Label(self.position_frame, text="Position Name").grid(row=1, column=0) + self.position_name_entry = tk.Entry(self.position_frame) + self.position_name_entry.grid(row=1, column=1) + + # Separator + separator = ttk.Separator(self.position_frame, orient='horizontal') + separator.grid(row=2, column=0, columnspan=2, sticky='ew') + + # Title for Current Position + current_position_label = tk.Label(self.position_frame, text="Current Position", font=("Arial", 10, "bold")) + current_position_label.grid(row=3, column=0, columnspan=2) + + # Separator + separator = ttk.Separator(self.position_frame, orient='horizontal') + separator.grid(row=4, column=0, columnspan=2, sticky='ew') + + # Position Values Entries + position_labels = ['Joint 1', 'Joint 2', 'Joint 3', 'Joint 4', 'Joint 5', 'Joint 6', 'Joint 7'] + for i, label in enumerate(position_labels, start=5): + tk.Label(self.position_frame, text=label).grid(row=i, column=0) + entry = tk.Entry(self.position_frame, state='readonly', width=10) + entry.grid(row=i, column=1) + self.position_entries[label.replace(' ', '_').lower()] = entry + + self.set_initial_position_values() + + # Separator + separator = ttk.Separator(self.position_frame, orient='horizontal') + separator.grid(row=14, column=0, columnspan=2, sticky='ew') + + # Set Button + set_button = tk.Button(self.position_frame, text="Set New Position", command=self.position_set_event) + set_button.grid(row=15, column=0, columnspan=2) + + self.position_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 Position + current_position_label = tk.Label(self.grasp_frame, text="Grasp Position", font=("Arial", 10, "bold")) + current_position_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 Position Selector + idle_label = tk.Label(self.grasp_frame, text="Idle Position", 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 Position Selector + hower_label = tk.Label(self.grasp_frame, text="Hower Position", 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 Position Selector + grasp_label = tk.Label(self.grasp_frame, text="Grasp Position", 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 Position Selector + drop_label = tk.Label(self.grasp_frame, text="Drop Position", font="Arial") + drop_label.grid(row=8, column=0) + self.drop_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10) + self.drop_selector.grid(row=8, column=1) + self.drop_selector.bind('<>', self.drop_selector_event) + + # Separator + separator = ttk.Separator(self.grasp_frame, orient='horizontal') + separator.grid(row=12, column=0, columnspan=2, sticky='ew') + + # Set Button + set_button = tk.Button(self.grasp_frame, text="Set New Grasp", command=self.grasp_set_event) + set_button.grid(row=13, column=0, columnspan=2) + + def create_db_edit_column(self): + self.db_frame = tk.Frame(self.root) + self.db_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + + submenu_label = tk.Label(self.db_frame, text="Edit Database", font=("Arial", 12, "bold")) + submenu_label.grid(row=0, column=0, columnspan=2) + + # Separator + separator = ttk.Separator(self.db_frame, orient='horizontal') + separator.grid(row=1, column=0, columnspan=2, sticky='ew') + + # Position Database Selector + position_db_label = tk.Label(self.db_frame, text="Positions", font="Arial") + position_db_label.grid(row=2, column=0) + self.position_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8) + self.position_db_selector.grid(row=2, column=1) + self.position_db_selector.bind('<>', self.position_db_selector_event) + + # Position Delete Button + position_delete_button = tk.Button(self.db_frame, text="Delete Position", command=self.position_delete_event) + position_delete_button.grid(row=3, column=0, columnspan=2) + + # Separator + separator = ttk.Separator(self.db_frame, orient='horizontal') + separator.grid(row=4, column=0, columnspan=2, sticky='ew') + + # Grasp Database Selector + grasp_db_label = tk.Label(self.db_frame, text="Grasp", font="Arial") + grasp_db_label.grid(row=5, column=0) + self.grasp_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8) + self.grasp_db_selector.grid(row=5, column=1) + self.grasp_db_selector.bind('<>', self.grasp_db_selector_event) + + # Grasp Delete Button + grasp_delete_button = tk.Button(self.db_frame, text="Delete Grasp", command=self.grasp_delete_event) + grasp_delete_button.grid(row=6, column=0, columnspan=2) + # + + +def main(args=None): + rclpy.init(args=args) + gui_node = GUINode() + + thread_spin = threading.Thread(target=rclpy.spin, args=(gui_node,)) + thread_spin.start() + + root = tk.Tk() + + # app = MyGUI(root) + app = MyGUI(root, gui_node) + while True: + app.update_from_database() + app.update_position_values() + root.update() + + gui_node.destroy_node() + rclpy.shutdown() + app.conn.close() + + +if __name__ == '__main__': + main() diff --git a/franka_logistics/position_run_gui.py b/franka_logistics/position_run_gui.py new file mode 100644 index 0000000..094507f --- /dev/null +++ b/franka_logistics/position_run_gui.py @@ -0,0 +1,439 @@ +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 sensor_msgs.msg import JointState +from franka_cps_msgs.action import GraspSequence + + +class GUINode(Node): + def __init__(self): + super().__init__('position_run_gui_node') + self.execution_client = ActionClient(self, GraspSequence, 'franka_grasp/grasp_sequence') + + self._send_goal_future = None + self._get_result_future = None + + def send_grasp_sequence_request(self, idle, hower, grasp, drop): + goal_msg = GraspSequence.Goal() + #self.get_logger().info(f"New Grasp Sequence Request {idle, hower, grasp, drop}") + goal_msg.idle = idle + goal_msg.hower = hower + goal_msg.grasp = grasp + goal_msg.drop = drop + + self.execution_client.wait_for_server() + self._send_goal_future = self.execution_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def feedback_callback(self, feedback_msg): + feedback = feedback_msg.feedback + self.get_logger().info(f'Feedback: {feedback.current_step}') + + def goal_response_callback(self, future): + goal_handle = future.result() + if not goal_handle.accepted: + self.get_logger().info('Grasp Sequence Goal rejected!') + return + + self.get_logger().info('Grasp Sequence Goal accepted!') + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.get_result_callback) + + def get_result_callback(self, future): + result = future.result().result + success = result.success + message = result.error_code + self.get_logger().info(f'Grasp Sequence result - Success: {success}, ErrorCode: {message}') + + +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.position_db = '' + self.grasp_db = '' + + # Grasp Run Submenu attributes + self.grasp_run = '' + self.grasp_entries = {} + self.grasp_values = { + 'idle_position': 'idle', + 'hower_position': 'hower', + 'grasp_position': 'grasp', + 'drop_position': '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.position_db_selector = None + self.grasp_db_selector = None + + self.grasp_run_selector = None + + # Frames + self.grasp_frame = None + self.db_frame = None + self.run_frame = None + self.separator_1 = None + self.separator_2 = None + + # Initially set up Learn mode + self.setup_run_mode() + + # + def setup_run_mode(self): + self.create_run_column() + self.separator_1 = tk.Canvas(self.root, width=1, bg="grey") + self.separator_1.pack(side=tk.LEFT, fill=tk.Y) + self.create_grasp_column() + self.separator_2 = tk.Canvas(self.root, width=1, bg="grey") + self.separator_2.pack(side=tk.LEFT, fill=tk.Y) + self.create_db_edit_column() + + # + + # + def get_position_names(self): + self.cursor.execute("SELECT position_name FROM position_table") + return [row[0] for row in self.cursor.fetchall()] + + def get_grasp_names(self): + self.cursor.execute("SELECT sequence_name FROM position_sequence_table") + return [row[0] for row in self.cursor.fetchall()] + + def get_position_by_name(self, position_name): + self.node.get_logger().info(f"Position:{position_name}") + self.cursor.execute( + "SELECT joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7 FROM position_table WHERE position_name = ?", + (position_name,)) + result = self.cursor.fetchone() + if result: + position = JointState() + self.node.get_logger().info(f"{result}") + position.position = result + return position + else: + self.node.get_logger().warn("Position not found") + return None + + def get_grasp_sequence_by_name(self, sequence_name): + self.cursor.execute( + "SELECT position_idle, position_hower, position_grasp, position_drop" + " FROM position_sequence_table WHERE sequence_name = ?", + (sequence_name,)) + result = self.cursor.fetchone() + if result: + self.grasp_values['idle_position'] = result[0] + self.grasp_values['hower_position'] = result[1] + self.grasp_values['grasp_position'] = result[2] + self.grasp_values['drop_position'] = 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 position_sequence_table" + """ (sequence_name, position_idle, position_hower, position_grasp, position_drop) + VALUES (?, ?, ?, ?, ?)""", + (sequence_name, idle, hower, grasp, drop)) + + self.conn.commit() + + def delete_position_by_name(self, position_name): + self.cursor.execute("DELETE FROM position_table WHERE position_name = ?", (position_name,)) + + self.conn.commit() + + def delete_grasp_by_name(self, sequence_name): + self.cursor.execute("DELETE FROM position_sequence_table WHERE sequence_name = ?", (sequence_name,)) + + self.conn.commit() + + # + + # + def update_grasp_values(self): + for key, value in self.grasp_values.items(): + if key in self.grasp_entries: + self.grasp_entries[key].config(state='normal') + self.grasp_entries[key].delete(0, tk.END) + self.grasp_entries[key].insert(0, f"{value}") + self.grasp_entries[key].config(state='readonly') + def update_position_selectors(self, db_positions): + self.idle_selector['values'] = db_positions + self.hower_selector['values'] = db_positions + self.grasp_selector['values'] = db_positions + self.drop_selector['values'] = db_positions + + def update_db_selectors(self, db_positions, db_grasp): + self.position_db_selector['values'] = db_positions + self.grasp_db_selector['values'] = db_grasp + + def update_from_database(self): + db_positions = self.get_position_names() + db_grasps = self.get_grasp_names() + self.update_position_selectors(db_positions) + self.update_db_selectors(db_positions, db_grasps) + self.grasp_run_selector['values'] = db_grasps + + # + + # + # Position 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 position_db_selector_event(self, event): + self.position_db = self.position_db_selector.get() + + def grasp_db_selector_event(self, event): + self.grasp_db = self.grasp_db_selector.get() + + def position_delete_event(self): + self.delete_position_by_name(self.position_db) + print("Position: ", self.position_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 = self.get_position_by_name(self.grasp_values['idle_position'] ) + hower = self.get_position_by_name(self.grasp_values['hower_position']) + grasp = self.get_position_by_name(self.grasp_values['grasp_position']) + drop = self.get_position_by_name(self.grasp_values['drop_position']) + + feedback = self.node.send_grasp_sequence_request(idle, hower, grasp, drop) + + print("Grasp: ", self.grasp_run, ' is performed!') + + # + + # + 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 Position + current_position_label = tk.Label(self.grasp_frame, text="Grasp Positions", font=("Arial", 10, "bold")) + current_position_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 Position Selector + idle_label = tk.Label(self.grasp_frame, text="Idle Position", 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 Position Selector + hower_label = tk.Label(self.grasp_frame, text="Hower Position", 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 Position Selector + grasp_label = tk.Label(self.grasp_frame, text="Grasp Position", 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 Position Selector + drop_label = tk.Label(self.grasp_frame, text="Drop Position", font="Arial") + drop_label.grid(row=8, column=0) + self.drop_selector = ttk.Combobox(self.grasp_frame, values=[""], width=10) + self.drop_selector.grid(row=8, column=1) + self.drop_selector.bind('<>', self.drop_selector_event) + + # Separator + separator = ttk.Separator(self.grasp_frame, orient='horizontal') + separator.grid(row=12, column=0, columnspan=2, sticky='ew') + + # Set Button + set_button = tk.Button(self.grasp_frame, text="Set New Grasp", command=self.grasp_set_event) + set_button.grid(row=13, column=0, columnspan=2) + + def create_db_edit_column(self): + self.db_frame = tk.Frame(self.root) + self.db_frame.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + + submenu_label = tk.Label(self.db_frame, text="Edit Database", font=("Arial", 12, "bold")) + submenu_label.grid(row=0, column=0, columnspan=2) + + # Separator + separator = ttk.Separator(self.db_frame, orient='horizontal') + separator.grid(row=1, column=0, columnspan=2, sticky='ew') + + # Position Database Selector + position_db_label = tk.Label(self.db_frame, text="Positions", font="Arial") + position_db_label.grid(row=2, column=0) + self.position_db_selector = ttk.Combobox(self.db_frame, values=[""], width=8) + self.position_db_selector.grid(row=2, column=1) + self.position_db_selector.bind('<>', self.position_db_selector_event) + + # Position Delete Button + position_delete_button = tk.Button(self.db_frame, text="Delete Position", command=self.position_delete_event) + position_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') + + # Position 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 Position', 'Hower Position', 'Grasp Position', 'Drop Position'] + + 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 Perform Button + grasp_perform_button = tk.Button(self.run_frame, text="Perform Grasp", command=self.grasp_perform_event) + grasp_perform_button.grid(row=11, column=0, columnspan=2) + + # + + +def main(args=None): + rclpy.init(args=args) + gui_node = GUINode() + + thread_spin = threading.Thread(target=rclpy.spin, args=(gui_node,)) + thread_spin.start() + + root = tk.Tk() + + # app = MyGUI(root) + app = MyGUI(root, gui_node) + while True: + app.update_from_database() + app.update_grasp_values() + root.update() + + gui_node.destroy_node() + rclpy.shutdown() + app.conn.close() + + +if __name__ == '__main__': + main() diff --git a/launch/position_learn.launch.py b/launch/position_learn.launch.py new file mode 100644 index 0000000..9039d8b --- /dev/null +++ b/launch/position_learn.launch.py @@ -0,0 +1,146 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + Shutdown) +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + + # planning_context + franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', + 'panda_arm.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true', + ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, + ' fake_sensor_commands:=', fake_sensor_commands]) + + robot_description = {'robot_description': robot_description_config} + + # Publish TF + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description], + ) + + ros2_controllers_path = os.path.join( + get_package_share_directory('franka_moveit_config'), + 'config', + 'panda_ros_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, ros2_controllers_path], + remappings=[('joint_states', 'franka/joint_states')], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + on_exit=Shutdown(), + ) + + load_controllers = [] + for controller in ['joint_state_broadcaster']: + load_controllers += [ + ExecuteProcess( + cmd=['ros2 run controller_manager spawner {}'.format(controller)], + shell=True, + output='screen', + ) + ] + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[ + {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}], + ) + + franka_robot_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + arguments=['franka_robot_state_broadcaster'], + output='screen', + condition=UnlessCondition(use_fake_hardware), + ) + + franka_grasp_demo = Node( + package='franka_logistics', + executable='franka_grasp_demo', + name='franka_grasp_demo', + output='screen', + ) + + franka_gui = Node( + package='franka_logistics', + executable='franka_position_learn_gui', + name='franka_position_learn_gui', + output='screen', + ) + + robot_arg = DeclareLaunchArgument( + robot_ip_parameter_name, + default_value='192.168.1.211', + description='Hostname or IP address of the robot.') + + use_fake_hardware_arg = DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware') + fake_sensor_commands_arg = DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)) + + gripper_launch_file = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]), + launch_arguments={'robot_ip': robot_ip, + use_fake_hardware_parameter_name: use_fake_hardware}.items(), + ) + + return LaunchDescription( + [ + robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + robot_state_publisher, + joint_state_publisher, + franka_gui, + franka_grasp_demo, + ros2_control_node, + franka_robot_state_broadcaster, + gripper_launch_file + ] + load_controllers + ) \ No newline at end of file diff --git a/launch/position_run.launch.py b/launch/position_run.launch.py new file mode 100644 index 0000000..a7b563b --- /dev/null +++ b/launch/position_run.launch.py @@ -0,0 +1,146 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, + Shutdown) +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + + # planning_context + franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots', + 'panda_arm.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true', + ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, + ' fake_sensor_commands:=', fake_sensor_commands]) + + robot_description = {'robot_description': robot_description_config} + + # Publish TF + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description], + ) + + ros2_controllers_path = os.path.join( + get_package_share_directory('franka_moveit_config'), + 'config', + 'panda_ros_controllers.yaml', + ) + ros2_control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[robot_description, ros2_controllers_path], + remappings=[('joint_states', 'franka/joint_states')], + output={ + 'stdout': 'screen', + 'stderr': 'screen', + }, + on_exit=Shutdown(), + ) + + load_controllers = [] + for controller in ['panda_arm_controller', 'joint_state_broadcaster']: + load_controllers += [ + ExecuteProcess( + cmd=['ros2 run controller_manager spawner {}'.format(controller)], + shell=True, + output='screen', + ) + ] + + joint_state_publisher = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[ + {'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}], + ) + + franka_robot_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + arguments=['franka_robot_state_broadcaster'], + output='screen', + condition=UnlessCondition(use_fake_hardware), + ) + + franka_grasp_demo = Node( + package='franka_logistics', + executable='franka_grasp_demo', + name='franka_grasp_demo', + output='screen', + ) + + franka_gui = Node( + package='franka_logistics', + executable='franka_position_run_gui', + name='franka_position_run_gui', + output='screen', + ) + + robot_arg = DeclareLaunchArgument( + robot_ip_parameter_name, + default_value='192.168.1.211', + description='Hostname or IP address of the robot.') + + use_fake_hardware_arg = DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware') + fake_sensor_commands_arg = DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)) + + gripper_launch_file = IncludeLaunchDescription( + PythonLaunchDescriptionSource([PathJoinSubstitution( + [FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]), + launch_arguments={'robot_ip': robot_ip, + use_fake_hardware_parameter_name: use_fake_hardware}.items(), + ) + + return LaunchDescription( + [ + robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + robot_state_publisher, + joint_state_publisher, + franka_gui, + franka_grasp_demo, + ros2_control_node, + franka_robot_state_broadcaster, + gripper_launch_file + ] + load_controllers + ) \ No newline at end of file diff --git a/package.xml b/package.xml index 5288a37..d106380 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,10 @@ geometry_msgs moveit_msgs std_msgs + sensor_msgs franka_cps_msgs + control_msgs + trajectory_msgs ament_copyright ament_flake8 diff --git a/setup.py b/setup.py index 7f2742d..4cc5140 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ setup( ['resource/' + package_name]), (os.path.join('share', package_name), glob('launch/*.launch.py')), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, @@ -27,7 +28,10 @@ setup( 'pose_learn = franka_logistics.pose_learn_node: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' + 'franka_run_gui = franka_logistics.Run_GUI:main', + 'franka_position_learn_gui = franka_logistics.position_learn_gui:main', + 'franka_position_run_gui = franka_logistics.position_run_gui:main', + 'franka_grasp_demo = franka_logistics.grasp_demo_node:main' ], }, )