[AN] (feat) Interface with config fully works

examples/log.py

@@ -0,0 +1,19 @@
+import socket
+from ssl import SOCK_STREAM
+from oscpy import *
+
+UDP_IP = "172.18.0.1"
+UDP_PORT = 5005
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.bind((UDP_IP, UDP_PORT))
+
+while True:
+    data, addr = sock.recvfrom(10*1024)
+    if "#bundle" in data[:8].decode():
+        b = Bundle(data)
+        print(b)
+    else:
+        m = Message(data)
+        print(m)
+

examples/move.py

@@ -0,0 +1,45 @@
+import socket
+import time
+from oscpy import *
+
+
+SEND_IP = "172.18.0.2"
+SEND_PORT = 8000
+RECV_IP = "172.18.0.1"
+RECV_PORT = 7000
+
+pub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+msg = Message("/joint_positions", [-1]*6).encode()
+n = pub.sendto(msg, (SEND_IP, SEND_PORT))
+if n > 0:
+    print(f"INFO: Sent {n} bytes")
+
+time.sleep(10)
+
+msg = Message("/tcp_coordinates", [-1]*6).encode()
+n = pub.sendto(msg, (SEND_IP, SEND_PORT))
+if n > 0:
+    print(f"INFO: Sent {n} bytes")
+
+time.sleep(10)
+
+sub.bind((RECV_IP, RECV_PORT))
+joint_names = []
+found = False
+while not found:
+    data, addr = sub.recvfrom(5*1024)
+    if "#bundle" in data[:8].decode():
+        b = Bundle().decode(data)
+        for msg in b.msgs:
+            if "/joint_state/name" in msg.address:
+                joint_names = msg.args
+                found = True
+
+if joint_names:
+    msg = Message(f"/joint_position/{joint_names[2]}", [ 0 ])
+    n = pub.sendto(msg.encode(), (SEND_IP, SEND_PORT))
+    if n > 0:
+        print(f"INFO: Sent {n} bytes")
+

examples/oscpy/__init__.py

@@ -0,0 +1,138 @@
+import struct
+from typing import final, override
+
+OSC_TYPES = str | int | float | bool | bytes
+
+def get_string_size(data: bytes) -> int:
+    n = data.find(b'\x00')
+    n = n + 4 - n%4
+    return n
+
+def to_osc_string(s: str) -> bytes:
+    n = len(s)
+    b = struct.pack(f'{n}s{4 - n%4}x', s.encode())
+    return b
+
+def to_osc_blob(b: bytes) -> bytes:
+    n = len(b)
+    b = struct.pack(f'i{4 - n%4}p', n, b)
+    return b
+
+def parse_string(data: bytes) -> tuple[bytes, str]:
+    n = get_string_size(data)
+    values: tuple[bytes] = struct.unpack(f'>{n}s', data[:n])
+    value = values[0].split(b"\x00", 1)
+    return data[n:], value[0].decode()
+
+def parse_float(data: bytes) -> tuple[bytes, float]:
+    values: tuple[float] = struct.unpack('>f', data[:4])
+    return data[4:], values[0]
+
+def parse_int(data: bytes) -> tuple[bytes, int]:
+    values: tuple[int] = struct.unpack('>i', data[:4])
+    return data[4:], values[0]
+
+def parse_blob(data: bytes) -> tuple[bytes, bytes]:
+    n_values: tuple[int] = struct.unpack('>i', data[:4])
+    n: int = n_values[0]
+    values: tuple[bytes] = struct.unpack(f'>{n}p', data[4:4+n])
+    return data[4+n:], values[0]
+
+def parse_args(tt: str, data: bytes) -> list[OSC_TYPES]:
+    tt = tt[1:]
+    args: list[OSC_TYPES] = []
+    for c in tt:
+        match c:
+            case 's':
+                data, val = parse_string(data)
+                args.append(val)
+            case 'b':
+                data, val = parse_blob(data)
+                args.append(val)
+            case 'f':
+                data, val = parse_float(data)
+                args.append(val)
+            case 'i':
+                data, val = parse_int(data)
+                args.append(val)
+            case _:
+                print(f"[ERROR]: Got {c}")
+    return args
+
+def encode_args(args: list[OSC_TYPES]) -> bytes:
+    encoded = b''
+    for arg in args:
+        match arg:
+            case str(): encoded += to_osc_string(arg)
+            case float(): encoded += struct.pack('>f', arg)
+            case int(): encoded += struct.pack('>i', arg)
+            case bytes(): encoded += to_osc_blob(arg)
+    return encoded
+
+def parse_type(arg: OSC_TYPES) -> str:
+    match arg:
+        case str(): return "s"
+        case float(): return "f"
+        case int(): return "i"
+        case bytes(): return "b"
+
+@final
+class Message:
+    def __init__(self, address: str = "/", args: list[OSC_TYPES] = []):
+        self.address: str = address
+        self.type_tags = ","
+        for arg in args:
+            self.type_tags = self.type_tags + parse_type(arg)
+        self.args: list[OSC_TYPES] = args
+    
+    def decode(self, data: bytes):
+        data, self.address = parse_string(data)
+        data, self.type_tags = parse_string(data)
+        self.args = parse_args(self.type_tags, data)
+        return self
+
+    def encode(self) -> bytes:
+        msg  = to_osc_string(self.address)
+        msg += to_osc_string(self.type_tags)
+        msg += encode_args(self.args)
+        return msg
+
+    @override
+    def __str__(self) -> str:
+        return f"{self.address} [{self.type_tags}]: {self.args}"
+
+@final
+class Bundle:
+    def __init__(self, time_tag: float = 0.0, msgs: list[Message] = []):
+        self.header = "#bundle"
+        self.ttag = time_tag
+        self.msgs = msgs
+    
+    def decode(self, data: bytes):
+        data, self.header = parse_string(data)
+        data[4:]
+        data, self.ttag = parse_float(data)
+        data = data[4:]
+        self.msgs: list[Message] = []
+        while len(data) > 0:
+            data, n = parse_int(data)
+            msg_data = data[:n]
+            data = data[n:]
+            self.msgs.append(Message().decode(msg_data))
+        return self
+
+    def encode(self) -> bytes:
+        bundle  = to_osc_string("#bundle")
+        bundle += struct.pack('4xf4x', self.ttag)
+        for msg in self.msgs:
+            msg_data = msg.encode()
+            bundle += struct.pack('i', len(msg_data))
+            bundle += msg_data
+        return bundle
+
+    @override
+    def __str__(self) -> str:
+        out = f"{self.header} ({self.ttag}):\n"
+        for msg in self.msgs:
+            out += f"  - {msg}\n"
+        return out

