diff --git a/workspace/.gitignore b/workspace/.gitignore
new file mode 100644
index 0000000..f93cda8
--- /dev/null
+++ b/workspace/.gitignore
@@ -0,0 +1,6 @@
+build/*
+build/**
+install/*
+install/**
+log/*
+log/**
diff --git a/workspace/build/.built_by b/workspace/build/.built_by
deleted file mode 100644
index 06e74ac..0000000
--- a/workspace/build/.built_by
+++ /dev/null
@@ -1 +0,0 @@
-colcon
diff --git a/workspace/build/COLCON_IGNORE b/workspace/build/COLCON_IGNORE
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py b/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py b/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py
deleted file mode 100644
index 0b28d5b..0000000
--- a/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py
+++ /dev/null
@@ -1,1099 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from sensor_msgs.msg import JointState
-from rcl_interfaces.msg import Log
-from osc4py3.as_allthreads import *
-from osc4py3 import oscmethod as osm
-import xml.etree.ElementTree as ET
-import numpy as np
-import spatialmath as sm
-import roboticstoolbox as rtb
-from osc4py3 import oscbuildparse
-import time
-import os
-import re
-import socket
-
-class JointNameListener(Node):
-    def __init__(self):
-        super().__init__('joint_name_listener')
-        self.subscription = self.create_subscription(
-            JointState,
-            '/joint_states',
-            self.joint_state_callback,
-            1
-        )
-        self.joint_names = None
-
-    def joint_state_callback(self, msg: JointState):
-        self.joint_names = list(msg.name)
-
-class OSC_ROS2_interface(Node):
-    """Node to publish joint trajectories based on OSC messages."""
-    
-    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
-        super().__init__('scaled_joint_trajectory_publisher')
-        
-
-        self.subscription = self.create_subscription(
-            JointState,
-            '/joint_states',
-            self.joint_states_callback,
-            1
-        )
-
-        self.subscription = self.create_subscription(
-            Log,
-            '/rosout',
-            self.log_callback,
-            100
-        )
-
-        # Store received joint positions
-        self.current_joint_positions = None
-        self.joint_names = joint_names
-        self.joint_velocity_limits = joint_velocity_limits
-        self.cost_mask = cost_mask
-        self.robot = robot
-        self.desired = None
-        self.previous_desired = None
-        self.log_dict = {
-            10: "DEBUG",
-            20: "INFO",
-            30: "WARN",
-            40: "ERROR",
-            50: "FATAL",
-        }
-        self.speed_scaling = 0.2
-        self.new = False
-        self.n_joints = len(joint_names)   
-
-        if robot:
-            while True:
-                print('+-' * 50)
-                set_limits = input("Do you want to set limits for x, y and z? (y/n): ").strip().lower()
-                if set_limits == 'y':
-                    while True:
-                                try:
-                                    self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
-                                    self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
-                                    self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
-                                    
-                                    if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
-                                        print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
-                                        continue
-                                    
-                                    if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
-                                    (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
-                                    (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
-                                        print("Invalid input. Lower limit must be less than upper limit for each axis.")
-                                        continue
-                                    
-                                    print(f"Current limits:")
-                                    print(f"x: {self.x_limits}")
-                                    print(f"y: {self.y_limits}")
-                                    print(f"z: {self.z_limits}")
-                                    con = True
-                                    while con:
-                                        confirm = input("Do you want your TCP to move in this range? (y/n): ").strip().lower()
-                                        if confirm == 'y':
-                                            break
-                                        elif confirm == 'n':
-                                            print("Please re-enter the limits.")
-                                            con = False
-                                        else:
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    if con: break
-                                except ValueError:
-                                    print("Invalid input. Please enter numeric values only.")
-                    break
-                elif set_limits == 'n': 
-                    self.x_limits = [None, None]
-                    self.y_limits = [None, None]
-                    self.z_limits = [None, None]
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-
-            while True:
-                print('+-' * 50)
-                set_limits = input("Do you want to set workspace limits in x, y and z direction? (y/n): ").strip().lower()
-                if set_limits == 'y':
-                    while True:
-                                try:
-                                    self.x_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
-                                    self.y_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
-                                    self.z_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
-                                    
-                                    if len(self.x_limits_workspace) != 2 or len(self.y_limits_workspace) != 2 or len(self.z_limits_workspace) != 2:
-                                        print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
-                                        continue
-                                    
-                                    if (self.x_limits_workspace[0] is not None and self.x_limits_workspace[1] is not None and self.x_limits_workspace[0] >= self.x_limits_workspace[1]) or \
-                                    (self.y_limits_workspace[0] is not None and self.y_limits_workspace[1] is not None and self.y_limits_workspace[0] >= self.y_limits_workspace[1]) or \
-                                    (self.z_limits_workspace[0] is not None and self.z_limits_workspace[1] is not None and self.z_limits_workspace[0] >= self.z_limits_workspace[1]):
-                                        print("Invalid input. Lower limit must be less than upper limit for each axis.")
-                                        continue
-                                    
-                                    print(f"Current limits:")
-                                    print(f"x: {self.x_limits_workspace}")
-                                    print(f"y: {self.y_limits_workspace}")
-                                    print(f"z: {self.z_limits_workspace}")
-                                    con = True
-                                    while con:
-                                        confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
-                                        if confirm == 'y':
-                                            break
-                                        elif confirm == 'n':
-                                            print("Please re-enter the limits.")
-                                            con = False
-                                        else:
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    if con: break
-                                except ValueError:
-                                    print("Invalid input. Please enter numeric values only.")
-                    break
-                elif set_limits == 'n': 
-                    self.x_limits_workspace = [None, None]
-                    self.y_limits_workspace = [None, None]
-                    self.z_limits_workspace = [None, None]
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-                        # Ask the user if they want to set new joint limits
-
-                        # Ask the user if they want to set new joint limits
-            while True:
-                print('+-'*50)
-                update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
-                if update_limits == 'y':
-                    for i in range(len(self.joint_names)):
-                        while True:
-                            try:
-                                lim = self.robot.qlim.copy()
-                                # Find the link corresponding to the joint name
-                                print("-" * 50)
-                                print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
-                                lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
-                                upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
-                                
-                                if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
-                                    print("Invalid input. Lower limit must be less than upper limit.")
-                                    continue
-                                lower_limit = float(lower_limit) if lower_limit!="" else None
-                                upper_limit = float(upper_limit) if upper_limit!="" else None
-                                if lower_limit!=None:
-                                    if lower_limit<lim[0][i]:
-                                        while True:
-                                            sure = input(f"Are you sure you want to set the lower limit to {lower_limit} rad which is less than the default limit {lim[0][i]} (y/n)?: ").strip().lower()
-                                            if sure == 'y':
-                                                lim[0][i] = float(lower_limit)
-                                                break
-                                            elif sure == 'n':
-                                                print("Lower limit not changed.")
-                                                break
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    else: lim[0][i] = float(lower_limit)
-                                if upper_limit!=None:
-                                    if upper_limit>lim[1][i]:
-                                        while True:
-                                            sure = input(f"Are you sure you want to set the upper limit to {upper_limit} rad which is more than the default limit {lim[1][i]} (y/n)?: ").strip().lower()
-                                            if sure == 'y':
-                                                lim[1][i] = float(upper_limit)
-                                                break
-                                            elif sure == 'n':
-                                                print("Upper limit not changed.")
-                                                break
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    else: lim[1][i] = float(upper_limit)
-                                self.robot.qlim = lim
-                                print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
-                                print("-" * 50)
-                                break
-                            except ValueError:
-                                print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
-                    break
-                if update_limits == 'n':
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-            '''
-            use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
-            if use_link_mask == 'y':
-                while True:
-                    try:
-    '''
-        else:
-            while True:
-                print('+-' * 50)
-                update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower()
-                if update_limits == 'y':
-                    self.joint_lim = [[None]*self.n_joints,[None]*self.n_joints]
-                    for i, joint in enumerate(self.joint_names):
-                        while True:
-                            try:
-                                print("-" * 50)
-                                lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter for None): ").strip()
-                                upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter for None): ").strip()
-                                
-                                if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
-                                    print('--' * 50)
-                                    print("      !!!      Invalid input. Lower limit must be less than upper limit.      !!!      ")
-                                    continue
-                                self.joint_lim[0][i] = float(lower_limit) if lower_limit!="" else None
-                                self.joint_lim[1][i] = float(upper_limit) if upper_limit!="" else None
-                                break
-                            except ValueError:
-                                print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
-                    print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
-                    break
-                elif update_limits == 'n':
-                    self.joint_lim = None
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-                
-
-
-        while True:
-            try:
-                print('+-' * 50)
-                self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip()
-                if self.trajectory_topic_name == "":
-                    self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
-                    break
-                elif self.trajectory_topic_name.startswith("/"):
-                    break
-                else:
-                    print("Invalid topic name. A valid topic name should start with '/'.")
-            except Exception as e:
-                print(f"An error occurred: {e}")
-
-        # ROS2 Publisher
-        self.publisher = self.create_publisher(
-            JointTrajectory, 
-            self.trajectory_topic_name, 
-            1
-        )
-
-        while True:
-            try:
-                print('+-' * 50)
-                log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): "))
-                if log_ip == "":
-                    log_ip = "127.0.0.1"
-                print('--' * 50)
-                log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ")
-                if log_port == "":
-                    log_port = 5005
-                else:
-                    log_port = int(log_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid IP address.")
-                continue
-        while True:
-            try:
-                print('+-' * 50)
-                state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): "))
-                if state_ip == "":
-                    state_ip = "127.0.0.1"
-                print('--' * 50)
-                state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ")
-                if state_port == "":
-                    state_port = 7000
-                else:
-                    state_port = int(state_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid IP address.")
-                continue
-        while True:
-            try:
-                print('+-' * 50)
-                commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ")
-                if commands_port == state_port:
-                    print("The commands port must be different from the state port.")
-                    continue
-                if commands_port == "":
-                    commands_port = 8000
-                else:
-                    commands_port = int(commands_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid port.")
-                continue
-                                
-
-        osc_startup()
-
-        osc_udp_client(state_ip, state_port, "osc_client")
-
-        osc_udp_client(log_ip, log_port, "osc_log_client")
-
-        osc_udp_server('0.0.0.0', commands_port, "osc_server")
-
-        # Register OSC handler
-        osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_ADDRESS+osm.OSCARG_DATAUNPACK)
-        osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        print('--' * 50)
-        while True:
-            try:
-                print('+-' * 50)
-                self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ")
-                if self.hz == "":
-                    self.hz = 100
-                else:
-                    self.hz = float(self.hz)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid number.")
-                continue
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Sending joint states to {state_ip}:{state_port}')
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Sending log messages to {log_ip}:{log_port}')
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
-        print()
-        print('=-=' * 50)
-        print()
-
-        self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}")
-        self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
-        self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
-        self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
-        self.create_timer(1/self.hz, self.update_position)  # Timer to update the position
-        self.create_timer(3, self.reset_prev)  # reset the previous desired position
-
-    def reset_prev(self): self.previous_desired = None
-
-    def speed_scaling_handler(self, *args):
-        """Handles incoming OSC messages for speed scaling."""
-        try:
-            if len(args) == 1:
-                if args[0] < 0:
-                    self.speed_scaling = -float(args[0])
-                else:
-                    self.speed_scaling = float(args[0])
-                if self.speed_scaling > 1:
-                    self.get_logger().warn(f"speed_scaling_handler: Attention! Speed scaling {self.speed_scaling} is greater than 1!")
-                self.get_logger().info(f"Speed scaling set to {self.speed_scaling}")
-            else:
-                self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.")
-        except Exception as e:
-            self.get_logger().fatal(f"speed_scaling_handler: {e}")
-    
-    def joint_trajectory_handler(self, *args):
-        try:
-                if len(args[0]) == 6:
-                    points = [[float(j) for j in i] for i in args]
-                elif len(args[0]) >= 7:
-                    points = [[float(j) for j in i[:6]] for i in args]
-                    self.get_logger().warn(f"joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
-                else:
-                    self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.")
-                    return
-
-                self.desired = ["joint_trajectory"] + points
-                self.new = True
-        except Exception as e:
-            self.get_logger().fatal(f"joint_trajectory_handler: {e}")
-
-    def joint_position_handler(self, address, *args):
-        """Handles incoming OSC messages for joint positions."""
-        try:
-            joint_name = address.split("/")[-1]
-            
-            if joint_name in self.joint_names:
-                if len(args) == 1:
-                    position = float(args[0])
-                    index = self.joint_names.index(joint_name)
-                    if self.robot:
-                        if position < self.robot.qlim[0][index]:
-                            position = self.robot.qlim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
-                            )
-                        elif position > self.robot.qlim[1][index]:
-                            position = self.robot.qlim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
-                            )
-                    else:
-                        if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
-                            position = self.joint_lim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
-                            )
-                        elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
-                            position = self.joint_lim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
-                            )
-                    desired_joint_positions = self.current_joint_positions
-                    desired_joint_positions[index] = position
-                    self.desired = ["joint_positions"] + desired_joint_positions
-                    self.new = True
-                elif len(args) == 2:
-                    position = float(args[0])
-                    duration = float(args[1])
-                    index = self.joint_names.index(joint_name)
-                    if self.robot:
-                        if position < self.robot.qlim[0][index]:
-                            position = self.robot.qlim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
-                            )
-                        elif position > self.robot.qlim[1][index]:
-                            position = self.robot.qlim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
-                            )
-                    else:
-                        if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
-                            position = self.joint_lim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
-                            )
-                        elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
-                            position = self.joint_lim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
-                            )
-                    desired_joint_positions = self.current_joint_positions
-                    desired_joint_positions[index] = position
-                    self.desired = ["joint_positions"] + desired_joint_positions + [duration]
-                    self.new = True
-                else:
-                    self.get_logger().warn(f"joint_position_handler: Invalid number of arguments for joint position. Expected 1, but got {len(args)}.")
-            else:
-                self.get_logger().warn(f"joint_position_handler: Joint '{joint_name}' not found in the robot model.")
-        except Exception as e:
-            self.get_logger().fatal(f"joint_position_handler: {e}")
-        
-
-    def cartesian_trajectory_handler(self, *args):
-        """Handles incoming OSC messages for cartesian trajectory."""
-        if self.robot:
-            try:
-                if len(args[0]) == 6:
-                    points = [[float(j) for j in i] for i in args]
-                elif len(args[0]) >= 7:
-                    points = [[float(j) for j in i[:6]] for i in args]
-                    self.get_logger().warn(f"cartesian_trajectory_handler: Duration is not supported for cartesian trajectory yet. Ignoring duration.")
-                else:
-                    self.get_logger().warn(f"cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.")
-                    return
-
-                self.desired = ["cartesian_trajectory"] + points
-                self.new = True
-            except Exception as e:
-                self.get_logger().fatal(f"cartesian_trajectory_handler: {e}")
-        else:
-            self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.")
-            return
-
-    def joint_positions_handler(self, *args):
-        """Handles incoming OSC messages for joint positions."""
-        try:
-            if len(args) == len(self.joint_names):
-                desired_joint_positions = [float(i) for i in list(args)]
-            elif len(args) == len(self.joint_names) + 1:
-                desired_joint_positions = [float(i) for i in list(args)]
-            else:
-                self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.")
-                return
-
-            # Check if joint positions exceed limits
-            if self.robot:
-                for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):  # Exclude duration if present
-                    if position < self.robot.qlim[0][i]:
-                        desired_joint_positions[i] = self.robot.qlim[0][i]
-                        self.get_logger().warn(
-                            f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
-                        )
-                    elif position > self.robot.qlim[1][i]:
-                        desired_joint_positions[i] = self.robot.qlim[1][i]
-                        self.get_logger().warn(
-                            f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
-                        )
-            else:
-                if self.joint_lim:
-                    for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):
-                        if self.joint_lim[0][i] is not None:
-                            if position < self.joint_lim[0][i]:
-                                desired_joint_positions[i] = self.joint_lim[0][i]
-                                self.get_logger().warn(
-                                    f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}."
-                                )
-                        elif self.joint_lim[1][i] is not None:
-                            if position > self.joint_lim[1][i]:
-                                desired_joint_positions[i] = self.joint_lim[1][i]
-                                self.get_logger().warn(
-                                    f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}."
-                                )
-
-            self.desired = ["joint_positions"] + desired_joint_positions
-            self.new = True
-        except Exception as e:
-            self.get_logger().fatal(f"joint_positions_handler: {e}")
-            return
-
-    def tcp_coordinates_handler(self, *args):
-    # Ensure the desired joint positions are within the specified limits
-        if self.robot:
-            try:
-                if len(args) == 6:
-                    x, y, z, r, p, yaw = [float(i) for i in list(args)]
-                    duration = None
-                elif len(args) >= 7:
-                    x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)]
-                else:
-                    self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.")
-                    return
-
-                if self.x_limits[0] is not None:
-                    x = max(self.x_limits[0], x)
-                if self.x_limits[1] is not None:
-                    x = min(self.x_limits[1], x)
-                if self.y_limits[0] is not None:
-                    y = max(self.y_limits[0], y)
-                if self.y_limits[1] is not None:
-                    y = min(self.y_limits[1], y)
-                if self.z_limits[0] is not None:
-                    z = max(self.z_limits[0], z)
-                if self.z_limits[1] is not None:
-                    z = min(self.z_limits[1], z)
-
-                if x != args[0] or y != args[1] or z != args[2]:
-                    self.get_logger().warn(
-                    f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: "
-                    f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
-                    )
-
-                self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration]
-                self.new = True
-            except Exception as e:
-                self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
-        else:
-            self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.")
-            return
-            
-
-    def joint_states_callback(self, msg: JointState):
-        """Callback function to handle incoming joint states."""
-        try:
-            msg_time = oscbuildparse.OSCMessage(f"/time", ',s', [str(time.time())])
-            osc_send(msg_time, "osc_client")
-            if not(self.joint_names): self.joint_names = msg.name
-            joint_position_dict = dict(zip(msg.name, msg.position))
-            self.current_joint_positions = [float(joint_position_dict[name]) for name in self.joint_names]
-            joint_position_dict = dict(zip(msg.name, msg.velocity))
-            self.current_joint_velocities = [float(joint_position_dict[name]) for name in self.joint_names]
-
-            if self.robot:
-                tcp_position = self.robot.fkine(self.current_joint_positions).t
-                tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy()
-
-                msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]])
-                #msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]])
-                #msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]])
-                #msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]])
-                #msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]])
-                #msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]])
-                #msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]])
-                #bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw])
-                #osc_send(bun, "osc_client") 
-                osc_send(msg_tcp, "osc_client")
-            
-            msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
-            msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
-            msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
-            msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name])
-            bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort])
-            osc_send(bun, "osc_client")
-
-            #for i, name in enumerate(msg.name):
-            #    msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]])
-            #    msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]])
-            #    msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]])
-            #    bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort])
-            #    osc_send(bun, "osc_client")
-
-        except Exception as e:
-            self.get_logger().fatal(f"joint_states_callback: {e}")
-
-    def send_joint_positions(self):
-        self.previous_desired = None
-        self.new = False
-        try:
-            if len(self.desired) == len(self.joint_names) + 2:
-                desired_joint_positions = [float(i) for i in self.desired[1:-1]]
-                duration = self.desired[-1]
-                msg = JointTrajectory()
-                msg.joint_names = self.joint_names
-                point = JointTrajectoryPoint()
-                point.positions = desired_joint_positions
-                point.time_from_start.sec = int(duration)
-                point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                msg.points.append(point)
-                self.publisher.publish(msg)
-            elif len(self.desired) == len(self.joint_names) + 1:
-                desired_joint_positions = [float(i) for i in self.desired[1:]]
-                msg = JointTrajectory()
-                msg.joint_names = self.joint_names
-                point = JointTrajectoryPoint()
-                point.positions = desired_joint_positions
-                duration = 0
-                for p1, p2, max_vel in zip(desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()):
-                    duration = max(duration, abs(p1 - p2) / max_vel)
-                duration = duration + min(2, 0.2*duration)
-                duration /= self.speed_scaling
-                if duration == 0:
-                    self.get_logger().warn("send_joint_positions: Duration is 0.")
-                    return
-                point.time_from_start.sec = int(duration)
-                point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                msg.points.append(point)
-                self.publisher.publish(msg)
-            else: 
-                self.get_logger().warn(f"send_joint_positions: Invalid number of arguments for joint positions. Expected {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]) or {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]), but got {len(self.desired)}.")
-                return
-        except Exception as e:
-            self.get_logger().fatal(f"send_joint_positions: {e}")
-            return
-
-    def trapezoidal_timestamps(self, num_points,total_duration,  flat_ratio = 0.3):
-        
-        if num_points == 2:
-            return [0, total_duration]
-        n = int(num_points*(1-flat_ratio)/2)
-        start = np.cos(np.linspace(0, np.pi, n))+2
-        end = np.cos(np.linspace(-np.pi, 0, n))+2
-        flat = np.ones(num_points-2*n)
-
-        timestamps = np.concatenate((start, flat, end))
-        timestamps *= total_duration / timestamps.sum()
-        timestamps = np.cumsum(timestamps)
-
-        return timestamps.tolist()
-
-
-    def send_tcp_coordinates(self):
-        """Send the desired TCP coordinates to the robot."""
-        try:
-            self.new = False
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            steps_per_m = 100
-            if self.previous_desired == None: 
-                [x,y,z] = self.robot.fkine(self.current_joint_positions).t
-                [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
-            else:
-                [x,y,z] = self.previous_desired[1:4]
-                [roll, pitch, yaw] = self.previous_desired[4:-1]
-            x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
-            self.previous_desired = self.desired
-            steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
-            if steps < 2: steps = 2
-            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
-            '''if self.previous_desired:
-                [x,y,z] = self.previous_desired[1:4]
-                q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])        
-            else: 
-                [x, y, z] = self.robot.fkine(self.current_joint_positions).t
-                q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
-
-            x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
-            q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
-
-            steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
-            if steps < 2:
-                steps = 2
-            
-            cart_traj = []
-            for i in range(steps):
-                alpha = i / (steps - 1)
-
-                # Convert to arrays for robust interpolation if needed
-                q0_array = q0.vec
-                q1_array = q1.vec
-                dot = np.dot(q0_array, q1_array)
-                dot = np.clip(dot, -1.0, 1.0)
-
-                if abs(dot) > 0.9995:
-                    # Linear interpolation + normalization
-                    q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
-                    q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
-                    q_interp = sm.UnitQuaternion(q_interp_array)
-                else:
-                    q_interp = q0.interp(q1, alpha)
-
-                # Interpolate translation
-                pos_interp = [
-                    x + (x1 - x) * alpha,
-                    y + (y1 - y) * alpha,
-                    z + (z1 - z) * alpha
-                ]
-
-                # Compose SE3 transform
-                cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
-
-
-            
-            if self.desired[-1]:
-                timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
-                for j in range(steps):
-                    sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
-                    if sol[1] == 1:
-                        fowards = self.robot.fkine_all(sol[0])
-                        out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
-                        if np.any(out_of_bounds):
-                            #print(fowards.t)
-                            #indices = np.where(out_of_bounds)[0]
-                            #print(f"indices: {indices}")
-                            self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
-                            '''
-                            for i in indices:
-                                try:
-                                    print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
-                                except IndexError:
-                                    print(f"index {i} is out of bounds, but no corresponding joint found.")
-                            self.previous_desired_tcp_position = self.desired_tcp_position
-                            '''
-                            break
-                        duration = timestamps[j]
-                        if duration == 0:
-                            prev_sol = list(sol[0])
-                            continue
-                        point = JointTrajectoryPoint()
-                        point.positions = list(sol[0])
-                        point.time_from_start.sec = int(duration)
-                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                        msg.points.append(point)
-                        prev_sol = list(sol[0])
-                    else:
-                        self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
-                        prev_sol = self.current_joint_positions
-                if len(msg.points) == 0:
-                    self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
-                    self.previous_desired = self.desired    
-                    return
-                msg.header.stamp = self.get_clock().now().to_msg()
-                self.publisher.publish(msg)
-                self.previous_desired = self.desired
-
-
-
-            else:
-                prev_duration = 0
-                '''
-                if self.previous_desired == None: 
-                    [x,y,z] = self.robot.fkine(self.current_joint_positions).t
-                    [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
-                else:
-                    [x,y,z] = self.previous_desired[:3]
-                    [roll, pitch, yaw] = self.previous_desired[3:]
-                '''
-                for j in range(steps):
-                    sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
-                    if sol[1] == 1:
-                        fowards = self.robot.fkine_all(sol[0])
-                        out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
-                        if np.any(out_of_bounds):
-                            #print(fowards.t)
-                            #indices = np.where(out_of_bounds)[0]
-                            #print(f"indices: {indices}")
-                            self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
-                            '''
-                            for i in indices:
-                                try:
-                                    print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
-                                except IndexError:
-                                    print(f"index {i} is out of bounds, but no corresponding joint found.")
-                            self.previous_desired_tcp_position = self.desired_tcp_position
-                            '''
-                            break
-                        duration = 0
-                        prev = self.current_joint_positions if j == 0 else prev_sol
-                        for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
-                            duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
-                        prev_sol = list(sol[0])
-                        if duration == 0:
-                            continue
-                        point = JointTrajectoryPoint()
-                        point.positions = list(sol[0])
-                        duration *= 1.2
-                        duration /= self.speed_scaling
-                        duration += prev_duration
-                        prev_duration = duration
-                        point.time_from_start.sec = int(duration)
-                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                        msg.points.append(point)
-                    else:
-                        self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
-                        prev_sol = self.current_joint_positions
-                if len(msg.points) == 0:
-                    self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
-                    self.previous_desired = self.desired    
-                    return
-                msg.header.stamp = self.get_clock().now().to_msg()
-                self.publisher.publish(msg)
-                self.previous_desired = self.desired
-        except Exception as e:
-            self.get_logger().fatal(f"send_tcp_coordinates: {e}")
-
-    def send_joint_trajectory(self):
-        
-        try:
-            self.new = False
-            viapoints = np.array([i for i in self.desired[1:]])
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            traj = rtb.mstraj(viapoints, q0 = self.current_joint_positions ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=[self.speed_scaling*i for i in self.joint_velocity_limits.values()])
-            msg.points = []
-            for i in range(len(traj.q)):
-                    point = JointTrajectoryPoint()
-                    point.positions = list(traj.q[i])
-                    point.time_from_start.sec = int(traj.t[i])
-                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
-                    msg.points.append(point)
-            msg.header.stamp = self.get_clock().now().to_msg()
-            self.publisher.publish(msg)
-            self.previous_desired = None
-        except Exception as e:
-            print(f'Error in joint_angles_handler: {e}')
-
-    def send_cartesian_trajectory(self):
-        
-        try:
-            self.new = False
-            viapoints = np.array([i[:6] for i in self.desired[1:]])
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            x,y,z = self.robot.fkine(self.current_joint_positions).t
-            r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy()
-            q0 = [x, y, z, r, p, yaw]
-            traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=2*self.speed_scaling)
-            msg.points = []
-            prev_sol = self.current_joint_positions
-            n = max(int(len(traj.q)/200), 1)
-            for i in range(len(traj.q)):
-                T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz')
-                sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True)
-                if sol[1] == 1:
-                    point = JointTrajectoryPoint()
-                    point.positions = list(sol[0])
-                    point.time_from_start.sec = int(traj.t[i])
-                    if traj.t[i] == 0:
-                        continue
-                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
-                    msg.points.append(point)
-                    prev_sol = list(sol[0])
-                else: self.get_logger().warn(f"send_cartesian_trajectory: IK could not find a solution for (x,y,z) = {traj.q[i][:3]} and (r,p,y) = {traj.q[i][3:]}!")
-            msg.header.stamp = self.get_clock().now().to_msg()
-            msg.points = msg.points[::n]
-            self.publisher.publish(msg)
-            self.previous_desired = None
-        except Exception as e:
-            print(f'Error in joint_angles_handler: {e}')
-
-        self.previous_desired = None
-
-    def update_position(self):
-        """Calls the appropriate function to update the robot's position."""
-        try:
-            if self.desired is None or not(self.new):
-                return
-
-            if self.desired[0] == "joint_positions":
-                self.new = False
-                self.send_joint_positions()
-                return
-            elif self.desired[0] == "tcp_coordinates":
-                self.new = False
-                self.send_tcp_coordinates()
-                return
-            elif self.desired[0] == "joint_trajectory":
-                self.new = False
-                self.send_joint_trajectory()
-                return
-            elif self.desired[0] == "cartesian_trajectory":
-                self.new = False
-                self.send_cartesian_trajectory()
-                return
-            else:
-                self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
-                return
-
-
-        except Exception as e:
-            self.get_logger().fatal(f'update_position: {e}')
-                
-
-    def clean_log_string(self, s):
-
-        s = str(s)
-
-        # Remove ANSI escape sequences (e.g., \x1b[31m)
-        ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
-        s = ansi_escape.sub('', s)
-
-        # Replace tabs/newlines with spaces
-        s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
-
-        # Strip leading/trailing whitespace
-        s = s.strip()
-
-        # Optionally enforce ASCII only (replace non-ASCII chars with '?')
-        s = s.encode('ascii', 'replace').decode('ascii')
-
-        return s
-
-    
-    def log_callback(self, msg: Log):
-        """Callback function to handle incoming log messages."""
-
-        # Send the log message as an OSC message
-        msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
-        osc_send(msg_log, "osc_log_client")
-
-
-def main():
-    """Main function to get joint names and start the ROS 2 & OSC system."""
-    rclpy.init()
-    while True:
-        use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
-        if use_urdf == 'y':
-            while True:
-                robot_urdf = input("Enter the absolute path to the URDF file: ")
-                if os.path.isfile(robot_urdf):
-                    if not robot_urdf.endswith('.urdf'):
-                        print("The file is not a URDF file. Please enter a valid URDF file.")
-                        continue
-                    break
-                else:
-                    print("Invalid path. Please enter a valid path to the URDF file.")
-            tree = ET.parse(robot_urdf)
-            root = tree.getroot()
-            robot = rtb.ERobot.URDF(robot_urdf)
-            joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
-            print(robot)
-            joint_velocity_limits = {}
-
-            # Iterate over all joints in the URDF
-            for joint in root.findall('.//joint'):
-                joint_name = joint.get('name')  # Get the name of the joint
-                
-                # Look for the <limit> tag under each joint
-                limit = joint.find('limit')
-                
-                if limit is not None:
-                    # Extract the velocity limit (if it exists)
-                    velocity_limit = limit.get('velocity')
-                    
-                    if velocity_limit is not None:
-                        joint_velocity_limits[joint_name] = float(velocity_limit)
-            
-            while True:
-                try:
-                    print('-+'*50)
-                    print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
-                    print("The cost mask 111000 means that the IK will only consider translation and no rotaion.")
-                    cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
-                    if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
-                        break
-                    else:
-                        print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
-                except ValueError:
-                    print("Invalid input. Please enter integers only.")
-            print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}")
-            break
-        elif use_urdf == 'n':
-            node = JointNameListener()
-            print("Wainting 10 sec for JointState messages to extract joint names...")
-            rclpy.spin_once(node)
-            counter = 0
-            while not(node.joint_names):
-                if counter > 100:
-                    joint_names = None
-                    break
-                counter+=1
-                time.sleep(0.1)
-                rclpy.spin_once(node)
-            joint_names = node.joint_names
-            node.destroy_node()
-            if joint_names:
-                correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower()
-                while True:
-                    if correct_names == 'y':
-                        break
-                    elif correct_names == 'n':
-                        joint_names = None
-                        break
-                    correct_names = input("Invalid input. Please enter 'y' or 'n'.")
-            if not(joint_names):
-                print("Please enter the joint names manually.")
-                while True:
-                        joint_names = []
-                        print('-+'*50)
-                        print("Enter the joint names manually one by one. Type 'done' when you are finished:")
-                        print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.")
-                        while True:
-                            print('-'*50)
-                            joint_name = input("Enter joint name (or 'done' to finish): ").strip()
-                            if joint_name.lower() == 'done':
-                                break
-                            if joint_name:
-                                joint_names.append(joint_name)
-                        print('-+'*50)
-                        correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
-                        if correct_names.lower() == 'y':
-                            break
-                        else:
-                            print("Please re-enter the joint names.")
-            while True:
-                try:
-                    joint_velocity_limits = {}
-                    for name in joint_names:
-                        while True:
-                            try:
-                                print('--'*50)
-                                limit = input(f"Enter the velocity limit for joint '{name}': ").strip()
-                                if limit == "":
-                                    continue
-                                else:
-                                    joint_velocity_limits[name] = float(limit)
-                                    break
-                            except ValueError:
-                                print("Invalid input. Please enter a numeric value or press Enter to skip.")
-                    break
-                except ValueError:
-                    print("Invalid input. Please enter numeric values or leave blank to skip.")
-            robot = None
-            cost_mask = None
-            break
-        print("Invalid input. Please enter 'y' or 'n'.")
-
-
-    node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
-    
-    # Run ROS 2 spin, and osc_process will be handled by the timer
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        print("")
-    finally:
-        node.destroy_node()
-        osc_terminate()
-
-if __name__ == '__main__':
-    main()
diff --git a/workspace/build/osc_ros2/colcon_build.rc b/workspace/build/osc_ros2/colcon_build.rc
deleted file mode 100644
index 573541a..0000000
--- a/workspace/build/osc_ros2/colcon_build.rc
+++ /dev/null
@@ -1 +0,0 @@
-0
diff --git a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh
deleted file mode 100644
index f9867d5..0000000
--- a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh
+++ /dev/null
@@ -1 +0,0 @@
-# generated from colcon_core/shell/template/command_prefix.sh.em
diff --git a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env
deleted file mode 100644
index 5f4a769..0000000
--- a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env
+++ /dev/null
@@ -1,20 +0,0 @@
-AMENT_PREFIX_PATH=/workspace/install/osc_ros2:/opt/ros/humble
-COLCON=1
-COLCON_PREFIX_PATH=/workspace/install
-HOME=/root
-HOSTNAME=ad72f3440b4e
-LANG=C.UTF-8
-LC_ALL=C.UTF-8
-LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
-LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
-OLDPWD=/
-PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
-PWD=/workspace/build/osc_ros2
-PYTHONPATH=/workspace/install/osc_ros2/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
-ROS_DISTRO=humble
-ROS_LOCALHOST_ONLY=0
-ROS_PYTHON_VERSION=3
-ROS_VERSION=2
-SHLVL=1
-TERM=xterm
-_=/usr/bin/colcon
diff --git a/workspace/build/osc_ros2/install.log b/workspace/build/osc_ros2/install.log
deleted file mode 100644
index bebe111..0000000
--- a/workspace/build/osc_ros2/install.log
+++ /dev/null
@@ -1,14 +0,0 @@
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc
-/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2
-/workspace/install/osc_ros2/share/osc_ros2/package.xml
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt
-/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt
-/workspace/install/osc_ros2/lib/osc_ros2/interface
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO b/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO
deleted file mode 100644
index 275623c..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO
+++ /dev/null
@@ -1,12 +0,0 @@
-Metadata-Version: 2.1
-Name: osc-ros2
-Version: 1.0.0
-Summary: Creates an interface for communication between OSC and Ros2
-Home-page: UNKNOWN
-Maintainer: Alexander Schaefer
-Maintainer-email: a.schaefer@tuhh.de
-License: Apache-2.0
-Platform: UNKNOWN
-
-UNKNOWN
-
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt
deleted file mode 100644
index 650a016..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-package.xml
-setup.cfg
-setup.py
-../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt
-../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-../../build/osc_ros2/osc_ros2.egg-info/zip-safe
-osc_ros2/__init__.py
-osc_ros2/osc_ros2.py
-resource/osc_ros2
-test/test_copyright.py
-test/test_flake8.py
-test/test_pep257.py
\ No newline at end of file
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
deleted file mode 100644
index 8b13789..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt
deleted file mode 100644
index 008581c..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-[console_scripts]
-interface = osc_ros2.osc_ros2:main
-
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt
deleted file mode 100644
index 3c8c415..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-numpy==1.22.4
-osc4py3
-roboticstoolbox-python
-scipy==1.7.3
-setuptools
-spatialmath-python
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt
deleted file mode 100644
index 99ee4eb..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-osc_ros2
diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe b/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe
deleted file mode 100644
index 8b13789..0000000
--- a/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc b/workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
deleted file mode 100644
index 5cfe643..0000000
Binary files a/workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc and /dev/null differ
diff --git a/workspace/build/osc_ros2/prefix_override/sitecustomize.py b/workspace/build/osc_ros2/prefix_override/sitecustomize.py
deleted file mode 100644
index f14fb81..0000000
--- a/workspace/build/osc_ros2/prefix_override/sitecustomize.py
+++ /dev/null
@@ -1,4 +0,0 @@
-import sys
-if sys.prefix == '/usr':
-    sys.real_prefix = sys.prefix
-    sys.prefix = sys.exec_prefix = '/workspace/install/osc_ros2'
diff --git a/workspace/install/.colcon_install_layout b/workspace/install/.colcon_install_layout
deleted file mode 100644
index 3aad533..0000000
--- a/workspace/install/.colcon_install_layout
+++ /dev/null
@@ -1 +0,0 @@
-isolated
diff --git a/workspace/install/COLCON_IGNORE b/workspace/install/COLCON_IGNORE
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/install/_local_setup_util_ps1.py b/workspace/install/_local_setup_util_ps1.py
deleted file mode 100644
index 3c6d9e8..0000000
--- a/workspace/install/_local_setup_util_ps1.py
+++ /dev/null
@@ -1,407 +0,0 @@
-# Copyright 2016-2019 Dirk Thomas
-# Licensed under the Apache License, Version 2.0
-
-import argparse
-from collections import OrderedDict
-import os
-from pathlib import Path
-import sys
-
-
-FORMAT_STR_COMMENT_LINE = '# {comment}'
-FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"'
-FORMAT_STR_USE_ENV_VAR = '$env:{name}'
-FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"'  # noqa: E501
-FORMAT_STR_REMOVE_LEADING_SEPARATOR = ''  # noqa: E501
-FORMAT_STR_REMOVE_TRAILING_SEPARATOR = ''  # noqa: E501
-
-DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
-DSV_TYPE_SET = 'set'
-DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
-DSV_TYPE_SOURCE = 'source'
-
-
-def main(argv=sys.argv[1:]):  # noqa: D103
-    parser = argparse.ArgumentParser(
-        description='Output shell commands for the packages in topological '
-                    'order')
-    parser.add_argument(
-        'primary_extension',
-        help='The file extension of the primary shell')
-    parser.add_argument(
-        'additional_extension', nargs='?',
-        help='The additional file extension to be considered')
-    parser.add_argument(
-        '--merged-install', action='store_true',
-        help='All install prefixes are merged into a single location')
-    args = parser.parse_args(argv)
-
-    packages = get_packages(Path(__file__).parent, args.merged_install)
-
-    ordered_packages = order_packages(packages)
-    for pkg_name in ordered_packages:
-        if _include_comments():
-            print(
-                FORMAT_STR_COMMENT_LINE.format_map(
-                    {'comment': 'Package: ' + pkg_name}))
-        prefix = os.path.abspath(os.path.dirname(__file__))
-        if not args.merged_install:
-            prefix = os.path.join(prefix, pkg_name)
-        for line in get_commands(
-            pkg_name, prefix, args.primary_extension,
-            args.additional_extension
-        ):
-            print(line)
-
-    for line in _remove_ending_separators():
-        print(line)
-
-
-def get_packages(prefix_path, merged_install):
-    """
-    Find packages based on colcon-specific files created during installation.
-
-    :param Path prefix_path: The install prefix path of all packages
-    :param bool merged_install: The flag if the packages are all installed
-      directly in the prefix or if each package is installed in a subdirectory
-      named after the package
-    :returns: A mapping from the package name to the set of runtime
-      dependencies
-    :rtype: dict
-    """
-    packages = {}
-    # since importing colcon_core isn't feasible here the following constant
-    # must match colcon_core.location.get_relative_package_index_path()
-    subdirectory = 'share/colcon-core/packages'
-    if merged_install:
-        # return if workspace is empty
-        if not (prefix_path / subdirectory).is_dir():
-            return packages
-        # find all files in the subdirectory
-        for p in (prefix_path / subdirectory).iterdir():
-            if not p.is_file():
-                continue
-            if p.name.startswith('.'):
-                continue
-            add_package_runtime_dependencies(p, packages)
-    else:
-        # for each subdirectory look for the package specific file
-        for p in prefix_path.iterdir():
-            if not p.is_dir():
-                continue
-            if p.name.startswith('.'):
-                continue
-            p = p / subdirectory / p.name
-            if p.is_file():
-                add_package_runtime_dependencies(p, packages)
-
-    # remove unknown dependencies
-    pkg_names = set(packages.keys())
-    for k in packages.keys():
-        packages[k] = {d for d in packages[k] if d in pkg_names}
-
-    return packages
-
-
-def add_package_runtime_dependencies(path, packages):
-    """
-    Check the path and if it exists extract the packages runtime dependencies.
-
-    :param Path path: The resource file containing the runtime dependencies
-    :param dict packages: A mapping from package names to the sets of runtime
-      dependencies to add to
-    """
-    content = path.read_text()
-    dependencies = set(content.split(os.pathsep) if content else [])
-    packages[path.name] = dependencies
-
-
-def order_packages(packages):
-    """
-    Order packages topologically.
-
-    :param dict packages: A mapping from package name to the set of runtime
-      dependencies
-    :returns: The package names
-    :rtype: list
-    """
-    # select packages with no dependencies in alphabetical order
-    to_be_ordered = list(packages.keys())
-    ordered = []
-    while to_be_ordered:
-        pkg_names_without_deps = [
-            name for name in to_be_ordered if not packages[name]]
-        if not pkg_names_without_deps:
-            reduce_cycle_set(packages)
-            raise RuntimeError(
-                'Circular dependency between: ' + ', '.join(sorted(packages)))
-        pkg_names_without_deps.sort()
-        pkg_name = pkg_names_without_deps[0]
-        to_be_ordered.remove(pkg_name)
-        ordered.append(pkg_name)
-        # remove item from dependency lists
-        for k in list(packages.keys()):
-            if pkg_name in packages[k]:
-                packages[k].remove(pkg_name)
-    return ordered
-
-
-def reduce_cycle_set(packages):
-    """
-    Reduce the set of packages to the ones part of the circular dependency.
-
-    :param dict packages: A mapping from package name to the set of runtime
-      dependencies which is modified in place
-    """
-    last_depended = None
-    while len(packages) > 0:
-        # get all remaining dependencies
-        depended = set()
-        for pkg_name, dependencies in packages.items():
-            depended = depended.union(dependencies)
-        # remove all packages which are not dependent on
-        for name in list(packages.keys()):
-            if name not in depended:
-                del packages[name]
-        if last_depended:
-            # if remaining packages haven't changed return them
-            if last_depended == depended:
-                return packages.keys()
-        # otherwise reduce again
-        last_depended = depended
-
-
-def _include_comments():
-    # skipping comment lines when COLCON_TRACE is not set speeds up the
-    # processing especially on Windows
-    return bool(os.environ.get('COLCON_TRACE'))
-
-
-def get_commands(pkg_name, prefix, primary_extension, additional_extension):
-    commands = []
-    package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
-    if os.path.exists(package_dsv_path):
-        commands += process_dsv_file(
-            package_dsv_path, prefix, primary_extension, additional_extension)
-    return commands
-
-
-def process_dsv_file(
-    dsv_path, prefix, primary_extension=None, additional_extension=None
-):
-    commands = []
-    if _include_comments():
-        commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
-    with open(dsv_path, 'r') as h:
-        content = h.read()
-    lines = content.splitlines()
-
-    basenames = OrderedDict()
-    for i, line in enumerate(lines):
-        # skip over empty or whitespace-only lines
-        if not line.strip():
-            continue
-        # skip over comments
-        if line.startswith('#'):
-            continue
-        try:
-            type_, remainder = line.split(';', 1)
-        except ValueError:
-            raise RuntimeError(
-                "Line %d in '%s' doesn't contain a semicolon separating the "
-                'type from the arguments' % (i + 1, dsv_path))
-        if type_ != DSV_TYPE_SOURCE:
-            # handle non-source lines
-            try:
-                commands += handle_dsv_types_except_source(
-                    type_, remainder, prefix)
-            except RuntimeError as e:
-                raise RuntimeError(
-                    "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
-        else:
-            # group remaining source lines by basename
-            path_without_ext, ext = os.path.splitext(remainder)
-            if path_without_ext not in basenames:
-                basenames[path_without_ext] = set()
-            assert ext.startswith('.')
-            ext = ext[1:]
-            if ext in (primary_extension, additional_extension):
-                basenames[path_without_ext].add(ext)
-
-    # add the dsv extension to each basename if the file exists
-    for basename, extensions in basenames.items():
-        if not os.path.isabs(basename):
-            basename = os.path.join(prefix, basename)
-        if os.path.exists(basename + '.dsv'):
-            extensions.add('dsv')
-
-    for basename, extensions in basenames.items():
-        if not os.path.isabs(basename):
-            basename = os.path.join(prefix, basename)
-        if 'dsv' in extensions:
-            # process dsv files recursively
-            commands += process_dsv_file(
-                basename + '.dsv', prefix, primary_extension=primary_extension,
-                additional_extension=additional_extension)
-        elif primary_extension in extensions and len(extensions) == 1:
-            # source primary-only files
-            commands += [
-                FORMAT_STR_INVOKE_SCRIPT.format_map({
-                    'prefix': prefix,
-                    'script_path': basename + '.' + primary_extension})]
-        elif additional_extension in extensions:
-            # source non-primary files
-            commands += [
-                FORMAT_STR_INVOKE_SCRIPT.format_map({
-                    'prefix': prefix,
-                    'script_path': basename + '.' + additional_extension})]
-
-    return commands
-
-
-def handle_dsv_types_except_source(type_, remainder, prefix):
-    commands = []
-    if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
-        try:
-            env_name, value = remainder.split(';', 1)
-        except ValueError:
-            raise RuntimeError(
-                "doesn't contain a semicolon separating the environment name "
-                'from the value')
-        try_prefixed_value = os.path.join(prefix, value) if value else prefix
-        if os.path.exists(try_prefixed_value):
-            value = try_prefixed_value
-        if type_ == DSV_TYPE_SET:
-            commands += _set(env_name, value)
-        elif type_ == DSV_TYPE_SET_IF_UNSET:
-            commands += _set_if_unset(env_name, value)
-        else:
-            assert False
-    elif type_ in (
-        DSV_TYPE_APPEND_NON_DUPLICATE,
-        DSV_TYPE_PREPEND_NON_DUPLICATE,
-        DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
-    ):
-        try:
-            env_name_and_values = remainder.split(';')
-        except ValueError:
-            raise RuntimeError(
-                "doesn't contain a semicolon separating the environment name "
-                'from the values')
-        env_name = env_name_and_values[0]
-        values = env_name_and_values[1:]
-        for value in values:
-            if not value:
-                value = prefix
-            elif not os.path.isabs(value):
-                value = os.path.join(prefix, value)
-            if (
-                type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
-                not os.path.exists(value)
-            ):
-                comment = f'skip extending {env_name} with not existing ' \
-                    f'path: {value}'
-                if _include_comments():
-                    commands.append(
-                        FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
-            elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
-                commands += _append_unique_value(env_name, value)
-            else:
-                commands += _prepend_unique_value(env_name, value)
-    else:
-        raise RuntimeError(
-            'contains an unknown environment hook type: ' + type_)
-    return commands
-
-
-env_state = {}
-
-
-def _append_unique_value(name, value):
-    global env_state
-    if name not in env_state:
-        if os.environ.get(name):
-            env_state[name] = set(os.environ[name].split(os.pathsep))
-        else:
-            env_state[name] = set()
-    # append even if the variable has not been set yet, in case a shell script sets the
-    # same variable without the knowledge of this Python script.
-    # later _remove_ending_separators() will cleanup any unintentional leading separator
-    extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': extend + value})
-    if value not in env_state[name]:
-        env_state[name].add(value)
-    else:
-        if not _include_comments():
-            return []
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-def _prepend_unique_value(name, value):
-    global env_state
-    if name not in env_state:
-        if os.environ.get(name):
-            env_state[name] = set(os.environ[name].split(os.pathsep))
-        else:
-            env_state[name] = set()
-    # prepend even if the variable has not been set yet, in case a shell script sets the
-    # same variable without the knowledge of this Python script.
-    # later _remove_ending_separators() will cleanup any unintentional trailing separator
-    extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value + extend})
-    if value not in env_state[name]:
-        env_state[name].add(value)
-    else:
-        if not _include_comments():
-            return []
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-# generate commands for removing prepended underscores
-def _remove_ending_separators():
-    # do nothing if the shell extension does not implement the logic
-    if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
-        return []
-
-    global env_state
-    commands = []
-    for name in env_state:
-        # skip variables that already had values before this script started prepending
-        if name in os.environ:
-            continue
-        commands += [
-            FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
-            FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
-    return commands
-
-
-def _set(name, value):
-    global env_state
-    env_state[name] = value
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value})
-    return [line]
-
-
-def _set_if_unset(name, value):
-    global env_state
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value})
-    if env_state.get(name, os.environ.get(name)):
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-if __name__ == '__main__':  # pragma: no cover
-    try:
-        rc = main()
-    except RuntimeError as e:
-        print(str(e), file=sys.stderr)
-        rc = 1
-    sys.exit(rc)
diff --git a/workspace/install/_local_setup_util_sh.py b/workspace/install/_local_setup_util_sh.py
deleted file mode 100644
index f67eaa9..0000000
--- a/workspace/install/_local_setup_util_sh.py
+++ /dev/null
@@ -1,407 +0,0 @@
-# Copyright 2016-2019 Dirk Thomas
-# Licensed under the Apache License, Version 2.0
-
-import argparse
-from collections import OrderedDict
-import os
-from pathlib import Path
-import sys
-
-
-FORMAT_STR_COMMENT_LINE = '# {comment}'
-FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
-FORMAT_STR_USE_ENV_VAR = '${name}'
-FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"'  # noqa: E501
-FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi'  # noqa: E501
-FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi'  # noqa: E501
-
-DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
-DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
-DSV_TYPE_SET = 'set'
-DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
-DSV_TYPE_SOURCE = 'source'
-
-
-def main(argv=sys.argv[1:]):  # noqa: D103
-    parser = argparse.ArgumentParser(
-        description='Output shell commands for the packages in topological '
-                    'order')
-    parser.add_argument(
-        'primary_extension',
-        help='The file extension of the primary shell')
-    parser.add_argument(
-        'additional_extension', nargs='?',
-        help='The additional file extension to be considered')
-    parser.add_argument(
-        '--merged-install', action='store_true',
-        help='All install prefixes are merged into a single location')
-    args = parser.parse_args(argv)
-
-    packages = get_packages(Path(__file__).parent, args.merged_install)
-
-    ordered_packages = order_packages(packages)
-    for pkg_name in ordered_packages:
-        if _include_comments():
-            print(
-                FORMAT_STR_COMMENT_LINE.format_map(
-                    {'comment': 'Package: ' + pkg_name}))
-        prefix = os.path.abspath(os.path.dirname(__file__))
-        if not args.merged_install:
-            prefix = os.path.join(prefix, pkg_name)
-        for line in get_commands(
-            pkg_name, prefix, args.primary_extension,
-            args.additional_extension
-        ):
-            print(line)
-
-    for line in _remove_ending_separators():
-        print(line)
-
-
-def get_packages(prefix_path, merged_install):
-    """
-    Find packages based on colcon-specific files created during installation.
-
-    :param Path prefix_path: The install prefix path of all packages
-    :param bool merged_install: The flag if the packages are all installed
-      directly in the prefix or if each package is installed in a subdirectory
-      named after the package
-    :returns: A mapping from the package name to the set of runtime
-      dependencies
-    :rtype: dict
-    """
-    packages = {}
-    # since importing colcon_core isn't feasible here the following constant
-    # must match colcon_core.location.get_relative_package_index_path()
-    subdirectory = 'share/colcon-core/packages'
-    if merged_install:
-        # return if workspace is empty
-        if not (prefix_path / subdirectory).is_dir():
-            return packages
-        # find all files in the subdirectory
-        for p in (prefix_path / subdirectory).iterdir():
-            if not p.is_file():
-                continue
-            if p.name.startswith('.'):
-                continue
-            add_package_runtime_dependencies(p, packages)
-    else:
-        # for each subdirectory look for the package specific file
-        for p in prefix_path.iterdir():
-            if not p.is_dir():
-                continue
-            if p.name.startswith('.'):
-                continue
-            p = p / subdirectory / p.name
-            if p.is_file():
-                add_package_runtime_dependencies(p, packages)
-
-    # remove unknown dependencies
-    pkg_names = set(packages.keys())
-    for k in packages.keys():
-        packages[k] = {d for d in packages[k] if d in pkg_names}
-
-    return packages
-
-
-def add_package_runtime_dependencies(path, packages):
-    """
-    Check the path and if it exists extract the packages runtime dependencies.
-
-    :param Path path: The resource file containing the runtime dependencies
-    :param dict packages: A mapping from package names to the sets of runtime
-      dependencies to add to
-    """
-    content = path.read_text()
-    dependencies = set(content.split(os.pathsep) if content else [])
-    packages[path.name] = dependencies
-
-
-def order_packages(packages):
-    """
-    Order packages topologically.
-
-    :param dict packages: A mapping from package name to the set of runtime
-      dependencies
-    :returns: The package names
-    :rtype: list
-    """
-    # select packages with no dependencies in alphabetical order
-    to_be_ordered = list(packages.keys())
-    ordered = []
-    while to_be_ordered:
-        pkg_names_without_deps = [
-            name for name in to_be_ordered if not packages[name]]
-        if not pkg_names_without_deps:
-            reduce_cycle_set(packages)
-            raise RuntimeError(
-                'Circular dependency between: ' + ', '.join(sorted(packages)))
-        pkg_names_without_deps.sort()
-        pkg_name = pkg_names_without_deps[0]
-        to_be_ordered.remove(pkg_name)
-        ordered.append(pkg_name)
-        # remove item from dependency lists
-        for k in list(packages.keys()):
-            if pkg_name in packages[k]:
-                packages[k].remove(pkg_name)
-    return ordered
-
-
-def reduce_cycle_set(packages):
-    """
-    Reduce the set of packages to the ones part of the circular dependency.
-
-    :param dict packages: A mapping from package name to the set of runtime
-      dependencies which is modified in place
-    """
-    last_depended = None
-    while len(packages) > 0:
-        # get all remaining dependencies
-        depended = set()
-        for pkg_name, dependencies in packages.items():
-            depended = depended.union(dependencies)
-        # remove all packages which are not dependent on
-        for name in list(packages.keys()):
-            if name not in depended:
-                del packages[name]
-        if last_depended:
-            # if remaining packages haven't changed return them
-            if last_depended == depended:
-                return packages.keys()
-        # otherwise reduce again
-        last_depended = depended
-
-
-def _include_comments():
-    # skipping comment lines when COLCON_TRACE is not set speeds up the
-    # processing especially on Windows
-    return bool(os.environ.get('COLCON_TRACE'))
-
-
-def get_commands(pkg_name, prefix, primary_extension, additional_extension):
-    commands = []
-    package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
-    if os.path.exists(package_dsv_path):
-        commands += process_dsv_file(
-            package_dsv_path, prefix, primary_extension, additional_extension)
-    return commands
-
-
-def process_dsv_file(
-    dsv_path, prefix, primary_extension=None, additional_extension=None
-):
-    commands = []
-    if _include_comments():
-        commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
-    with open(dsv_path, 'r') as h:
-        content = h.read()
-    lines = content.splitlines()
-
-    basenames = OrderedDict()
-    for i, line in enumerate(lines):
-        # skip over empty or whitespace-only lines
-        if not line.strip():
-            continue
-        # skip over comments
-        if line.startswith('#'):
-            continue
-        try:
-            type_, remainder = line.split(';', 1)
-        except ValueError:
-            raise RuntimeError(
-                "Line %d in '%s' doesn't contain a semicolon separating the "
-                'type from the arguments' % (i + 1, dsv_path))
-        if type_ != DSV_TYPE_SOURCE:
-            # handle non-source lines
-            try:
-                commands += handle_dsv_types_except_source(
-                    type_, remainder, prefix)
-            except RuntimeError as e:
-                raise RuntimeError(
-                    "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
-        else:
-            # group remaining source lines by basename
-            path_without_ext, ext = os.path.splitext(remainder)
-            if path_without_ext not in basenames:
-                basenames[path_without_ext] = set()
-            assert ext.startswith('.')
-            ext = ext[1:]
-            if ext in (primary_extension, additional_extension):
-                basenames[path_without_ext].add(ext)
-
-    # add the dsv extension to each basename if the file exists
-    for basename, extensions in basenames.items():
-        if not os.path.isabs(basename):
-            basename = os.path.join(prefix, basename)
-        if os.path.exists(basename + '.dsv'):
-            extensions.add('dsv')
-
-    for basename, extensions in basenames.items():
-        if not os.path.isabs(basename):
-            basename = os.path.join(prefix, basename)
-        if 'dsv' in extensions:
-            # process dsv files recursively
-            commands += process_dsv_file(
-                basename + '.dsv', prefix, primary_extension=primary_extension,
-                additional_extension=additional_extension)
-        elif primary_extension in extensions and len(extensions) == 1:
-            # source primary-only files
-            commands += [
-                FORMAT_STR_INVOKE_SCRIPT.format_map({
-                    'prefix': prefix,
-                    'script_path': basename + '.' + primary_extension})]
-        elif additional_extension in extensions:
-            # source non-primary files
-            commands += [
-                FORMAT_STR_INVOKE_SCRIPT.format_map({
-                    'prefix': prefix,
-                    'script_path': basename + '.' + additional_extension})]
-
-    return commands
-
-
-def handle_dsv_types_except_source(type_, remainder, prefix):
-    commands = []
-    if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
-        try:
-            env_name, value = remainder.split(';', 1)
-        except ValueError:
-            raise RuntimeError(
-                "doesn't contain a semicolon separating the environment name "
-                'from the value')
-        try_prefixed_value = os.path.join(prefix, value) if value else prefix
-        if os.path.exists(try_prefixed_value):
-            value = try_prefixed_value
-        if type_ == DSV_TYPE_SET:
-            commands += _set(env_name, value)
-        elif type_ == DSV_TYPE_SET_IF_UNSET:
-            commands += _set_if_unset(env_name, value)
-        else:
-            assert False
-    elif type_ in (
-        DSV_TYPE_APPEND_NON_DUPLICATE,
-        DSV_TYPE_PREPEND_NON_DUPLICATE,
-        DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
-    ):
-        try:
-            env_name_and_values = remainder.split(';')
-        except ValueError:
-            raise RuntimeError(
-                "doesn't contain a semicolon separating the environment name "
-                'from the values')
-        env_name = env_name_and_values[0]
-        values = env_name_and_values[1:]
-        for value in values:
-            if not value:
-                value = prefix
-            elif not os.path.isabs(value):
-                value = os.path.join(prefix, value)
-            if (
-                type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
-                not os.path.exists(value)
-            ):
-                comment = f'skip extending {env_name} with not existing ' \
-                    f'path: {value}'
-                if _include_comments():
-                    commands.append(
-                        FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
-            elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
-                commands += _append_unique_value(env_name, value)
-            else:
-                commands += _prepend_unique_value(env_name, value)
-    else:
-        raise RuntimeError(
-            'contains an unknown environment hook type: ' + type_)
-    return commands
-
-
-env_state = {}
-
-
-def _append_unique_value(name, value):
-    global env_state
-    if name not in env_state:
-        if os.environ.get(name):
-            env_state[name] = set(os.environ[name].split(os.pathsep))
-        else:
-            env_state[name] = set()
-    # append even if the variable has not been set yet, in case a shell script sets the
-    # same variable without the knowledge of this Python script.
-    # later _remove_ending_separators() will cleanup any unintentional leading separator
-    extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': extend + value})
-    if value not in env_state[name]:
-        env_state[name].add(value)
-    else:
-        if not _include_comments():
-            return []
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-def _prepend_unique_value(name, value):
-    global env_state
-    if name not in env_state:
-        if os.environ.get(name):
-            env_state[name] = set(os.environ[name].split(os.pathsep))
-        else:
-            env_state[name] = set()
-    # prepend even if the variable has not been set yet, in case a shell script sets the
-    # same variable without the knowledge of this Python script.
-    # later _remove_ending_separators() will cleanup any unintentional trailing separator
-    extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value + extend})
-    if value not in env_state[name]:
-        env_state[name].add(value)
-    else:
-        if not _include_comments():
-            return []
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-# generate commands for removing prepended underscores
-def _remove_ending_separators():
-    # do nothing if the shell extension does not implement the logic
-    if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
-        return []
-
-    global env_state
-    commands = []
-    for name in env_state:
-        # skip variables that already had values before this script started prepending
-        if name in os.environ:
-            continue
-        commands += [
-            FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
-            FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
-    return commands
-
-
-def _set(name, value):
-    global env_state
-    env_state[name] = value
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value})
-    return [line]
-
-
-def _set_if_unset(name, value):
-    global env_state
-    line = FORMAT_STR_SET_ENV_VAR.format_map(
-        {'name': name, 'value': value})
-    if env_state.get(name, os.environ.get(name)):
-        line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
-    return [line]
-
-
-if __name__ == '__main__':  # pragma: no cover
-    try:
-        rc = main()
-    except RuntimeError as e:
-        print(str(e), file=sys.stderr)
-        rc = 1
-    sys.exit(rc)
diff --git a/workspace/install/local_setup.bash b/workspace/install/local_setup.bash
deleted file mode 100644
index 03f0025..0000000
--- a/workspace/install/local_setup.bash
+++ /dev/null
@@ -1,121 +0,0 @@
-# generated from colcon_bash/shell/template/prefix.bash.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# a bash script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
-else
-  _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_bash_prepend_unique_value() {
-  # arguments
-  _listname="$1"
-  _value="$2"
-
-  # get values from variable
-  eval _values=\"\$$_listname\"
-  # backup the field separator
-  _colcon_prefix_bash_prepend_unique_value_IFS="$IFS"
-  IFS=":"
-  # start with the new value
-  _all_values="$_value"
-  _contained_value=""
-  # iterate over existing values in the variable
-  for _item in $_values; do
-    # ignore empty strings
-    if [ -z "$_item" ]; then
-      continue
-    fi
-    # ignore duplicates of _value
-    if [ "$_item" = "$_value" ]; then
-      _contained_value=1
-      continue
-    fi
-    # keep non-duplicate values
-    _all_values="$_all_values:$_item"
-  done
-  unset _item
-  if [ -z "$_contained_value" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      if [ "$_all_values" = "$_value" ]; then
-        echo "export $_listname=$_value"
-      else
-        echo "export $_listname=$_value:\$$_listname"
-      fi
-    fi
-  fi
-  unset _contained_value
-  # restore the field separator
-  IFS="$_colcon_prefix_bash_prepend_unique_value_IFS"
-  unset _colcon_prefix_bash_prepend_unique_value_IFS
-  # export the updated variable
-  eval export $_listname=\"$_all_values\"
-  unset _all_values
-  unset _values
-
-  unset _value
-  unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_bash_prepend_unique_value
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
-  if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
-    echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
-    return 1
-  fi
-  _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
-  # try the Python executable known at configure time
-  _colcon_python_executable="/usr/bin/python3"
-  # if it doesn't exist try a fall back
-  if [ ! -f "$_colcon_python_executable" ]; then
-    if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
-      echo "error: unable to find python3 executable"
-      return 1
-    fi
-    _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
-  fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
-  echo "$(declare -f _colcon_prefix_sh_source_script)"
-  echo "# Execute generated script:"
-  echo "# <<<"
-  echo "${_colcon_ordered_commands}"
-  echo "# >>>"
-  echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX
diff --git a/workspace/install/local_setup.ps1 b/workspace/install/local_setup.ps1
deleted file mode 100644
index 6f68c8d..0000000
--- a/workspace/install/local_setup.ps1
+++ /dev/null
@@ -1,55 +0,0 @@
-# generated from colcon_powershell/shell/template/prefix.ps1.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# check environment variable for custom Python executable
-if ($env:COLCON_PYTHON_EXECUTABLE) {
-  if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) {
-    echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist"
-    exit 1
-  }
-  $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE"
-} else {
-  # use the Python executable known at configure time
-  $_colcon_python_executable="/usr/bin/python3"
-  # if it doesn't exist try a fall back
-  if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) {
-    if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) {
-      echo "error: unable to find python3 executable"
-      exit 1
-    }
-    $_colcon_python_executable="python3"
-  }
-}
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-function _colcon_prefix_powershell_source_script {
-  param (
-    $_colcon_prefix_powershell_source_script_param
-  )
-  # source script with conditional trace output
-  if (Test-Path $_colcon_prefix_powershell_source_script_param) {
-    if ($env:COLCON_TRACE) {
-      echo ". '$_colcon_prefix_powershell_source_script_param'"
-    }
-    . "$_colcon_prefix_powershell_source_script_param"
-  } else {
-    Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'"
-  }
-}
-
-# get all commands in topological order
-$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1
-
-# execute all commands in topological order
-if ($env:COLCON_TRACE) {
-  echo "Execute generated script:"
-  echo "<<<"
-  $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output
-  echo ">>>"
-}
-if ($_colcon_ordered_commands) {
-  $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression
-}
diff --git a/workspace/install/local_setup.sh b/workspace/install/local_setup.sh
deleted file mode 100644
index 24bb54f..0000000
--- a/workspace/install/local_setup.sh
+++ /dev/null
@@ -1,137 +0,0 @@
-# generated from colcon_core/shell/template/prefix.sh.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/workspace/install"
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
-    echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
-    unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
-    return 1
-  fi
-else
-  _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_sh_prepend_unique_value() {
-  # arguments
-  _listname="$1"
-  _value="$2"
-
-  # get values from variable
-  eval _values=\"\$$_listname\"
-  # backup the field separator
-  _colcon_prefix_sh_prepend_unique_value_IFS="$IFS"
-  IFS=":"
-  # start with the new value
-  _all_values="$_value"
-  _contained_value=""
-  # iterate over existing values in the variable
-  for _item in $_values; do
-    # ignore empty strings
-    if [ -z "$_item" ]; then
-      continue
-    fi
-    # ignore duplicates of _value
-    if [ "$_item" = "$_value" ]; then
-      _contained_value=1
-      continue
-    fi
-    # keep non-duplicate values
-    _all_values="$_all_values:$_item"
-  done
-  unset _item
-  if [ -z "$_contained_value" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      if [ "$_all_values" = "$_value" ]; then
-        echo "export $_listname=$_value"
-      else
-        echo "export $_listname=$_value:\$$_listname"
-      fi
-    fi
-  fi
-  unset _contained_value
-  # restore the field separator
-  IFS="$_colcon_prefix_sh_prepend_unique_value_IFS"
-  unset _colcon_prefix_sh_prepend_unique_value_IFS
-  # export the updated variable
-  eval export $_listname=\"$_all_values\"
-  unset _all_values
-  unset _values
-
-  unset _value
-  unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_sh_prepend_unique_value
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
-  if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
-    echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
-    return 1
-  fi
-  _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
-  # try the Python executable known at configure time
-  _colcon_python_executable="/usr/bin/python3"
-  # if it doesn't exist try a fall back
-  if [ ! -f "$_colcon_python_executable" ]; then
-    if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
-      echo "error: unable to find python3 executable"
-      return 1
-    fi
-    _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
-  fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
-  echo "_colcon_prefix_sh_source_script() {
-    if [ -f \"\$1\" ]; then
-      if [ -n \"\$COLCON_TRACE\" ]; then
-        echo \"# . \\\"\$1\\\"\"
-      fi
-      . \"\$1\"
-    else
-      echo \"not found: \\\"\$1\\\"\" 1>&2
-    fi
-  }"
-  echo "# Execute generated script:"
-  echo "# <<<"
-  echo "${_colcon_ordered_commands}"
-  echo "# >>>"
-  echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
diff --git a/workspace/install/local_setup.zsh b/workspace/install/local_setup.zsh
deleted file mode 100644
index b648710..0000000
--- a/workspace/install/local_setup.zsh
+++ /dev/null
@@ -1,134 +0,0 @@
-# generated from colcon_zsh/shell/template/prefix.zsh.em
-
-# This script extends the environment with all packages contained in this
-# prefix path.
-
-# a zsh script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
-else
-  _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to convert array-like strings into arrays
-# to workaround SH_WORD_SPLIT not being set
-_colcon_prefix_zsh_convert_to_array() {
-  local _listname=$1
-  local _dollar="$"
-  local _split="{="
-  local _to_array="(\"$_dollar$_split$_listname}\")"
-  eval $_listname=$_to_array
-}
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prefix_zsh_prepend_unique_value() {
-  # arguments
-  _listname="$1"
-  _value="$2"
-
-  # get values from variable
-  eval _values=\"\$$_listname\"
-  # backup the field separator
-  _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS"
-  IFS=":"
-  # start with the new value
-  _all_values="$_value"
-  _contained_value=""
-  # workaround SH_WORD_SPLIT not being set
-  _colcon_prefix_zsh_convert_to_array _values
-  # iterate over existing values in the variable
-  for _item in $_values; do
-    # ignore empty strings
-    if [ -z "$_item" ]; then
-      continue
-    fi
-    # ignore duplicates of _value
-    if [ "$_item" = "$_value" ]; then
-      _contained_value=1
-      continue
-    fi
-    # keep non-duplicate values
-    _all_values="$_all_values:$_item"
-  done
-  unset _item
-  if [ -z "$_contained_value" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      if [ "$_all_values" = "$_value" ]; then
-        echo "export $_listname=$_value"
-      else
-        echo "export $_listname=$_value:\$$_listname"
-      fi
-    fi
-  fi
-  unset _contained_value
-  # restore the field separator
-  IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS"
-  unset _colcon_prefix_zsh_prepend_unique_value_IFS
-  # export the updated variable
-  eval export $_listname=\"$_all_values\"
-  unset _all_values
-  unset _values
-
-  unset _value
-  unset _listname
-}
-
-# add this prefix to the COLCON_PREFIX_PATH
-_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX"
-unset _colcon_prefix_zsh_prepend_unique_value
-unset _colcon_prefix_zsh_convert_to_array
-
-# check environment variable for custom Python executable
-if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
-  if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
-    echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
-    return 1
-  fi
-  _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
-else
-  # try the Python executable known at configure time
-  _colcon_python_executable="/usr/bin/python3"
-  # if it doesn't exist try a fall back
-  if [ ! -f "$_colcon_python_executable" ]; then
-    if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
-      echo "error: unable to find python3 executable"
-      return 1
-    fi
-    _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
-  fi
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_sh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# get all commands in topological order
-_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)"
-unset _colcon_python_executable
-if [ -n "$COLCON_TRACE" ]; then
-  echo "$(declare -f _colcon_prefix_sh_source_script)"
-  echo "# Execute generated script:"
-  echo "# <<<"
-  echo "${_colcon_ordered_commands}"
-  echo "# >>>"
-  echo "unset _colcon_prefix_sh_source_script"
-fi
-eval "${_colcon_ordered_commands}"
-unset _colcon_ordered_commands
-
-unset _colcon_prefix_sh_source_script
-
-unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX
diff --git a/workspace/install/osc_ros2/lib/osc_ros2/interface b/workspace/install/osc_ros2/lib/osc_ros2/interface
deleted file mode 100755
index 61d20a1..0000000
--- a/workspace/install/osc_ros2/lib/osc_ros2/interface
+++ /dev/null
@@ -1,33 +0,0 @@
-#!/usr/bin/python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'osc-ros2==1.0.0','console_scripts','interface'
-import re
-import sys
-
-# for compatibility with easy_install; see #2198
-__requires__ = 'osc-ros2==1.0.0'
-
-try:
-    from importlib.metadata import distribution
-except ImportError:
-    try:
-        from importlib_metadata import distribution
-    except ImportError:
-        from pkg_resources import load_entry_point
-
-
-def importlib_load_entry_point(spec, group, name):
-    dist_name, _, _ = spec.partition('==')
-    matches = (
-        entry_point
-        for entry_point in distribution(dist_name).entry_points
-        if entry_point.group == group and entry_point.name == name
-    )
-    return next(matches).load()
-
-
-globals().setdefault('load_entry_point', importlib_load_entry_point)
-
-
-if __name__ == '__main__':
-    sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('osc-ros2==1.0.0', 'console_scripts', 'interface')())
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO
deleted file mode 100644
index 275623c..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO
+++ /dev/null
@@ -1,12 +0,0 @@
-Metadata-Version: 2.1
-Name: osc-ros2
-Version: 1.0.0
-Summary: Creates an interface for communication between OSC and Ros2
-Home-page: UNKNOWN
-Maintainer: Alexander Schaefer
-Maintainer-email: a.schaefer@tuhh.de
-License: Apache-2.0
-Platform: UNKNOWN
-
-UNKNOWN
-
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt
deleted file mode 100644
index 650a016..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-package.xml
-setup.cfg
-setup.py
-../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt
-../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-../../build/osc_ros2/osc_ros2.egg-info/zip-safe
-osc_ros2/__init__.py
-osc_ros2/osc_ros2.py
-resource/osc_ros2
-test/test_copyright.py
-test/test_flake8.py
-test/test_pep257.py
\ No newline at end of file
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt
deleted file mode 100644
index 8b13789..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt
deleted file mode 100644
index 008581c..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt
+++ /dev/null
@@ -1,3 +0,0 @@
-[console_scripts]
-interface = osc_ros2.osc_ros2:main
-
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt
deleted file mode 100644
index 3c8c415..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt
+++ /dev/null
@@ -1,6 +0,0 @@
-numpy==1.22.4
-osc4py3
-roboticstoolbox-python
-scipy==1.7.3
-setuptools
-spatialmath-python
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt
deleted file mode 100644
index 99ee4eb..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt
+++ /dev/null
@@ -1 +0,0 @@
-osc_ros2
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe
deleted file mode 100644
index 8b13789..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe
+++ /dev/null
@@ -1 +0,0 @@
-
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc
deleted file mode 100644
index 849cee7..0000000
Binary files a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc and /dev/null differ
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc
deleted file mode 100644
index d310618..0000000
Binary files a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc and /dev/null differ
diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py
deleted file mode 100644
index 0b28d5b..0000000
--- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py
+++ /dev/null
@@ -1,1099 +0,0 @@
-import rclpy
-from rclpy.node import Node
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from sensor_msgs.msg import JointState
-from rcl_interfaces.msg import Log
-from osc4py3.as_allthreads import *
-from osc4py3 import oscmethod as osm
-import xml.etree.ElementTree as ET
-import numpy as np
-import spatialmath as sm
-import roboticstoolbox as rtb
-from osc4py3 import oscbuildparse
-import time
-import os
-import re
-import socket
-
-class JointNameListener(Node):
-    def __init__(self):
-        super().__init__('joint_name_listener')
-        self.subscription = self.create_subscription(
-            JointState,
-            '/joint_states',
-            self.joint_state_callback,
-            1
-        )
-        self.joint_names = None
-
-    def joint_state_callback(self, msg: JointState):
-        self.joint_names = list(msg.name)
-
-class OSC_ROS2_interface(Node):
-    """Node to publish joint trajectories based on OSC messages."""
-    
-    def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask):
-        super().__init__('scaled_joint_trajectory_publisher')
-        
-
-        self.subscription = self.create_subscription(
-            JointState,
-            '/joint_states',
-            self.joint_states_callback,
-            1
-        )
-
-        self.subscription = self.create_subscription(
-            Log,
-            '/rosout',
-            self.log_callback,
-            100
-        )
-
-        # Store received joint positions
-        self.current_joint_positions = None
-        self.joint_names = joint_names
-        self.joint_velocity_limits = joint_velocity_limits
-        self.cost_mask = cost_mask
-        self.robot = robot
-        self.desired = None
-        self.previous_desired = None
-        self.log_dict = {
-            10: "DEBUG",
-            20: "INFO",
-            30: "WARN",
-            40: "ERROR",
-            50: "FATAL",
-        }
-        self.speed_scaling = 0.2
-        self.new = False
-        self.n_joints = len(joint_names)   
-
-        if robot:
-            while True:
-                print('+-' * 50)
-                set_limits = input("Do you want to set limits for x, y and z? (y/n): ").strip().lower()
-                if set_limits == 'y':
-                    while True:
-                                try:
-                                    self.x_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
-                                    self.y_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
-                                    self.z_limits = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
-                                    
-                                    if len(self.x_limits) != 2 or len(self.y_limits) != 2 or len(self.z_limits) != 2:
-                                        print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
-                                        continue
-                                    
-                                    if (self.x_limits[0] is not None and self.x_limits[1] is not None and self.x_limits[0] >= self.x_limits[1]) or \
-                                    (self.y_limits[0] is not None and self.y_limits[1] is not None and self.y_limits[0] >= self.y_limits[1]) or \
-                                    (self.z_limits[0] is not None and self.z_limits[1] is not None and self.z_limits[0] >= self.z_limits[1]):
-                                        print("Invalid input. Lower limit must be less than upper limit for each axis.")
-                                        continue
-                                    
-                                    print(f"Current limits:")
-                                    print(f"x: {self.x_limits}")
-                                    print(f"y: {self.y_limits}")
-                                    print(f"z: {self.z_limits}")
-                                    con = True
-                                    while con:
-                                        confirm = input("Do you want your TCP to move in this range? (y/n): ").strip().lower()
-                                        if confirm == 'y':
-                                            break
-                                        elif confirm == 'n':
-                                            print("Please re-enter the limits.")
-                                            con = False
-                                        else:
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    if con: break
-                                except ValueError:
-                                    print("Invalid input. Please enter numeric values only.")
-                    break
-                elif set_limits == 'n': 
-                    self.x_limits = [None, None]
-                    self.y_limits = [None, None]
-                    self.z_limits = [None, None]
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-
-            while True:
-                print('+-' * 50)
-                set_limits = input("Do you want to set workspace limits in x, y and z direction? (y/n): ").strip().lower()
-                if set_limits == 'y':
-                    while True:
-                                try:
-                                    self.x_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for x (space-separated, enter 'x' for no limit): ").split()]
-                                    self.y_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for y (space-separated, enter 'x' for no limit): ").split()]
-                                    self.z_limits_workspace = [float(i) if i != 'x' else None for i in input("Enter the lower and upper limits for z (space-separated, enter 'x' for no limit): ").split()]
-                                    
-                                    if len(self.x_limits_workspace) != 2 or len(self.y_limits_workspace) != 2 or len(self.z_limits_workspace) != 2:
-                                        print("Invalid input. Please enter exactly two values (or leave blank) for each limit.")
-                                        continue
-                                    
-                                    if (self.x_limits_workspace[0] is not None and self.x_limits_workspace[1] is not None and self.x_limits_workspace[0] >= self.x_limits_workspace[1]) or \
-                                    (self.y_limits_workspace[0] is not None and self.y_limits_workspace[1] is not None and self.y_limits_workspace[0] >= self.y_limits_workspace[1]) or \
-                                    (self.z_limits_workspace[0] is not None and self.z_limits_workspace[1] is not None and self.z_limits_workspace[0] >= self.z_limits_workspace[1]):
-                                        print("Invalid input. Lower limit must be less than upper limit for each axis.")
-                                        continue
-                                    
-                                    print(f"Current limits:")
-                                    print(f"x: {self.x_limits_workspace}")
-                                    print(f"y: {self.y_limits_workspace}")
-                                    print(f"z: {self.z_limits_workspace}")
-                                    con = True
-                                    while con:
-                                        confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower()
-                                        if confirm == 'y':
-                                            break
-                                        elif confirm == 'n':
-                                            print("Please re-enter the limits.")
-                                            con = False
-                                        else:
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    if con: break
-                                except ValueError:
-                                    print("Invalid input. Please enter numeric values only.")
-                    break
-                elif set_limits == 'n': 
-                    self.x_limits_workspace = [None, None]
-                    self.y_limits_workspace = [None, None]
-                    self.z_limits_workspace = [None, None]
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-                        # Ask the user if they want to set new joint limits
-
-                        # Ask the user if they want to set new joint limits
-            while True:
-                print('+-'*50)
-                update_limits = input("Do you want to set new joint limits? (y/n): ").strip().lower()
-                if update_limits == 'y':
-                    for i in range(len(self.joint_names)):
-                        while True:
-                            try:
-                                lim = self.robot.qlim.copy()
-                                # Find the link corresponding to the joint name
-                                print("-" * 50)
-                                print(f"Current position limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
-                                lower_limit = input(f"Enter the new lower limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
-                                upper_limit = input(f"Enter the new upper limit for joint '{self.joint_names[i]}' (or press Enter to keep current): ").strip()
-                                
-                                if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
-                                    print("Invalid input. Lower limit must be less than upper limit.")
-                                    continue
-                                lower_limit = float(lower_limit) if lower_limit!="" else None
-                                upper_limit = float(upper_limit) if upper_limit!="" else None
-                                if lower_limit!=None:
-                                    if lower_limit<lim[0][i]:
-                                        while True:
-                                            sure = input(f"Are you sure you want to set the lower limit to {lower_limit} rad which is less than the default limit {lim[0][i]} (y/n)?: ").strip().lower()
-                                            if sure == 'y':
-                                                lim[0][i] = float(lower_limit)
-                                                break
-                                            elif sure == 'n':
-                                                print("Lower limit not changed.")
-                                                break
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    else: lim[0][i] = float(lower_limit)
-                                if upper_limit!=None:
-                                    if upper_limit>lim[1][i]:
-                                        while True:
-                                            sure = input(f"Are you sure you want to set the upper limit to {upper_limit} rad which is more than the default limit {lim[1][i]} (y/n)?: ").strip().lower()
-                                            if sure == 'y':
-                                                lim[1][i] = float(upper_limit)
-                                                break
-                                            elif sure == 'n':
-                                                print("Upper limit not changed.")
-                                                break
-                                            print("Invalid input. Please enter 'y' or 'n'.")
-                                    else: lim[1][i] = float(upper_limit)
-                                self.robot.qlim = lim
-                                print(f"New limits for joint '{self.joint_names[i]}': [{self.robot.qlim[0][i]} {self.robot.qlim[1][i]}] rad")
-                                print("-" * 50)
-                                break
-                            except ValueError:
-                                print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
-                    break
-                if update_limits == 'n':
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-            '''
-            use_link_mask = input("Do you want to use a link mask? (y/n): ").strip().lower()
-            if use_link_mask == 'y':
-                while True:
-                    try:
-    '''
-        else:
-            while True:
-                print('+-' * 50)
-                update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower()
-                if update_limits == 'y':
-                    self.joint_lim = [[None]*self.n_joints,[None]*self.n_joints]
-                    for i, joint in enumerate(self.joint_names):
-                        while True:
-                            try:
-                                print("-" * 50)
-                                lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter for None): ").strip()
-                                upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter for None): ").strip()
-                                
-                                if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
-                                    print('--' * 50)
-                                    print("      !!!      Invalid input. Lower limit must be less than upper limit.      !!!      ")
-                                    continue
-                                self.joint_lim[0][i] = float(lower_limit) if lower_limit!="" else None
-                                self.joint_lim[1][i] = float(upper_limit) if upper_limit!="" else None
-                                break
-                            except ValueError:
-                                print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
-                    print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
-                    break
-                elif update_limits == 'n':
-                    self.joint_lim = None
-                    break
-                print("Invalid input. Please enter 'y' or 'n'.")
-                
-
-
-        while True:
-            try:
-                print('+-' * 50)
-                self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip()
-                if self.trajectory_topic_name == "":
-                    self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
-                    break
-                elif self.trajectory_topic_name.startswith("/"):
-                    break
-                else:
-                    print("Invalid topic name. A valid topic name should start with '/'.")
-            except Exception as e:
-                print(f"An error occurred: {e}")
-
-        # ROS2 Publisher
-        self.publisher = self.create_publisher(
-            JointTrajectory, 
-            self.trajectory_topic_name, 
-            1
-        )
-
-        while True:
-            try:
-                print('+-' * 50)
-                log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): "))
-                if log_ip == "":
-                    log_ip = "127.0.0.1"
-                print('--' * 50)
-                log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ")
-                if log_port == "":
-                    log_port = 5005
-                else:
-                    log_port = int(log_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid IP address.")
-                continue
-        while True:
-            try:
-                print('+-' * 50)
-                state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): "))
-                if state_ip == "":
-                    state_ip = "127.0.0.1"
-                print('--' * 50)
-                state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ")
-                if state_port == "":
-                    state_port = 7000
-                else:
-                    state_port = int(state_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid IP address.")
-                continue
-        while True:
-            try:
-                print('+-' * 50)
-                commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ")
-                if commands_port == state_port:
-                    print("The commands port must be different from the state port.")
-                    continue
-                if commands_port == "":
-                    commands_port = 8000
-                else:
-                    commands_port = int(commands_port)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid port.")
-                continue
-                                
-
-        osc_startup()
-
-        osc_udp_client(state_ip, state_port, "osc_client")
-
-        osc_udp_client(log_ip, log_port, "osc_log_client")
-
-        osc_udp_server('0.0.0.0', commands_port, "osc_server")
-
-        # Register OSC handler
-        osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_ADDRESS+osm.OSCARG_DATAUNPACK)
-        osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK)
-        print('--' * 50)
-        while True:
-            try:
-                print('+-' * 50)
-                self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ")
-                if self.hz == "":
-                    self.hz = 100
-                else:
-                    self.hz = float(self.hz)
-                break
-            except ValueError:
-                print("Invalid input. Please enter a valid number.")
-                continue
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Sending joint states to {state_ip}:{state_port}')
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Sending log messages to {log_ip}:{log_port}')
-        print()
-        print('=-=' * 50)
-        print()
-        print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
-        print()
-        print('=-=' * 50)
-        print()
-
-        self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}")
-        self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
-        self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
-        self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
-        self.create_timer(1/self.hz, self.update_position)  # Timer to update the position
-        self.create_timer(3, self.reset_prev)  # reset the previous desired position
-
-    def reset_prev(self): self.previous_desired = None
-
-    def speed_scaling_handler(self, *args):
-        """Handles incoming OSC messages for speed scaling."""
-        try:
-            if len(args) == 1:
-                if args[0] < 0:
-                    self.speed_scaling = -float(args[0])
-                else:
-                    self.speed_scaling = float(args[0])
-                if self.speed_scaling > 1:
-                    self.get_logger().warn(f"speed_scaling_handler: Attention! Speed scaling {self.speed_scaling} is greater than 1!")
-                self.get_logger().info(f"Speed scaling set to {self.speed_scaling}")
-            else:
-                self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.")
-        except Exception as e:
-            self.get_logger().fatal(f"speed_scaling_handler: {e}")
-    
-    def joint_trajectory_handler(self, *args):
-        try:
-                if len(args[0]) == 6:
-                    points = [[float(j) for j in i] for i in args]
-                elif len(args[0]) >= 7:
-                    points = [[float(j) for j in i[:6]] for i in args]
-                    self.get_logger().warn(f"joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
-                else:
-                    self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.")
-                    return
-
-                self.desired = ["joint_trajectory"] + points
-                self.new = True
-        except Exception as e:
-            self.get_logger().fatal(f"joint_trajectory_handler: {e}")
-
-    def joint_position_handler(self, address, *args):
-        """Handles incoming OSC messages for joint positions."""
-        try:
-            joint_name = address.split("/")[-1]
-            
-            if joint_name in self.joint_names:
-                if len(args) == 1:
-                    position = float(args[0])
-                    index = self.joint_names.index(joint_name)
-                    if self.robot:
-                        if position < self.robot.qlim[0][index]:
-                            position = self.robot.qlim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
-                            )
-                        elif position > self.robot.qlim[1][index]:
-                            position = self.robot.qlim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
-                            )
-                    else:
-                        if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
-                            position = self.joint_lim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
-                            )
-                        elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
-                            position = self.joint_lim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
-                            )
-                    desired_joint_positions = self.current_joint_positions
-                    desired_joint_positions[index] = position
-                    self.desired = ["joint_positions"] + desired_joint_positions
-                    self.new = True
-                elif len(args) == 2:
-                    position = float(args[0])
-                    duration = float(args[1])
-                    index = self.joint_names.index(joint_name)
-                    if self.robot:
-                        if position < self.robot.qlim[0][index]:
-                            position = self.robot.qlim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}."
-                            )
-                        elif position > self.robot.qlim[1][index]:
-                            position = self.robot.qlim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}."
-                            )
-                    else:
-                        if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]:
-                            position = self.joint_lim[0][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}."
-                            )
-                        elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]:
-                            position = self.joint_lim[1][index]
-                            self.get_logger().warn(
-                                f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}."
-                            )
-                    desired_joint_positions = self.current_joint_positions
-                    desired_joint_positions[index] = position
-                    self.desired = ["joint_positions"] + desired_joint_positions + [duration]
-                    self.new = True
-                else:
-                    self.get_logger().warn(f"joint_position_handler: Invalid number of arguments for joint position. Expected 1, but got {len(args)}.")
-            else:
-                self.get_logger().warn(f"joint_position_handler: Joint '{joint_name}' not found in the robot model.")
-        except Exception as e:
-            self.get_logger().fatal(f"joint_position_handler: {e}")
-        
-
-    def cartesian_trajectory_handler(self, *args):
-        """Handles incoming OSC messages for cartesian trajectory."""
-        if self.robot:
-            try:
-                if len(args[0]) == 6:
-                    points = [[float(j) for j in i] for i in args]
-                elif len(args[0]) >= 7:
-                    points = [[float(j) for j in i[:6]] for i in args]
-                    self.get_logger().warn(f"cartesian_trajectory_handler: Duration is not supported for cartesian trajectory yet. Ignoring duration.")
-                else:
-                    self.get_logger().warn(f"cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.")
-                    return
-
-                self.desired = ["cartesian_trajectory"] + points
-                self.new = True
-            except Exception as e:
-                self.get_logger().fatal(f"cartesian_trajectory_handler: {e}")
-        else:
-            self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.")
-            return
-
-    def joint_positions_handler(self, *args):
-        """Handles incoming OSC messages for joint positions."""
-        try:
-            if len(args) == len(self.joint_names):
-                desired_joint_positions = [float(i) for i in list(args)]
-            elif len(args) == len(self.joint_names) + 1:
-                desired_joint_positions = [float(i) for i in list(args)]
-            else:
-                self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.")
-                return
-
-            # Check if joint positions exceed limits
-            if self.robot:
-                for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):  # Exclude duration if present
-                    if position < self.robot.qlim[0][i]:
-                        desired_joint_positions[i] = self.robot.qlim[0][i]
-                        self.get_logger().warn(
-                            f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}."
-                        )
-                    elif position > self.robot.qlim[1][i]:
-                        desired_joint_positions[i] = self.robot.qlim[1][i]
-                        self.get_logger().warn(
-                            f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}."
-                        )
-            else:
-                if self.joint_lim:
-                    for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]):
-                        if self.joint_lim[0][i] is not None:
-                            if position < self.joint_lim[0][i]:
-                                desired_joint_positions[i] = self.joint_lim[0][i]
-                                self.get_logger().warn(
-                                    f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}."
-                                )
-                        elif self.joint_lim[1][i] is not None:
-                            if position > self.joint_lim[1][i]:
-                                desired_joint_positions[i] = self.joint_lim[1][i]
-                                self.get_logger().warn(
-                                    f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}."
-                                )
-
-            self.desired = ["joint_positions"] + desired_joint_positions
-            self.new = True
-        except Exception as e:
-            self.get_logger().fatal(f"joint_positions_handler: {e}")
-            return
-
-    def tcp_coordinates_handler(self, *args):
-    # Ensure the desired joint positions are within the specified limits
-        if self.robot:
-            try:
-                if len(args) == 6:
-                    x, y, z, r, p, yaw = [float(i) for i in list(args)]
-                    duration = None
-                elif len(args) >= 7:
-                    x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)]
-                else:
-                    self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.")
-                    return
-
-                if self.x_limits[0] is not None:
-                    x = max(self.x_limits[0], x)
-                if self.x_limits[1] is not None:
-                    x = min(self.x_limits[1], x)
-                if self.y_limits[0] is not None:
-                    y = max(self.y_limits[0], y)
-                if self.y_limits[1] is not None:
-                    y = min(self.y_limits[1], y)
-                if self.z_limits[0] is not None:
-                    z = max(self.z_limits[0], z)
-                if self.z_limits[1] is not None:
-                    z = min(self.z_limits[1], z)
-
-                if x != args[0] or y != args[1] or z != args[2]:
-                    self.get_logger().warn(
-                    f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: "
-                    f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})"
-                    )
-
-                self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration]
-                self.new = True
-            except Exception as e:
-                self.get_logger().fatal(f"tcp_coordinates_handler: {e}")
-        else:
-            self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.")
-            return
-            
-
-    def joint_states_callback(self, msg: JointState):
-        """Callback function to handle incoming joint states."""
-        try:
-            msg_time = oscbuildparse.OSCMessage(f"/time", ',s', [str(time.time())])
-            osc_send(msg_time, "osc_client")
-            if not(self.joint_names): self.joint_names = msg.name
-            joint_position_dict = dict(zip(msg.name, msg.position))
-            self.current_joint_positions = [float(joint_position_dict[name]) for name in self.joint_names]
-            joint_position_dict = dict(zip(msg.name, msg.velocity))
-            self.current_joint_velocities = [float(joint_position_dict[name]) for name in self.joint_names]
-
-            if self.robot:
-                tcp_position = self.robot.fkine(self.current_joint_positions).t
-                tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy()
-
-                msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]])
-                #msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]])
-                #msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]])
-                #msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]])
-                #msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]])
-                #msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]])
-                #msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]])
-                #bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw])
-                #osc_send(bun, "osc_client") 
-                osc_send(msg_tcp, "osc_client")
-            
-            msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
-            msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
-            msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
-            msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name])
-            bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort])
-            osc_send(bun, "osc_client")
-
-            #for i, name in enumerate(msg.name):
-            #    msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]])
-            #    msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]])
-            #    msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]])
-            #    bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort])
-            #    osc_send(bun, "osc_client")
-
-        except Exception as e:
-            self.get_logger().fatal(f"joint_states_callback: {e}")
-
-    def send_joint_positions(self):
-        self.previous_desired = None
-        self.new = False
-        try:
-            if len(self.desired) == len(self.joint_names) + 2:
-                desired_joint_positions = [float(i) for i in self.desired[1:-1]]
-                duration = self.desired[-1]
-                msg = JointTrajectory()
-                msg.joint_names = self.joint_names
-                point = JointTrajectoryPoint()
-                point.positions = desired_joint_positions
-                point.time_from_start.sec = int(duration)
-                point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                msg.points.append(point)
-                self.publisher.publish(msg)
-            elif len(self.desired) == len(self.joint_names) + 1:
-                desired_joint_positions = [float(i) for i in self.desired[1:]]
-                msg = JointTrajectory()
-                msg.joint_names = self.joint_names
-                point = JointTrajectoryPoint()
-                point.positions = desired_joint_positions
-                duration = 0
-                for p1, p2, max_vel in zip(desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()):
-                    duration = max(duration, abs(p1 - p2) / max_vel)
-                duration = duration + min(2, 0.2*duration)
-                duration /= self.speed_scaling
-                if duration == 0:
-                    self.get_logger().warn("send_joint_positions: Duration is 0.")
-                    return
-                point.time_from_start.sec = int(duration)
-                point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                msg.points.append(point)
-                self.publisher.publish(msg)
-            else: 
-                self.get_logger().warn(f"send_joint_positions: Invalid number of arguments for joint positions. Expected {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]) or {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]), but got {len(self.desired)}.")
-                return
-        except Exception as e:
-            self.get_logger().fatal(f"send_joint_positions: {e}")
-            return
-
-    def trapezoidal_timestamps(self, num_points,total_duration,  flat_ratio = 0.3):
-        
-        if num_points == 2:
-            return [0, total_duration]
-        n = int(num_points*(1-flat_ratio)/2)
-        start = np.cos(np.linspace(0, np.pi, n))+2
-        end = np.cos(np.linspace(-np.pi, 0, n))+2
-        flat = np.ones(num_points-2*n)
-
-        timestamps = np.concatenate((start, flat, end))
-        timestamps *= total_duration / timestamps.sum()
-        timestamps = np.cumsum(timestamps)
-
-        return timestamps.tolist()
-
-
-    def send_tcp_coordinates(self):
-        """Send the desired TCP coordinates to the robot."""
-        try:
-            self.new = False
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            steps_per_m = 100
-            if self.previous_desired == None: 
-                [x,y,z] = self.robot.fkine(self.current_joint_positions).t
-                [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
-            else:
-                [x,y,z] = self.previous_desired[1:4]
-                [roll, pitch, yaw] = self.previous_desired[4:-1]
-            x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
-            self.previous_desired = self.desired
-            steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
-            if steps < 2: steps = 2
-            cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
-            '''if self.previous_desired:
-                [x,y,z] = self.previous_desired[1:4]
-                q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])        
-            else: 
-                [x, y, z] = self.robot.fkine(self.current_joint_positions).t
-                q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
-
-            x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
-            q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
-
-            steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
-            if steps < 2:
-                steps = 2
-            
-            cart_traj = []
-            for i in range(steps):
-                alpha = i / (steps - 1)
-
-                # Convert to arrays for robust interpolation if needed
-                q0_array = q0.vec
-                q1_array = q1.vec
-                dot = np.dot(q0_array, q1_array)
-                dot = np.clip(dot, -1.0, 1.0)
-
-                if abs(dot) > 0.9995:
-                    # Linear interpolation + normalization
-                    q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
-                    q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
-                    q_interp = sm.UnitQuaternion(q_interp_array)
-                else:
-                    q_interp = q0.interp(q1, alpha)
-
-                # Interpolate translation
-                pos_interp = [
-                    x + (x1 - x) * alpha,
-                    y + (y1 - y) * alpha,
-                    z + (z1 - z) * alpha
-                ]
-
-                # Compose SE3 transform
-                cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
-
-
-            
-            if self.desired[-1]:
-                timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
-                for j in range(steps):
-                    sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
-                    if sol[1] == 1:
-                        fowards = self.robot.fkine_all(sol[0])
-                        out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
-                        if np.any(out_of_bounds):
-                            #print(fowards.t)
-                            #indices = np.where(out_of_bounds)[0]
-                            #print(f"indices: {indices}")
-                            self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
-                            '''
-                            for i in indices:
-                                try:
-                                    print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
-                                except IndexError:
-                                    print(f"index {i} is out of bounds, but no corresponding joint found.")
-                            self.previous_desired_tcp_position = self.desired_tcp_position
-                            '''
-                            break
-                        duration = timestamps[j]
-                        if duration == 0:
-                            prev_sol = list(sol[0])
-                            continue
-                        point = JointTrajectoryPoint()
-                        point.positions = list(sol[0])
-                        point.time_from_start.sec = int(duration)
-                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                        msg.points.append(point)
-                        prev_sol = list(sol[0])
-                    else:
-                        self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
-                        prev_sol = self.current_joint_positions
-                if len(msg.points) == 0:
-                    self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
-                    self.previous_desired = self.desired    
-                    return
-                msg.header.stamp = self.get_clock().now().to_msg()
-                self.publisher.publish(msg)
-                self.previous_desired = self.desired
-
-
-
-            else:
-                prev_duration = 0
-                '''
-                if self.previous_desired == None: 
-                    [x,y,z] = self.robot.fkine(self.current_joint_positions).t
-                    [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
-                else:
-                    [x,y,z] = self.previous_desired[:3]
-                    [roll, pitch, yaw] = self.previous_desired[3:]
-                '''
-                for j in range(steps):
-                    sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
-                    if sol[1] == 1:
-                        fowards = self.robot.fkine_all(sol[0])
-                        out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False)
-                        if np.any(out_of_bounds):
-                            #print(fowards.t)
-                            #indices = np.where(out_of_bounds)[0]
-                            #print(f"indices: {indices}")
-                            self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
-                            '''
-                            for i in indices:
-                                try:
-                                    print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
-                                except IndexError:
-                                    print(f"index {i} is out of bounds, but no corresponding joint found.")
-                            self.previous_desired_tcp_position = self.desired_tcp_position
-                            '''
-                            break
-                        duration = 0
-                        prev = self.current_joint_positions if j == 0 else prev_sol
-                        for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
-                            duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
-                        prev_sol = list(sol[0])
-                        if duration == 0:
-                            continue
-                        point = JointTrajectoryPoint()
-                        point.positions = list(sol[0])
-                        duration *= 1.2
-                        duration /= self.speed_scaling
-                        duration += prev_duration
-                        prev_duration = duration
-                        point.time_from_start.sec = int(duration)
-                        point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
-                        msg.points.append(point)
-                    else:
-                        self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
-                        prev_sol = self.current_joint_positions
-                if len(msg.points) == 0:
-                    self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
-                    self.previous_desired = self.desired    
-                    return
-                msg.header.stamp = self.get_clock().now().to_msg()
-                self.publisher.publish(msg)
-                self.previous_desired = self.desired
-        except Exception as e:
-            self.get_logger().fatal(f"send_tcp_coordinates: {e}")
-
-    def send_joint_trajectory(self):
-        
-        try:
-            self.new = False
-            viapoints = np.array([i for i in self.desired[1:]])
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            traj = rtb.mstraj(viapoints, q0 = self.current_joint_positions ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=[self.speed_scaling*i for i in self.joint_velocity_limits.values()])
-            msg.points = []
-            for i in range(len(traj.q)):
-                    point = JointTrajectoryPoint()
-                    point.positions = list(traj.q[i])
-                    point.time_from_start.sec = int(traj.t[i])
-                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
-                    msg.points.append(point)
-            msg.header.stamp = self.get_clock().now().to_msg()
-            self.publisher.publish(msg)
-            self.previous_desired = None
-        except Exception as e:
-            print(f'Error in joint_angles_handler: {e}')
-
-    def send_cartesian_trajectory(self):
-        
-        try:
-            self.new = False
-            viapoints = np.array([i[:6] for i in self.desired[1:]])
-            msg = JointTrajectory()
-            msg.joint_names = self.joint_names
-            x,y,z = self.robot.fkine(self.current_joint_positions).t
-            r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy()
-            q0 = [x, y, z, r, p, yaw]
-            traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=2*self.speed_scaling)
-            msg.points = []
-            prev_sol = self.current_joint_positions
-            n = max(int(len(traj.q)/200), 1)
-            for i in range(len(traj.q)):
-                T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz')
-                sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True)
-                if sol[1] == 1:
-                    point = JointTrajectoryPoint()
-                    point.positions = list(sol[0])
-                    point.time_from_start.sec = int(traj.t[i])
-                    if traj.t[i] == 0:
-                        continue
-                    point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9)
-                    msg.points.append(point)
-                    prev_sol = list(sol[0])
-                else: self.get_logger().warn(f"send_cartesian_trajectory: IK could not find a solution for (x,y,z) = {traj.q[i][:3]} and (r,p,y) = {traj.q[i][3:]}!")
-            msg.header.stamp = self.get_clock().now().to_msg()
-            msg.points = msg.points[::n]
-            self.publisher.publish(msg)
-            self.previous_desired = None
-        except Exception as e:
-            print(f'Error in joint_angles_handler: {e}')
-
-        self.previous_desired = None
-
-    def update_position(self):
-        """Calls the appropriate function to update the robot's position."""
-        try:
-            if self.desired is None or not(self.new):
-                return
-
-            if self.desired[0] == "joint_positions":
-                self.new = False
-                self.send_joint_positions()
-                return
-            elif self.desired[0] == "tcp_coordinates":
-                self.new = False
-                self.send_tcp_coordinates()
-                return
-            elif self.desired[0] == "joint_trajectory":
-                self.new = False
-                self.send_joint_trajectory()
-                return
-            elif self.desired[0] == "cartesian_trajectory":
-                self.new = False
-                self.send_cartesian_trajectory()
-                return
-            else:
-                self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
-                return
-
-
-        except Exception as e:
-            self.get_logger().fatal(f'update_position: {e}')
-                
-
-    def clean_log_string(self, s):
-
-        s = str(s)
-
-        # Remove ANSI escape sequences (e.g., \x1b[31m)
-        ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
-        s = ansi_escape.sub('', s)
-
-        # Replace tabs/newlines with spaces
-        s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
-
-        # Strip leading/trailing whitespace
-        s = s.strip()
-
-        # Optionally enforce ASCII only (replace non-ASCII chars with '?')
-        s = s.encode('ascii', 'replace').decode('ascii')
-
-        return s
-
-    
-    def log_callback(self, msg: Log):
-        """Callback function to handle incoming log messages."""
-
-        # Send the log message as an OSC message
-        msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
-        osc_send(msg_log, "osc_log_client")
-
-
-def main():
-    """Main function to get joint names and start the ROS 2 & OSC system."""
-    rclpy.init()
-    while True:
-        use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
-        if use_urdf == 'y':
-            while True:
-                robot_urdf = input("Enter the absolute path to the URDF file: ")
-                if os.path.isfile(robot_urdf):
-                    if not robot_urdf.endswith('.urdf'):
-                        print("The file is not a URDF file. Please enter a valid URDF file.")
-                        continue
-                    break
-                else:
-                    print("Invalid path. Please enter a valid path to the URDF file.")
-            tree = ET.parse(robot_urdf)
-            root = tree.getroot()
-            robot = rtb.ERobot.URDF(robot_urdf)
-            joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
-            print(robot)
-            joint_velocity_limits = {}
-
-            # Iterate over all joints in the URDF
-            for joint in root.findall('.//joint'):
-                joint_name = joint.get('name')  # Get the name of the joint
-                
-                # Look for the <limit> tag under each joint
-                limit = joint.find('limit')
-                
-                if limit is not None:
-                    # Extract the velocity limit (if it exists)
-                    velocity_limit = limit.get('velocity')
-                    
-                    if velocity_limit is not None:
-                        joint_velocity_limits[joint_name] = float(velocity_limit)
-            
-            while True:
-                try:
-                    print('-+'*50)
-                    print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
-                    print("The cost mask 111000 means that the IK will only consider translation and no rotaion.")
-                    cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")]
-                    if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
-                        break
-                    else:
-                        print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.")
-                except ValueError:
-                    print("Invalid input. Please enter integers only.")
-            print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}")
-            break
-        elif use_urdf == 'n':
-            node = JointNameListener()
-            print("Wainting 10 sec for JointState messages to extract joint names...")
-            rclpy.spin_once(node)
-            counter = 0
-            while not(node.joint_names):
-                if counter > 100:
-                    joint_names = None
-                    break
-                counter+=1
-                time.sleep(0.1)
-                rclpy.spin_once(node)
-            joint_names = node.joint_names
-            node.destroy_node()
-            if joint_names:
-                correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower()
-                while True:
-                    if correct_names == 'y':
-                        break
-                    elif correct_names == 'n':
-                        joint_names = None
-                        break
-                    correct_names = input("Invalid input. Please enter 'y' or 'n'.")
-            if not(joint_names):
-                print("Please enter the joint names manually.")
-                while True:
-                        joint_names = []
-                        print('-+'*50)
-                        print("Enter the joint names manually one by one. Type 'done' when you are finished:")
-                        print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.")
-                        while True:
-                            print('-'*50)
-                            joint_name = input("Enter joint name (or 'done' to finish): ").strip()
-                            if joint_name.lower() == 'done':
-                                break
-                            if joint_name:
-                                joint_names.append(joint_name)
-                        print('-+'*50)
-                        correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
-                        if correct_names.lower() == 'y':
-                            break
-                        else:
-                            print("Please re-enter the joint names.")
-            while True:
-                try:
-                    joint_velocity_limits = {}
-                    for name in joint_names:
-                        while True:
-                            try:
-                                print('--'*50)
-                                limit = input(f"Enter the velocity limit for joint '{name}': ").strip()
-                                if limit == "":
-                                    continue
-                                else:
-                                    joint_velocity_limits[name] = float(limit)
-                                    break
-                            except ValueError:
-                                print("Invalid input. Please enter a numeric value or press Enter to skip.")
-                    break
-                except ValueError:
-                    print("Invalid input. Please enter numeric values or leave blank to skip.")
-            robot = None
-            cost_mask = None
-            break
-        print("Invalid input. Please enter 'y' or 'n'.")
-
-
-    node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask)
-    
-    # Run ROS 2 spin, and osc_process will be handled by the timer
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        print("")
-    finally:
-        node.destroy_node()
-        osc_terminate()
-
-if __name__ == '__main__':
-    main()
diff --git a/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 b/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 b/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2
deleted file mode 100644
index aff3120..0000000
--- a/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2
+++ /dev/null
@@ -1 +0,0 @@
-rclpy
\ No newline at end of file
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv
deleted file mode 100644
index 79d4c95..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv
+++ /dev/null
@@ -1 +0,0 @@
-prepend-non-duplicate;AMENT_PREFIX_PATH;
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1
deleted file mode 100644
index 26b9997..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
-
-colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh
deleted file mode 100644
index f3041f6..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_core/shell/template/hook_prepend_value.sh.em
-
-_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv
deleted file mode 100644
index 257067d..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv
+++ /dev/null
@@ -1 +0,0 @@
-prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1
deleted file mode 100644
index caffe83..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
-
-colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh
deleted file mode 100644
index 660c348..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-# generated from colcon_core/shell/template/hook_prepend_value.sh.em
-
-_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.bash b/workspace/install/osc_ros2/share/osc_ros2/package.bash
deleted file mode 100644
index 92f5b25..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.bash
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_bash/shell/template/package.bash.em
-
-# This script extends the environment for this package.
-
-# a bash script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  # the prefix is two levels up from the package specific share directory
-  _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
-else
-  _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_bash_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$@"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# source sh script of this package
-_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/osc_ros2/package.sh"
-
-unset _colcon_package_bash_source_script
-unset _colcon_package_bash_COLCON_CURRENT_PREFIX
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.dsv b/workspace/install/osc_ros2/share/osc_ros2/package.dsv
deleted file mode 100644
index ece85b1..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.dsv
+++ /dev/null
@@ -1,6 +0,0 @@
-source;share/osc_ros2/hook/pythonpath.ps1
-source;share/osc_ros2/hook/pythonpath.dsv
-source;share/osc_ros2/hook/pythonpath.sh
-source;share/osc_ros2/hook/ament_prefix_path.ps1
-source;share/osc_ros2/hook/ament_prefix_path.dsv
-source;share/osc_ros2/hook/ament_prefix_path.sh
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.ps1 b/workspace/install/osc_ros2/share/osc_ros2/package.ps1
deleted file mode 100644
index ed891bc..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.ps1
+++ /dev/null
@@ -1,116 +0,0 @@
-# generated from colcon_powershell/shell/template/package.ps1.em
-
-# function to append a value to a variable
-# which uses colons as separators
-# duplicates as well as leading separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-function colcon_append_unique_value {
-  param (
-    $_listname,
-    $_value
-  )
-
-  # get values from variable
-  if (Test-Path Env:$_listname) {
-    $_values=(Get-Item env:$_listname).Value
-  } else {
-    $_values=""
-  }
-  $_duplicate=""
-  # start with no values
-  $_all_values=""
-  # iterate over existing values in the variable
-  if ($_values) {
-    $_values.Split(";") | ForEach {
-      # not an empty string
-      if ($_) {
-        # not a duplicate of _value
-        if ($_ -eq $_value) {
-          $_duplicate="1"
-        }
-        if ($_all_values) {
-          $_all_values="${_all_values};$_"
-        } else {
-          $_all_values="$_"
-        }
-      }
-    }
-  }
-  # append only non-duplicates
-  if (!$_duplicate) {
-    # avoid leading separator
-    if ($_all_values) {
-      $_all_values="${_all_values};${_value}"
-    } else {
-      $_all_values="${_value}"
-    }
-  }
-
-  # export the updated variable
-  Set-Item env:\$_listname -Value "$_all_values"
-}
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-function colcon_prepend_unique_value {
-  param (
-    $_listname,
-    $_value
-  )
-
-  # get values from variable
-  if (Test-Path Env:$_listname) {
-    $_values=(Get-Item env:$_listname).Value
-  } else {
-    $_values=""
-  }
-  # start with the new value
-  $_all_values="$_value"
-  # iterate over existing values in the variable
-  if ($_values) {
-    $_values.Split(";") | ForEach {
-      # not an empty string
-      if ($_) {
-        # not a duplicate of _value
-        if ($_ -ne $_value) {
-          # keep non-duplicate values
-          $_all_values="${_all_values};$_"
-        }
-      }
-    }
-  }
-  # export the updated variable
-  Set-Item env:\$_listname -Value "$_all_values"
-}
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-function colcon_package_source_powershell_script {
-  param (
-    $_colcon_package_source_powershell_script
-  )
-  # source script with conditional trace output
-  if (Test-Path $_colcon_package_source_powershell_script) {
-    if ($env:COLCON_TRACE) {
-      echo ". '$_colcon_package_source_powershell_script'"
-    }
-    . "$_colcon_package_source_powershell_script"
-  } else {
-    Write-Error "not found: '$_colcon_package_source_powershell_script'"
-  }
-}
-
-
-# a powershell script is able to determine its own path
-# the prefix is two levels up from the package specific share directory
-$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
-
-colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/osc_ros2/hook/pythonpath.ps1"
-colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/osc_ros2/hook/ament_prefix_path.ps1"
-
-Remove-Item Env:\COLCON_CURRENT_PREFIX
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.sh b/workspace/install/osc_ros2/share/osc_ros2/package.sh
deleted file mode 100644
index c4c25c5..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.sh
+++ /dev/null
@@ -1,87 +0,0 @@
-# generated from colcon_core/shell/template/package.sh.em
-
-# This script extends the environment for this package.
-
-# function to prepend a value to a variable
-# which uses colons as separators
-# duplicates as well as trailing separators are avoided
-# first argument: the name of the result variable
-# second argument: the value to be prepended
-_colcon_prepend_unique_value() {
-  # arguments
-  _listname="$1"
-  _value="$2"
-
-  # get values from variable
-  eval _values=\"\$$_listname\"
-  # backup the field separator
-  _colcon_prepend_unique_value_IFS=$IFS
-  IFS=":"
-  # start with the new value
-  _all_values="$_value"
-  # workaround SH_WORD_SPLIT not being set in zsh
-  if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
-    colcon_zsh_convert_to_array _values
-  fi
-  # iterate over existing values in the variable
-  for _item in $_values; do
-    # ignore empty strings
-    if [ -z "$_item" ]; then
-      continue
-    fi
-    # ignore duplicates of _value
-    if [ "$_item" = "$_value" ]; then
-      continue
-    fi
-    # keep non-duplicate values
-    _all_values="$_all_values:$_item"
-  done
-  unset _item
-  # restore the field separator
-  IFS=$_colcon_prepend_unique_value_IFS
-  unset _colcon_prepend_unique_value_IFS
-  # export the updated variable
-  eval export $_listname=\"$_all_values\"
-  unset _all_values
-  unset _values
-
-  unset _value
-  unset _listname
-}
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_package_sh_COLCON_CURRENT_PREFIX="/workspace/install/osc_ros2"
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
-    echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
-    unset _colcon_package_sh_COLCON_CURRENT_PREFIX
-    return 1
-  fi
-  COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
-fi
-unset _colcon_package_sh_COLCON_CURRENT_PREFIX
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_sh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$@"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# source sh hooks
-_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/osc_ros2/hook/pythonpath.sh"
-_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/osc_ros2/hook/ament_prefix_path.sh"
-
-unset _colcon_package_sh_source_script
-unset COLCON_CURRENT_PREFIX
-
-# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.xml b/workspace/install/osc_ros2/share/osc_ros2/package.xml
deleted file mode 100644
index 086e035..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>osc_ros2</name>
-  <version>1.0.0</version>
-  <description>Creates an interface for communication between OSC and ROS2</description>
-  <maintainer email="a.schaefer@tuhh.de">Alexander Schaefer</maintainer>
-  <license>Apache-2.0</license>
-
-  <depend>rclpy</depend>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.zsh b/workspace/install/osc_ros2/share/osc_ros2/package.zsh
deleted file mode 100644
index b8ae59f..0000000
--- a/workspace/install/osc_ros2/share/osc_ros2/package.zsh
+++ /dev/null
@@ -1,42 +0,0 @@
-# generated from colcon_zsh/shell/template/package.zsh.em
-
-# This script extends the environment for this package.
-
-# a zsh script is able to determine its own path if necessary
-if [ -z "$COLCON_CURRENT_PREFIX" ]; then
-  # the prefix is two levels up from the package specific share directory
-  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
-else
-  _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-# additional arguments: arguments to the script
-_colcon_package_zsh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$@"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# function to convert array-like strings into arrays
-# to workaround SH_WORD_SPLIT not being set
-colcon_zsh_convert_to_array() {
-  local _listname=$1
-  local _dollar="$"
-  local _split="{="
-  local _to_array="(\"$_dollar$_split$_listname}\")"
-  eval $_listname=$_to_array
-}
-
-# source sh script of this package
-_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/osc_ros2/package.sh"
-unset convert_zsh_to_array
-
-unset _colcon_package_zsh_source_script
-unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
diff --git a/workspace/install/setup.bash b/workspace/install/setup.bash
deleted file mode 100644
index 10ea0f7..0000000
--- a/workspace/install/setup.bash
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_bash/shell/template/prefix_chain.bash.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_bash_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
-_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
-
-unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_bash_source_script
diff --git a/workspace/install/setup.ps1 b/workspace/install/setup.ps1
deleted file mode 100644
index 558e9b9..0000000
--- a/workspace/install/setup.ps1
+++ /dev/null
@@ -1,29 +0,0 @@
-# generated from colcon_powershell/shell/template/prefix_chain.ps1.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-function _colcon_prefix_chain_powershell_source_script {
-  param (
-    $_colcon_prefix_chain_powershell_source_script_param
-  )
-  # source script with conditional trace output
-  if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) {
-    if ($env:COLCON_TRACE) {
-      echo ". '$_colcon_prefix_chain_powershell_source_script_param'"
-    }
-    . "$_colcon_prefix_chain_powershell_source_script_param"
-  } else {
-    Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'"
-  }
-}
-
-# source chained prefixes
-_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
-
-# source this prefix
-$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
-_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"
diff --git a/workspace/install/setup.sh b/workspace/install/setup.sh
deleted file mode 100644
index 70a84aa..0000000
--- a/workspace/install/setup.sh
+++ /dev/null
@@ -1,45 +0,0 @@
-# generated from colcon_core/shell/template/prefix_chain.sh.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# since a plain shell script can't determine its own path when being sourced
-# either use the provided COLCON_CURRENT_PREFIX
-# or fall back to the build time prefix (if it exists)
-_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/workspace/install
-if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
-  _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
-elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
-  echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
-  unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
-  return 1
-fi
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_sh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
-
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
-COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"
-_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
-
-unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_sh_source_script
-unset COLCON_CURRENT_PREFIX
diff --git a/workspace/install/setup.zsh b/workspace/install/setup.zsh
deleted file mode 100644
index 54799fd..0000000
--- a/workspace/install/setup.zsh
+++ /dev/null
@@ -1,31 +0,0 @@
-# generated from colcon_zsh/shell/template/prefix_chain.zsh.em
-
-# This script extends the environment with the environment of other prefix
-# paths which were sourced when this file was generated as well as all packages
-# contained in this prefix path.
-
-# function to source another script with conditional trace output
-# first argument: the path of the script
-_colcon_prefix_chain_zsh_source_script() {
-  if [ -f "$1" ]; then
-    if [ -n "$COLCON_TRACE" ]; then
-      echo "# . \"$1\""
-    fi
-    . "$1"
-  else
-    echo "not found: \"$1\"" 1>&2
-  fi
-}
-
-# source chained prefixes
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="/opt/ros/humble"
-_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
-
-# source this prefix
-# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
-COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
-_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
-
-unset COLCON_CURRENT_PREFIX
-unset _colcon_prefix_chain_zsh_source_script
diff --git a/workspace/log/COLCON_IGNORE b/workspace/log/COLCON_IGNORE
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/log/build_2025-05-28_04-13-20/events.log b/workspace/log/build_2025-05-28_04-13-20/events.log
deleted file mode 100644
index 90db505..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/events.log
+++ /dev/null
@@ -1,46 +0,0 @@
-[0.000000] (-) TimerEvent: {}
-[0.000225] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()}
-[0.000348] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'}
-[0.099483] (-) TimerEvent: {}
-[0.199628] (-) TimerEvent: {}
-[0.299775] (-) TimerEvent: {}
-[0.305620] (osc_ros2) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'egg_info', '--egg-base', '../../build/osc_ros2', 'build', '--build-base', '/workspace/build/osc_ros2/build', 'install', '--record', '/workspace/build/osc_ros2/install.log', '--single-version-externally-managed', 'install_data'], 'cwd': '/workspace/src/osc_ros2', 'env': {'HOSTNAME': 'ad72f3440b4e', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/', 'ROS_PYTHON_VERSION': '3', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', 'LS_COLORS': 'rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'PWD': '/workspace/build/osc_ros2', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages', 'COLCON': '1'}, 'shell': False}
-[0.399853] (-) TimerEvent: {}
-[0.419376] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'}
-[0.419581] (osc_ros2) StdoutLine: {'line': b'creating ../../build/osc_ros2/osc_ros2.egg-info\n'}
-[0.419665] (osc_ros2) StdoutLine: {'line': b'writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO\n'}
-[0.419773] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt\n'}
-[0.419835] (osc_ros2) StdoutLine: {'line': b'writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt\n'}
-[0.419888] (osc_ros2) StdoutLine: {'line': b'writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt\n'}
-[0.419941] (osc_ros2) StdoutLine: {'line': b'writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt\n'}
-[0.420009] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"}
-[0.420756] (osc_ros2) StdoutLine: {'line': b"reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"}
-[0.421180] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"}
-[0.421245] (osc_ros2) StdoutLine: {'line': b'running build\n'}
-[0.421312] (osc_ros2) StdoutLine: {'line': b'running build_py\n'}
-[0.421406] (osc_ros2) StdoutLine: {'line': b'creating /workspace/build/osc_ros2/build\n'}
-[0.421504] (osc_ros2) StdoutLine: {'line': b'creating /workspace/build/osc_ros2/build/lib\n'}
-[0.421594] (osc_ros2) StdoutLine: {'line': b'creating /workspace/build/osc_ros2/build/lib/osc_ros2\n'}
-[0.421645] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/__init__.py -> /workspace/build/osc_ros2/build/lib/osc_ros2\n'}
-[0.421689] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/osc_ros2.py -> /workspace/build/osc_ros2/build/lib/osc_ros2\n'}
-[0.421741] (osc_ros2) StdoutLine: {'line': b'running install\n'}
-[0.421782] (osc_ros2) StdoutLine: {'line': b'running install_lib\n'}
-[0.422031] (osc_ros2) StdoutLine: {'line': b'creating /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'}
-[0.422093] (osc_ros2) StdoutLine: {'line': b'copying /workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'}
-[0.422151] (osc_ros2) StdoutLine: {'line': b'copying /workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'}
-[0.422417] (osc_ros2) StdoutLine: {'line': b'byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc\n'}
-[0.422543] (osc_ros2) StdoutLine: {'line': b'byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc\n'}
-[0.427108] (osc_ros2) StdoutLine: {'line': b'running install_data\n'}
-[0.427204] (osc_ros2) StdoutLine: {'line': b'creating /workspace/install/osc_ros2/share/ament_index\n'}
-[0.427261] (osc_ros2) StdoutLine: {'line': b'creating /workspace/install/osc_ros2/share/ament_index/resource_index\n'}
-[0.427316] (osc_ros2) StdoutLine: {'line': b'creating /workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'}
-[0.427366] (osc_ros2) StdoutLine: {'line': b'copying resource/osc_ros2 -> /workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'}
-[0.427407] (osc_ros2) StdoutLine: {'line': b'copying package.xml -> /workspace/install/osc_ros2/share/osc_ros2\n'}
-[0.427447] (osc_ros2) StdoutLine: {'line': b'running install_egg_info\n'}
-[0.428124] (osc_ros2) StdoutLine: {'line': b'Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info\n'}
-[0.428731] (osc_ros2) StdoutLine: {'line': b'running install_scripts\n'}
-[0.438155] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2\n'}
-[0.438306] (osc_ros2) StdoutLine: {'line': b"writing list of installed files to '/workspace/build/osc_ros2/install.log'\n"}
-[0.447607] (osc_ros2) CommandEnded: {'returncode': 0}
-[0.452862] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0}
-[0.453237] (-) EventReactorShutdown: {}
diff --git a/workspace/log/build_2025-05-28_04-13-20/logger_all.log b/workspace/log/build_2025-05-28_04-13-20/logger_all.log
deleted file mode 100644
index d49b047..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/logger_all.log
+++ /dev/null
@@ -1,126 +0,0 @@
-[0.070s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build']
-[0.070s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=16, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7fa9a7cd8610>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7fa9a7df14b0>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7fa9a7df14b0>>, mixin_verb=('build',))
-[0.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
-[0.150s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
-[0.151s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
-[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
-[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
-[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
-[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
-[0.151s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
-[0.151s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/workspace'
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
-[0.151s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
-[0.157s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta'
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros']
-[0.158s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros'
-[0.159s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2'
-[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
-[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
-[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
-[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
-[0.159s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None'
-[0.170s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False'
-[0.170s] DEBUG:colcon.colcon_core.verb:Building package 'osc_ros2' with the following arguments: {'ament_cmake_args': None, 'build_base': '/workspace/build/osc_ros2', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/workspace/install/osc_ros2', 'merge_install': False, 'path': '/workspace/src/osc_ros2', 'symlink_install': False, 'test_result_base': None}
-[0.170s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
-[0.171s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
-[0.171s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/workspace/src/osc_ros2' with build type 'ament_python'
-[0.171s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path')
-[0.172s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
-[0.172s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1'
-[0.173s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv'
-[0.173s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh'
-[0.173s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
-[0.173s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
-[0.299s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/workspace/src/osc_ros2'
-[0.299s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
-[0.299s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
-[0.477s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.618s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.620s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2' for CMake module files
-[0.620s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2' for CMake config files
-[0.620s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib'
-[0.620s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/bin'
-[0.620s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc'
-[0.621s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib/python3.10/site-packages'
-[0.621s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath')
-[0.621s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1'
-[0.621s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv'
-[0.621s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh'
-[0.621s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/bin'
-[0.621s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2)
-[0.622s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.ps1'
-[0.622s] INFO:colcon.colcon_core.shell:Creating package descriptor '/workspace/install/osc_ros2/share/osc_ros2/package.dsv'
-[0.622s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.sh'
-[0.623s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.bash'
-[0.623s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.zsh'
-[0.623s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2)
-[0.624s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
-[0.624s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
-[0.624s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
-[0.624s] DEBUG:colcon.colcon_core.event_reactor:joining thread
-[0.628s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
-[0.628s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
-[0.628s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
-[0.628s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
-[0.628s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.NotSupported: Unable to autolaunch a dbus-daemon without a $DISPLAY for X11
-[0.628s] DEBUG:colcon.colcon_core.event_reactor:joined thread
-[0.628s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.ps1'
-[0.629s] INFO:colcon.colcon_core.shell:Creating prefix util module '/workspace/install/_local_setup_util_ps1.py'
-[0.629s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.ps1'
-[0.630s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.sh'
-[0.630s] INFO:colcon.colcon_core.shell:Creating prefix util module '/workspace/install/_local_setup_util_sh.py'
-[0.630s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.sh'
-[0.631s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.bash'
-[0.631s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.bash'
-[0.632s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.zsh'
-[0.632s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.zsh'
diff --git a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/command.log b/workspace/log/build_2025-05-28_04-13-20/osc_ros2/command.log
deleted file mode 100644
index d2728af..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/command.log
+++ /dev/null
@@ -1,2 +0,0 @@
-Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
diff --git a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stderr.log b/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stderr.log
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout.log b/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout.log
deleted file mode 100644
index 5f28670..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout.log
+++ /dev/null
@@ -1,35 +0,0 @@
-running egg_info
-creating ../../build/osc_ros2/osc_ros2.egg-info
-writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-running build
-running build_py
-creating /workspace/build/osc_ros2/build
-creating /workspace/build/osc_ros2/build/lib
-creating /workspace/build/osc_ros2/build/lib/osc_ros2
-copying osc_ros2/__init__.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-copying osc_ros2/osc_ros2.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-running install
-running install_lib
-creating /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-copying /workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-copying /workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc
-byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc
-running install_data
-creating /workspace/install/osc_ros2/share/ament_index
-creating /workspace/install/osc_ros2/share/ament_index/resource_index
-creating /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-copying resource/osc_ros2 -> /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-copying package.xml -> /workspace/install/osc_ros2/share/osc_ros2
-running install_egg_info
-Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-running install_scripts
-Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-writing list of installed files to '/workspace/build/osc_ros2/install.log'
diff --git a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout_stderr.log b/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout_stderr.log
deleted file mode 100644
index 5f28670..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/stdout_stderr.log
+++ /dev/null
@@ -1,35 +0,0 @@
-running egg_info
-creating ../../build/osc_ros2/osc_ros2.egg-info
-writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-running build
-running build_py
-creating /workspace/build/osc_ros2/build
-creating /workspace/build/osc_ros2/build/lib
-creating /workspace/build/osc_ros2/build/lib/osc_ros2
-copying osc_ros2/__init__.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-copying osc_ros2/osc_ros2.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-running install
-running install_lib
-creating /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-copying /workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-copying /workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc
-byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc
-running install_data
-creating /workspace/install/osc_ros2/share/ament_index
-creating /workspace/install/osc_ros2/share/ament_index/resource_index
-creating /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-copying resource/osc_ros2 -> /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-copying package.xml -> /workspace/install/osc_ros2/share/osc_ros2
-running install_egg_info
-Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-running install_scripts
-Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-writing list of installed files to '/workspace/build/osc_ros2/install.log'
diff --git a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/streams.log b/workspace/log/build_2025-05-28_04-13-20/osc_ros2/streams.log
deleted file mode 100644
index 45d9ff9..0000000
--- a/workspace/log/build_2025-05-28_04-13-20/osc_ros2/streams.log
+++ /dev/null
@@ -1,37 +0,0 @@
-[0.306s] Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.419s] running egg_info
-[0.419s] creating ../../build/osc_ros2/osc_ros2.egg-info
-[0.419s] writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-[0.419s] writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-[0.420s] writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-[0.420s] writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-[0.420s] writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-[0.420s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-[0.420s] reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-[0.421s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-[0.421s] running build
-[0.421s] running build_py
-[0.421s] creating /workspace/build/osc_ros2/build
-[0.421s] creating /workspace/build/osc_ros2/build/lib
-[0.421s] creating /workspace/build/osc_ros2/build/lib/osc_ros2
-[0.421s] copying osc_ros2/__init__.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-[0.421s] copying osc_ros2/osc_ros2.py -> /workspace/build/osc_ros2/build/lib/osc_ros2
-[0.421s] running install
-[0.421s] running install_lib
-[0.422s] creating /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-[0.422s] copying /workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-[0.422s] copying /workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2
-[0.422s] byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc
-[0.422s] byte-compiling /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc
-[0.427s] running install_data
-[0.427s] creating /workspace/install/osc_ros2/share/ament_index
-[0.427s] creating /workspace/install/osc_ros2/share/ament_index/resource_index
-[0.427s] creating /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-[0.427s] copying resource/osc_ros2 -> /workspace/install/osc_ros2/share/ament_index/resource_index/packages
-[0.427s] copying package.xml -> /workspace/install/osc_ros2/share/osc_ros2
-[0.427s] running install_egg_info
-[0.428s] Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-[0.428s] running install_scripts
-[0.438s] Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-[0.438s] writing list of installed files to '/workspace/build/osc_ros2/install.log'
-[0.447s] Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
diff --git a/workspace/log/build_2025-05-28_04-16-52/events.log b/workspace/log/build_2025-05-28_04-16-52/events.log
deleted file mode 100644
index 0498981..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/events.log
+++ /dev/null
@@ -1,30 +0,0 @@
-[0.000000] (-) TimerEvent: {}
-[0.000210] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()}
-[0.000283] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'}
-[0.099571] (-) TimerEvent: {}
-[0.199703] (-) TimerEvent: {}
-[0.299829] (-) TimerEvent: {}
-[0.305183] (osc_ros2) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'egg_info', '--egg-base', '../../build/osc_ros2', 'build', '--build-base', '/workspace/build/osc_ros2/build', 'install', '--record', '/workspace/build/osc_ros2/install.log', '--single-version-externally-managed', 'install_data'], 'cwd': '/workspace/src/osc_ros2', 'env': {'HOSTNAME': 'ad72f3440b4e', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/', 'ROS_PYTHON_VERSION': '3', 'COLCON_PREFIX_PATH': '/workspace/install', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', 'LS_COLORS': 'rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:', 'AMENT_PREFIX_PATH': '/workspace/install/osc_ros2:/opt/ros/humble', 'PWD': '/workspace/build/osc_ros2', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:/workspace/install/osc_ros2/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages', 'COLCON': '1'}, 'shell': False}
-[0.399896] (-) TimerEvent: {}
-[0.423494] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'}
-[0.423758] (osc_ros2) StdoutLine: {'line': b'writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO\n'}
-[0.423871] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt\n'}
-[0.423940] (osc_ros2) StdoutLine: {'line': b'writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt\n'}
-[0.423995] (osc_ros2) StdoutLine: {'line': b'writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt\n'}
-[0.424050] (osc_ros2) StdoutLine: {'line': b'writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt\n'}
-[0.424816] (osc_ros2) StdoutLine: {'line': b"reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"}
-[0.425191] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"}
-[0.425256] (osc_ros2) StdoutLine: {'line': b'running build\n'}
-[0.425303] (osc_ros2) StdoutLine: {'line': b'running build_py\n'}
-[0.425468] (osc_ros2) StdoutLine: {'line': b'running install\n'}
-[0.425604] (osc_ros2) StdoutLine: {'line': b'running install_lib\n'}
-[0.426145] (osc_ros2) StdoutLine: {'line': b'running install_data\n'}
-[0.426206] (osc_ros2) StdoutLine: {'line': b'running install_egg_info\n'}
-[0.427040] (osc_ros2) StdoutLine: {'line': b"removing '/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info' (and everything under it)\n"}
-[0.427275] (osc_ros2) StdoutLine: {'line': b'Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info\n'}
-[0.427875] (osc_ros2) StdoutLine: {'line': b'running install_scripts\n'}
-[0.437908] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2\n'}
-[0.438055] (osc_ros2) StdoutLine: {'line': b"writing list of installed files to '/workspace/build/osc_ros2/install.log'\n"}
-[0.447364] (osc_ros2) CommandEnded: {'returncode': 0}
-[0.452905] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0}
-[0.453236] (-) EventReactorShutdown: {}
diff --git a/workspace/log/build_2025-05-28_04-16-52/logger_all.log b/workspace/log/build_2025-05-28_04-16-52/logger_all.log
deleted file mode 100644
index 25c5e99..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/logger_all.log
+++ /dev/null
@@ -1,126 +0,0 @@
-[0.061s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build']
-[0.061s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=16, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7efccc97c8e0>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7efcccb11780>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7efcccb11780>>, mixin_verb=('build',))
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
-[0.138s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
-[0.138s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
-[0.138s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
-[0.138s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/workspace'
-[0.138s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
-[0.138s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
-[0.139s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install']
-[0.145s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore'
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install'
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg']
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg'
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta']
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta'
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros']
-[0.146s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros'
-[0.147s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2'
-[0.147s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
-[0.147s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
-[0.147s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
-[0.147s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
-[0.147s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None'
-[0.157s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False'
-[0.157s] DEBUG:colcon.colcon_core.verb:Building package 'osc_ros2' with the following arguments: {'ament_cmake_args': None, 'build_base': '/workspace/build/osc_ros2', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/workspace/install/osc_ros2', 'merge_install': False, 'path': '/workspace/src/osc_ros2', 'symlink_install': False, 'test_result_base': None}
-[0.157s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
-[0.158s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
-[0.158s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/workspace/src/osc_ros2' with build type 'ament_python'
-[0.158s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path')
-[0.160s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
-[0.160s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1'
-[0.160s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv'
-[0.160s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh'
-[0.161s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
-[0.161s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
-[0.288s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/workspace/src/osc_ros2'
-[0.288s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
-[0.288s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
-[0.465s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.605s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.607s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2' for CMake module files
-[0.607s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2' for CMake config files
-[0.608s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib'
-[0.608s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/bin'
-[0.608s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc'
-[0.608s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/lib/python3.10/site-packages'
-[0.608s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath')
-[0.608s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1'
-[0.608s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv'
-[0.608s] INFO:colcon.colcon_core.shell:Creating environment hook '/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh'
-[0.609s] Level 1:colcon.colcon_core.environment:checking '/workspace/install/osc_ros2/bin'
-[0.609s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2)
-[0.609s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.ps1'
-[0.609s] INFO:colcon.colcon_core.shell:Creating package descriptor '/workspace/install/osc_ros2/share/osc_ros2/package.dsv'
-[0.610s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.sh'
-[0.610s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.bash'
-[0.610s] INFO:colcon.colcon_core.shell:Creating package script '/workspace/install/osc_ros2/share/osc_ros2/package.zsh'
-[0.611s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2)
-[0.611s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
-[0.611s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
-[0.611s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
-[0.611s] DEBUG:colcon.colcon_core.event_reactor:joining thread
-[0.614s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
-[0.614s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
-[0.614s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
-[0.614s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
-[0.614s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.NotSupported: Unable to autolaunch a dbus-daemon without a $DISPLAY for X11
-[0.614s] DEBUG:colcon.colcon_core.event_reactor:joined thread
-[0.615s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.ps1'
-[0.615s] INFO:colcon.colcon_core.shell:Creating prefix util module '/workspace/install/_local_setup_util_ps1.py'
-[0.616s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.ps1'
-[0.617s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.sh'
-[0.617s] INFO:colcon.colcon_core.shell:Creating prefix util module '/workspace/install/_local_setup_util_sh.py'
-[0.617s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.sh'
-[0.618s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.bash'
-[0.618s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.bash'
-[0.619s] INFO:colcon.colcon_core.shell:Creating prefix script '/workspace/install/local_setup.zsh'
-[0.619s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/workspace/install/setup.zsh'
diff --git a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/command.log b/workspace/log/build_2025-05-28_04-16-52/osc_ros2/command.log
deleted file mode 100644
index d2728af..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/command.log
+++ /dev/null
@@ -1,2 +0,0 @@
-Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
diff --git a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stderr.log b/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stderr.log
deleted file mode 100644
index e69de29..0000000
diff --git a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout.log b/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout.log
deleted file mode 100644
index c53ed50..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout.log
+++ /dev/null
@@ -1,19 +0,0 @@
-running egg_info
-writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-running build
-running build_py
-running install
-running install_lib
-running install_data
-running install_egg_info
-removing '/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info' (and everything under it)
-Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-running install_scripts
-Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-writing list of installed files to '/workspace/build/osc_ros2/install.log'
diff --git a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout_stderr.log b/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout_stderr.log
deleted file mode 100644
index c53ed50..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/stdout_stderr.log
+++ /dev/null
@@ -1,19 +0,0 @@
-running egg_info
-writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-running build
-running build_py
-running install
-running install_lib
-running install_data
-running install_egg_info
-removing '/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info' (and everything under it)
-Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-running install_scripts
-Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-writing list of installed files to '/workspace/build/osc_ros2/install.log'
diff --git a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/streams.log b/workspace/log/build_2025-05-28_04-16-52/osc_ros2/streams.log
deleted file mode 100644
index 68e1362..0000000
--- a/workspace/log/build_2025-05-28_04-16-52/osc_ros2/streams.log
+++ /dev/null
@@ -1,21 +0,0 @@
-[0.306s] Invoking command in '/workspace/src/osc_ros2': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
-[0.423s] running egg_info
-[0.423s] writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO
-[0.424s] writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt
-[0.424s] writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt
-[0.424s] writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt
-[0.424s] writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt
-[0.425s] reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-[0.425s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'
-[0.425s] running build
-[0.425s] running build_py
-[0.425s] running install
-[0.425s] running install_lib
-[0.426s] running install_data
-[0.426s] running install_egg_info
-[0.427s] removing '/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info' (and everything under it)
-[0.427s] Copying ../../build/osc_ros2/osc_ros2.egg-info to /workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info
-[0.428s] running install_scripts
-[0.438s] Installing interface script to /workspace/install/osc_ros2/lib/osc_ros2
-[0.438s] writing list of installed files to '/workspace/build/osc_ros2/install.log'
-[0.447s] Invoked command in '/workspace/src/osc_ros2' returned '0': PYTHONPATH=/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /workspace/build/osc_ros2/build install --record /workspace/build/osc_ros2/install.log --single-version-externally-managed install_data
diff --git a/workspace/log/latest b/workspace/log/latest
deleted file mode 120000
index b57d247..0000000
--- a/workspace/log/latest
+++ /dev/null
@@ -1 +0,0 @@
-latest_build
\ No newline at end of file
diff --git a/workspace/log/latest_build b/workspace/log/latest_build
deleted file mode 120000
index 77bb7f9..0000000
--- a/workspace/log/latest_build
+++ /dev/null
@@ -1 +0,0 @@
-build_2025-05-28_04-16-52
\ No newline at end of file