task not working

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()

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()
+
+    # <editor-fold desc="Menu Methods">
+
+    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()
+
+    # </editor-fold>
+
+    # <editor-fold desc="Initialize Methods">
+    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
+
+    # </editor-fold>
+
+    # <editor-fold desc="SQLite Methods">
+    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()
+
+    # </editor-fold>
+
+    # <editor-fold desc="Update Methods">
+    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)
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Events">
+    # 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!')
+
+    # </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_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('<<ComboboxSelected>>', 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('<<ComboboxSelected>>', 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('<<ComboboxSelected>>', 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('<<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')
+
+        # 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('<<ComboboxSelected>>', 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('<<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_position_values()
+        root.update()
+
+    gui_node.destroy_node()
+    rclpy.shutdown()
+    app.conn.close()
+
+
+if __name__ == '__main__':
+    main()

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()
+
+    # <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_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()
+
+    # </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_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
+
+    # </editor-fold>
+
+    # <editor-fold desc="SubMenu Events">
+    # 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!')
+
+    # </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 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('<<ComboboxSelected>>', 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('<<ComboboxSelected>>', 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('<<ComboboxSelected>>', 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('<<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')
+
+        # 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('<<ComboboxSelected>>', 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('<<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')
+
+        # 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('<<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 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)
+
+    # </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()

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
+    )

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
+    )

package.xml

@@ -11,7 +11,10 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>franka_cps_msgs</depend>
+  <depend>control_msgs</depend>
+  <depend>trajectory_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

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