examples/state.py

@@ -0,0 +1,27 @@
+import socket
+import struct
+from oscpy import *
+
+UDP_IP = "172.18.0.1"
+UDP_PORT = 7000
+
+sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+sock.bind((UDP_IP, UDP_PORT))
+
+
+while True:
+    data, addr = sock.recvfrom(10*1024)
+    n = get_string_size(data)
+    [tag] = struct.unpack(f'{n}s', data[:n])
+    if "#bundle" in tag.decode():
+        b = Bundle(data)
+        print("Bundle:")
+        for msg in b.msgs:
+            print("  -", msg)
+    else:
+        msg = Message(data)
+        if msg.address == "/time":
+            msg.args[0] = float(msg.args[0])
+        print(msg)
+
+

examples/test.py

@@ -0,0 +1,7 @@
+from oscpy import *
+
+
+encoded = Message("/", ["test", 0.1, 9]).encode()
+new_msg = Message().decode(encoded).encode()
+assert(encoded == new_msg)
+

workspace/src/osc_ros2/osc_ros2/interactive_input.py

@@ -68,15 +68,15 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot]:
                     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.")
-                    rconf["cost_mask_string"] = input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")
+                    cost_mask_string = input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")
-                    cost_mask = [int(i) for i in rconf["cost_mask_string"]]
+                    rconf["cost_mask"] = [int(i) for i in cost_mask_string]
-                    if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
+                    if sum(rconf["cost_mask"]) <= robot.n and len(rconf["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]}")
+            print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if rconf['cost_mask'][i]==1]}")
             break
         elif use_urdf == 'n':
             node = JointNameListener()
@@ -140,7 +140,7 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot]:
                 except ValueError:
                     print("Invalid input. Please enter numeric values or leave blank to skip.")
             robot = None
-            cost_mask = None
+            rconf["cost_mask"] = None
             break
         print("Invalid input. Please enter 'y' or 'n'.")
     if robot:

workspace/src/osc_ros2/osc_ros2/osc_ros2.py

@@ -23,27 +23,37 @@ class OSC_ROS2_interface(Node):
     
     def __init__(self, robot, config):
         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_names = config["robot"]["joint_names"]
-        self.joint_velocity_limits = joint_velocity_limits
+        self.joint_velocity_limits = config["robot"]["joint_velocity_limits"]
-        self.cost_mask = cost_mask
+        self.cost_mask = config["robot"]["cost_mask"]
+        self.trajectory_topic_name = config["robot"]["trajectory_topic_name"]
+        self.hz = config["robot"]["hz"]
+        self.state_ip = config["network"]["state_ip"]
+        self.state_port = config["network"]["state_port"]
+        self.log_ip = config["network"]["log_ip"]
+        self.log_port = config["network"]["log_port"]
+        self.commands_port = config["network"]["commands_port"]
+        float_or_none = lambda v: None if "None" in v else float(v)
+        self.x_limits = list(map(float_or_none, config["robot"]["x_limits"]))
+        self.y_limits = list(map(float_or_none, config["robot"]["y_limits"]))
+        self.z_limits = list(map(float_or_none, config["robot"]["z_limits"]))
+        self.x_limits_workspace = list(map(float_or_none, config["robot"]["x_limits_workspace"]))
+        self.y_limits_workspace = list(map(float_or_none, config["robot"]["y_limits_workspace"]))
+        self.z_limits_workspace = list(map(float_or_none, config["robot"]["z_limits_workspace"]))
         self.robot = robot
         self.desired = None
         self.previous_desired = None
@@ -56,9 +66,7 @@ class OSC_ROS2_interface(Node):
         }
         self.speed_scaling = 0.2
         self.new = False
-        self.n_joints = len(joint_names)   
+        self.n_joints = len(self.joint_names)   
-
-
         # ROS2 Publisher
         self.publisher = self.create_publisher(
             JointTrajectory, 
@@ -66,10 +74,9 @@ class OSC_ROS2_interface(Node):
             1
         )
         osc_startup()
-        osc_udp_client(state_ip, state_port, "osc_client")
+        osc_udp_client(self.state_ip, self.state_port, "osc_client")
-        osc_udp_client(log_ip, log_port, "osc_log_client")
+        osc_udp_client(self.log_ip, self.log_port, "osc_log_client")
-        osc_udp_server('0.0.0.0', commands_port, "osc_server")
+        osc_udp_server('0.0.0.0', self.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)
@@ -77,11 +84,10 @@ class OSC_ROS2_interface(Node):
         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)
-
         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'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{self.commands_port}')
-        self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
+        self.get_logger().info(f'Sending joint states to {self.state_ip}:{self.state_port}')
-        self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
+        self.get_logger().info(f'Sending log messages to {self.log_ip}:{self.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
 
@@ -105,17 +111,16 @@ class OSC_ROS2_interface(Node):
     
     def joint_trajectory_handler(self, *args):
         try:
-                if len(args[0]) == 6:
+            if len(args[0]) == 6:
-                    points = [[float(j) for j in i] for i in args]
+                points = [[float(j) for j in i] for i in args]
-                elif len(args[0]) >= 7:
+            elif len(args[0]) >= 7:
-                    points = [[float(j) for j in i[:6]] for i in args]
+                points = [[float(j) for j in i[:6]] for i in args]
-                    self.get_logger().warn("joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
+                self.get_logger().warn("joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
-                else:
+            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])}.")
+                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
+                return
-
+            self.desired = ["joint_trajectory"] + points
-                self.desired = ["joint_trajectory"] + points
+            self.new = True
-                self.new = True
         except Exception as e:
             self.get_logger().fatal(f"joint_trajectory_handler: {e}")
 
@@ -123,7 +128,6 @@ class OSC_ROS2_interface(Node):
         """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])
@@ -191,7 +195,6 @@ class OSC_ROS2_interface(Node):
         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:
@@ -204,7 +207,6 @@ class OSC_ROS2_interface(Node):
                 else:
                     self.get_logger().warn("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:
@@ -223,7 +225,6 @@ class OSC_ROS2_interface(Node):
             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
@@ -252,7 +253,6 @@ class OSC_ROS2_interface(Node):
                                 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:
@@ -271,7 +271,6 @@ class OSC_ROS2_interface(Node):
                 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:
@@ -284,13 +283,11 @@ class OSC_ROS2_interface(Node):
                     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:
@@ -299,7 +296,6 @@ class OSC_ROS2_interface(Node):
             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:
@@ -311,11 +307,9 @@ class OSC_ROS2_interface(Node):
             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("/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]])
@@ -326,21 +320,18 @@ class OSC_ROS2_interface(Node):
                 #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("/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
             msg_velocity = oscbuildparse.OSCMessage("/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
             msg_effort = oscbuildparse.OSCMessage("/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
             msg_name = oscbuildparse.OSCMessage("/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}")
 
@@ -385,21 +376,17 @@ class OSC_ROS2_interface(Node):
             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:
@@ -460,9 +447,6 @@ class OSC_ROS2_interface(Node):
 
                 # 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):
@@ -504,9 +488,6 @@ class OSC_ROS2_interface(Node):
                 msg.header.stamp = self.get_clock().now().to_msg()
                 self.publisher.publish(msg)
                 self.previous_desired = self.desired
-
-
-
             else:
                 prev_duration = 0
                 '''
@@ -566,7 +547,6 @@ class OSC_ROS2_interface(Node):
             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:]])
@@ -587,7 +567,6 @@ class OSC_ROS2_interface(Node):
             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:]])
@@ -620,7 +599,6 @@ class OSC_ROS2_interface(Node):
             self.previous_desired = None
         except Exception as e:
             print(f'Error in joint_angles_handler: {e}')
-
         self.previous_desired = None
 
     def update_position(self):
@@ -628,7 +606,6 @@ class OSC_ROS2_interface(Node):
         try:
             if self.desired is None or not(self.new):
                 return
-
             if self.desired[0] == "joint_positions":
                 self.new = False
                 self.send_joint_positions()
@@ -648,42 +625,31 @@ class OSC_ROS2_interface(Node):
             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."""
     parser = argparse.ArgumentParser(description="An interface between ROS2 and OSC for robotic arm manipulators", usage="ros2 run osc_ros2 interface [-c CONFIG.TOML]")