From b4abe1eec147321c9ad9cffadfe4a380a3abfa4a Mon Sep 17 00:00:00 2001 From: Ali Date: Wed, 28 May 2025 06:00:56 +0200 Subject: [PATCH 1/4] [AN] (feat) Prepare for using configs --- workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 461 ++++++++++---------- 1 file changed, 239 insertions(+), 222 deletions(-) diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 0b28d5b..98da17b 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -1,3 +1,4 @@ +from typing import Any import rclpy from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -16,6 +17,7 @@ import re import socket class JointNameListener(Node): + joint_names: list[str]|None def __init__(self): super().__init__('joint_name_listener') self.subscription = self.create_subscription( @@ -69,202 +71,6 @@ class OSC_ROS2_interface(Node): 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_limitlim[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( @@ -966,28 +772,27 @@ class OSC_ROS2_interface(Node): 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() +def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: + config = {} + robot = None while True: - use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower() - if use_urdf == 'y': + config["use_urdf"] = input("Do you have a URDF file you want to use? (y/n): ").strip().lower() + if config["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'): + config["robot_urdf"] = input("Enter the absolute path to the URDF file: ") + if os.path.isfile(config["robot_urdf"]): + if not config["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) + tree = ET.parse(config["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'] + config["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 = {} + config["joint_velocity_limits"] = {} # Iterate over all joints in the URDF for joint in root.findall('.//joint'): @@ -1001,14 +806,15 @@ def main(): velocity_limit = limit.get('velocity') if velocity_limit is not None: - joint_velocity_limits[joint_name] = float(velocity_limit) + config["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): ")] + config["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 config["cost_mask_string"]] if sum(cost_mask) <= robot.n and len(cost_mask) == 6: break else: @@ -1029,21 +835,21 @@ def main(): counter+=1 time.sleep(0.1) rclpy.spin_once(node) - joint_names = node.joint_names + config["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() + if config["joint_names"]: + correct_names = input("The following joint names were found:\n" + "\n".join(config["joint_names"]) + "\nAre these correct? (y/n): ").strip().lower() while True: if correct_names == 'y': break elif correct_names == 'n': - joint_names = None + config["joint_names"] = None break correct_names = input("Invalid input. Please enter 'y' or 'n'.") - if not(joint_names): + if not(config["joint_names"]): print("Please enter the joint names manually.") while True: - joint_names = [] + config["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.") @@ -1053,17 +859,17 @@ def main(): if joint_name.lower() == 'done': break if joint_name: - joint_names.append(joint_name) + config["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() + correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {config["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: + config["joint_velocity_limits"] = {} + for name in config["joint_names"]: while True: try: print('--'*50) @@ -1071,7 +877,7 @@ def main(): if limit == "": continue else: - joint_velocity_limits[name] = float(limit) + config["joint_velocity_limits"][name] = float(limit) break except ValueError: print("Invalid input. Please enter a numeric value or press Enter to skip.") @@ -1082,9 +888,220 @@ def main(): cost_mask = None break print("Invalid input. Please enter 'y' or 'n'.") + 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: + config["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()] + config["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()] + config["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(config["x_limits"]) != 2 or len(config["y_limits"]) != 2 or len(config["z_limits"]) != 2: + print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") + continue + + if (config["x_limits"][0] is not None and config["x_limits"][1] is not None and config["x_limits"][0] >= config["x_limits"][1]) or \ + (config["y_limits"][0] is not None and config["y_limits"][1] is not None and config["y_limits"][0] >= config["y_limits"][1]) or \ + (config["z_limits"][0] is not None and config["z_limits"][1] is not None and config["z_limits"][0] >= config["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: {config["x_limits"]}") + print(f"y: {config["y_limits"]}") + print(f"z: {config["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': + config["x_limits"] = [None, None] + config["y_limits"] = [None, None] + config["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: + config["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()] + config["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()] + config["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(config["x_limits_workspace"]) != 2 or len(config["y_limits_workspace"]) != 2 or len(config["z_limits_workspace"]) != 2: + print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") + continue + + if (config["x_limits_workspace"][0] is not None and config["x_limits_workspace"][1] is not None and config["x_limits_workspace"][0] >= config["x_limits_workspace"][1]) or \ + (config["y_limits_workspace"][0] is not None and config["y_limits_workspace"][1] is not None and config["y_limits_workspace"][0] >= config["y_limits_workspace"][1]) or \ + (config["z_limits_workspace"][0] is not None and config["z_limits_workspace"][1] is not None and config["z_limits_workspace"][0] >= config["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: {config["x_limits_workspace"]}") + print(f"y: {config["y_limits_workspace"]}") + print(f"z: {config["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': + config["x_limits_workspace"] = [None, None] + config["y_limits_workspace"] = [None, None] + config["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 + 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(config["joint_names"])): + while True: + try: + lim = robot.qlim.copy() + # Find the link corresponding to the joint name + print("-" * 50) + print(f"Current position limits for joint '{config["joint_names"][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") + lower_limit = input(f"Enter the new lower limit for joint '{config["joint_names"][i]}' (or press Enter to keep current): ").strip() + upper_limit = input(f"Enter the new upper limit for joint '{config["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_limitlim[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) + robot.qlim = lim + print(f"New limits for joint '{config["joint_names"][i]}': [{robot.qlim[0][i]} {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() + n_joints = len(config["joint_names"]) + if update_limits == 'y': + config["joint_lim"] = [[]*n_joints,[]*n_joints] + for i, joint in enumerate(config["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 + config["joint_lim"][0][i] = float(lower_limit) if lower_limit!="" else None + config["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: {config["joint_lim"][0]}\n upper: {config["joint_lim"][1]}') + break + elif update_limits == 'n': + config["joint_lim"] = None + break + print("Invalid input. Please enter 'y' or 'n'.") + while True: + try: + print('+-' * 50) + config["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 config["trajectory_topic_name"] == "": + config["trajectory_topic_name"] = '/scaled_joint_trajectory_controller/joint_trajectory' + break + elif config["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}") + while True: + save_config = input("Do you want to save this interface? (y/n): ").strip().lower() + if save_config == "y": + config_path = input("Where do you want to save the config?: ").strip() + if not os.path.isdir(config_path): + print("Warning: Path does not exist") + print("Making Directory...") + os.mkdir(config_path) + print(f"[TODO] osc_ros2.py:1091:0: Write config to {config_path}/config.toml") + break + elif save_config == "n": + print("Will not be saving config") + print("Invalid input. Please enter 'y' or 'n'.") + return config, robot - node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask) +def main(): + """Main function to get joint names and start the ROS 2 & OSC system.""" + rclpy.init() + # TODO: if no config use interactive + config, robot = interactive_input() + node = OSC_ROS2_interface(config["joint_names"], config["joint_velocity_limits"], robot, config["cost_mask"]) # Run ROS 2 spin, and osc_process will be handled by the timer try: From b45371d39b1da3f6613ceeacad115408ac7c7389 Mon Sep 17 00:00:00 2001 From: Ali Date: Sun, 8 Jun 2025 10:41:50 +0200 Subject: [PATCH 2/4] [AN] (feat) Moves the interactive input to another file --- workspace/.gitignore | 3 + .../osc_ros2/osc_ros2/interactive_input.py | 351 ++++++++++++++++ workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 376 +----------------- 3 files changed, 375 insertions(+), 355 deletions(-) create mode 100644 workspace/.gitignore create mode 100644 workspace/src/osc_ros2/osc_ros2/interactive_input.py diff --git a/workspace/.gitignore b/workspace/.gitignore new file mode 100644 index 0000000..3026820 --- /dev/null +++ b/workspace/.gitignore @@ -0,0 +1,3 @@ +build/** +install/** +log/** diff --git a/workspace/src/osc_ros2/osc_ros2/interactive_input.py b/workspace/src/osc_ros2/osc_ros2/interactive_input.py new file mode 100644 index 0000000..ca0b421 --- /dev/null +++ b/workspace/src/osc_ros2/osc_ros2/interactive_input.py @@ -0,0 +1,351 @@ +import os +import time +import xml.etree.ElementTree as ET +from typing import Any + +import rclpy +import roboticstoolbox as rtb +from rclpy import Node +from sensor_msgs.msg import JointState + + +class JointNameListener(Node): + joint_names: list[str]|None + 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) + +def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: + config = {} + robot = None + 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: + config["urdf"] = input("Enter the absolute path to the URDF file: ") + if os.path.isfile(config["urdf"]): + if not config["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(config["robot_urdf"]) + root = tree.getroot() + robot = rtb.ERobot.URDF(config["urdf"]) + config["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) + config["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 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: + config["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.") + config["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 config["cost_mask_string"]] + 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: + config["joint_names"] = None + break + counter+=1 + time.sleep(0.1) + rclpy.spin_once(node) + config["joint_names"] = node.joint_names + node.destroy_node() + if config["joint_names"]: + correct_names = input("The following joint names were found:\n" + "\n".join(config["joint_names"]) + "\nAre these correct? (y/n): ").strip().lower() + while True: + if correct_names == 'y': + break + elif correct_names == 'n': + config["joint_names"] = None + break + correct_names = input("Invalid input. Please enter 'y' or 'n'.") + if not(config["joint_names"]): + print("Please enter the joint names manually.") + while True: + config["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: + config["joint_names"].append(joint_name) + print('-+'*50) + correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {config['joint_names']}. (y/n)?: ").strip() + if correct_names.lower() == 'y': + break + else: + print("Please re-enter the joint names.") + while True: + try: + config["joint_velocity_limits"] = {} + for name in config["joint_names"]: + while True: + try: + print('--'*50) + limit = input(f"Enter the velocity limit for joint '{name}': ").strip() + if limit == "": + continue + else: + config["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'.") + 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: + config["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()] + config["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()] + config["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(config["x_limits"]) != 2 or len(config["y_limits"]) != 2 or len(config["z_limits"]) != 2: + print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") + continue + + if (config["x_limits"][0] is not None and config["x_limits"][1] is not None and config["x_limits"][0] >= config["x_limits"][1]) or \ + (config["y_limits"][0] is not None and config["y_limits"][1] is not None and config["y_limits"][0] >= config["y_limits"][1]) or \ + (config["z_limits"][0] is not None and config["z_limits"][1] is not None and config["z_limits"][0] >= config["z_limits"][1]): + print("Invalid input. Lower limit must be less than upper limit for each axis.") + continue + + print("Current limits:") + print(f"x: {config['x_limits']}") + print(f"y: {config['y_limits']}") + print(f"z: {config['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': + config["x_limits"] = [None, None] + config["y_limits"] = [None, None] + config["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: + config["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()] + config["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()] + config["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(config["x_limits_workspace"]) != 2 or len(config["y_limits_workspace"]) != 2 or len(config["z_limits_workspace"]) != 2: + print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") + continue + + if (config["x_limits_workspace"][0] is not None and config["x_limits_workspace"][1] is not None and config["x_limits_workspace"][0] >= config["x_limits_workspace"][1]) or \ + (config["y_limits_workspace"][0] is not None and config["y_limits_workspace"][1] is not None and config["y_limits_workspace"][0] >= config["y_limits_workspace"][1]) or \ + (config["z_limits_workspace"][0] is not None and config["z_limits_workspace"][1] is not None and config["z_limits_workspace"][0] >= config["z_limits_workspace"][1]): + print("Invalid input. Lower limit must be less than upper limit for each axis.") + continue + + print("Current limits:") + print(f"x: {config['x_limits_workspace']}") + print(f"y: {config['y_limits_workspace']}") + print(f"z: {config['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': + config["x_limits_workspace"] = [None, None] + config["y_limits_workspace"] = [None, None] + config["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 + 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(config["joint_names"])): + while True: + try: + lim = robot.qlim.copy() + # Find the link corresponding to the joint name + print("-" * 50) + print(f"Current position limits for joint '{config['joint_names'][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") + lower_limit = input(f"Enter the new lower limit for joint '{config['joint_names'][i]}' (or press Enter to keep current): ").strip() + upper_limit = input(f"Enter the new upper limit for joint '{config['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 is not None: + if lower_limitlim[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) + robot.qlim = lim + print(f"New limits for joint '{config['joint_names'][i]}': [{robot.qlim[0][i]} {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() + n_joints = len(config["joint_names"]) + if update_limits == 'y': + config["joint_lim"] = [[]*n_joints,[]*n_joints] + for i, joint in enumerate(config["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 + config["joint_lim"][0][i] = float(lower_limit) if lower_limit!="" else None + config["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: {config["joint_lim"][0]}\n upper: {config["joint_lim"][1]}') + break + elif update_limits == 'n': + config["joint_lim"] = None + break + print("Invalid input. Please enter 'y' or 'n'.") + while True: + try: + print('+-' * 50) + config["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 config["trajectory_topic_name"] == "": + config["trajectory_topic_name"] = '/scaled_joint_trajectory_controller/joint_trajectory' + break + elif config["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}") + while True: + save_config = input("Do you want to save this interface? (y/n): ").strip().lower() + if save_config == "y": + config_path = input("Where do you want to save the config?: ").strip() + if not os.path.isdir(config_path): + print("Warning: Path does not exist") + print("Making Directory...") + os.mkdir(config_path) + print(f"[TODO] osc_ros2.py:1091:0: Write config to {config_path}/config.toml") + break + elif save_config == "n": + print("Will not be saving config") + print("Invalid input. Please enter 'y' or 'n'.") + return config, robot diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 98da17b..5d64fdf 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -1,4 +1,4 @@ -from typing import Any +#ruff: noqa: F403, F405 import rclpy from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -6,35 +6,19 @@ 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): - joint_names: list[str]|None - 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): + def __init__(self, robot, config): super().__init__('scaled_joint_trajectory_publisher') @@ -203,7 +187,7 @@ class OSC_ROS2_interface(Node): 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.") + self.get_logger().warn("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 @@ -294,9 +278,9 @@ class OSC_ROS2_interface(Node): 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.") + self.get_logger().warn("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])}.") + 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 @@ -397,9 +381,10 @@ class OSC_ROS2_interface(Node): 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())]) + msg_time = oscbuildparse.OSCMessage("/time", ',s', [str(time.time())]) osc_send(msg_time, "osc_client") - if not(self.joint_names): self.joint_names = msg.name + 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)) @@ -409,7 +394,7 @@ class OSC_ROS2_interface(Node): 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_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]]) #msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]]) @@ -420,10 +405,10 @@ class OSC_ROS2_interface(Node): #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]) + 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") @@ -500,7 +485,7 @@ class OSC_ROS2_interface(Node): msg = JointTrajectory() msg.joint_names = self.joint_names steps_per_m = 100 - if self.previous_desired == None: + if self.previous_desired is None: [x,y,z] = self.robot.fkine(self.current_joint_positions).t [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() else: @@ -509,7 +494,8 @@ class OSC_ROS2_interface(Node): 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 + 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] @@ -561,7 +547,7 @@ class OSC_ROS2_interface(Node): 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) + out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] is not None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] is not None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] is not None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] is not None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] is not None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] is not None else False) if np.any(out_of_bounds): #print(fowards.t) #indices = np.where(out_of_bounds)[0] @@ -613,7 +599,7 @@ class OSC_ROS2_interface(Node): 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) + out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] is not None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] is not None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] is not None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] is not None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] is not None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] is not None else False) if np.any(out_of_bounds): #print(fowards.t) #indices = np.where(out_of_bounds)[0] @@ -704,7 +690,8 @@ class OSC_ROS2_interface(Node): 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:]}!") + 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) @@ -772,328 +759,7 @@ class OSC_ROS2_interface(Node): 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 interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: - config = {} - robot = None - while True: - config["use_urdf"] = input("Do you have a URDF file you want to use? (y/n): ").strip().lower() - if config["use_urdf"] == 'y': - while True: - config["robot_urdf"] = input("Enter the absolute path to the URDF file: ") - if os.path.isfile(config["robot_urdf"]): - if not config["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(config["robot_urdf"]) - root = tree.getroot() - robot = rtb.ERobot.URDF(robot_urdf) - config["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) - config["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 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: - config["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.") - config["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 config["cost_mask_string"]] - 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) - config["joint_names"] = node.joint_names - node.destroy_node() - if config["joint_names"]: - correct_names = input("The following joint names were found:\n" + "\n".join(config["joint_names"]) + "\nAre these correct? (y/n): ").strip().lower() - while True: - if correct_names == 'y': - break - elif correct_names == 'n': - config["joint_names"] = None - break - correct_names = input("Invalid input. Please enter 'y' or 'n'.") - if not(config["joint_names"]): - print("Please enter the joint names manually.") - while True: - config["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: - config["joint_names"].append(joint_name) - print('-+'*50) - correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {config["joint_names"]}. (y/n)?: ").strip() - if correct_names.lower() == 'y': - break - else: - print("Please re-enter the joint names.") - while True: - try: - config["joint_velocity_limits"] = {} - for name in config["joint_names"]: - while True: - try: - print('--'*50) - limit = input(f"Enter the velocity limit for joint '{name}': ").strip() - if limit == "": - continue - else: - config["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'.") - 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: - config["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()] - config["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()] - config["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(config["x_limits"]) != 2 or len(config["y_limits"]) != 2 or len(config["z_limits"]) != 2: - print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") - continue - - if (config["x_limits"][0] is not None and config["x_limits"][1] is not None and config["x_limits"][0] >= config["x_limits"][1]) or \ - (config["y_limits"][0] is not None and config["y_limits"][1] is not None and config["y_limits"][0] >= config["y_limits"][1]) or \ - (config["z_limits"][0] is not None and config["z_limits"][1] is not None and config["z_limits"][0] >= config["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: {config["x_limits"]}") - print(f"y: {config["y_limits"]}") - print(f"z: {config["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': - config["x_limits"] = [None, None] - config["y_limits"] = [None, None] - config["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: - config["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()] - config["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()] - config["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(config["x_limits_workspace"]) != 2 or len(config["y_limits_workspace"]) != 2 or len(config["z_limits_workspace"]) != 2: - print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") - continue - - if (config["x_limits_workspace"][0] is not None and config["x_limits_workspace"][1] is not None and config["x_limits_workspace"][0] >= config["x_limits_workspace"][1]) or \ - (config["y_limits_workspace"][0] is not None and config["y_limits_workspace"][1] is not None and config["y_limits_workspace"][0] >= config["y_limits_workspace"][1]) or \ - (config["z_limits_workspace"][0] is not None and config["z_limits_workspace"][1] is not None and config["z_limits_workspace"][0] >= config["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: {config["x_limits_workspace"]}") - print(f"y: {config["y_limits_workspace"]}") - print(f"z: {config["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': - config["x_limits_workspace"] = [None, None] - config["y_limits_workspace"] = [None, None] - config["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 - 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(config["joint_names"])): - while True: - try: - lim = robot.qlim.copy() - # Find the link corresponding to the joint name - print("-" * 50) - print(f"Current position limits for joint '{config["joint_names"][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") - lower_limit = input(f"Enter the new lower limit for joint '{config["joint_names"][i]}' (or press Enter to keep current): ").strip() - upper_limit = input(f"Enter the new upper limit for joint '{config["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_limitlim[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) - robot.qlim = lim - print(f"New limits for joint '{config["joint_names"][i]}': [{robot.qlim[0][i]} {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() - n_joints = len(config["joint_names"]) - if update_limits == 'y': - config["joint_lim"] = [[]*n_joints,[]*n_joints] - for i, joint in enumerate(config["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 - config["joint_lim"][0][i] = float(lower_limit) if lower_limit!="" else None - config["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: {config["joint_lim"][0]}\n upper: {config["joint_lim"][1]}') - break - elif update_limits == 'n': - config["joint_lim"] = None - break - print("Invalid input. Please enter 'y' or 'n'.") - while True: - try: - print('+-' * 50) - config["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 config["trajectory_topic_name"] == "": - config["trajectory_topic_name"] = '/scaled_joint_trajectory_controller/joint_trajectory' - break - elif config["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}") - while True: - save_config = input("Do you want to save this interface? (y/n): ").strip().lower() - if save_config == "y": - config_path = input("Where do you want to save the config?: ").strip() - if not os.path.isdir(config_path): - print("Warning: Path does not exist") - print("Making Directory...") - os.mkdir(config_path) - print(f"[TODO] osc_ros2.py:1091:0: Write config to {config_path}/config.toml") - break - elif save_config == "n": - print("Will not be saving config") - print("Invalid input. Please enter 'y' or 'n'.") - return config, robot def main(): @@ -1101,7 +767,7 @@ def main(): rclpy.init() # TODO: if no config use interactive config, robot = interactive_input() - node = OSC_ROS2_interface(config["joint_names"], config["joint_velocity_limits"], robot, config["cost_mask"]) + node = OSC_ROS2_interface(robot, config) # Run ROS 2 spin, and osc_process will be handled by the timer try: From 6db7143bd5762448cc80ab0661c13a54b5c51849 Mon Sep 17 00:00:00 2001 From: Ali Date: Sun, 8 Jun 2025 11:15:59 +0200 Subject: [PATCH 3/4] [AN] (feat) Successfully writes and reads config --- .../__pycache__/__init__.cpython-310.pyc | Bin 142 -> 0 bytes .../__pycache__/osc_ros2.cpython-310.pyc | Bin 34230 -> 0 bytes .../osc_ros2_unit_quater.cpython-310.pyc | Bin 24940 -> 0 bytes .../osc_ros2/osc_ros2/interactive_input.py | 220 ++++++++++++------ workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 94 +------- workspace/ur10e.toml | 27 +++ 6 files changed, 187 insertions(+), 154 deletions(-) delete mode 100644 workspace/src/osc_ros2/osc_ros2/__pycache__/__init__.cpython-310.pyc delete mode 100644 workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc delete mode 100644 workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2_unit_quater.cpython-310.pyc create mode 100644 workspace/ur10e.toml diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/__init__.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 579e1571924891cc844e982df2d3f0f9b9399820..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 142 zcmd1j<>g`kf-hG@GeGoX5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;!HazLTSVd45rL zaY15os(w;wW=@KJesOYqQGT%zf)O8|nU`4-AFo$Xd5gm)H$SB`C)EyQR525fU||3N DA{QS+ diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc deleted file mode 100644 index 574fd9e23862a1e274ec780c0f324d9312a0cbaf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 34230 zcmd6Q3zQsJT3%Om^>cb28jVJycS)8t8d+|Q^k4~->T~Bo*qfYz==kEtM0A)s(b(Y-~V<0s&{wi6ZrS3-@Ef{=`RwAFEbMVXCU$* ze$H1ASP4s;O-%5r&uVk}gs$;EIh&j_CXBh%L`veuY!S%PXXRf_Sow;6DLJtX@h+=?ctPUZt!}I5)x^Y(ifQ$p)9v}nj!S6Y z(oV^5r0-q4X<~dt^NsPkRTx+`YE^URuD@* zS)ay5+lL@mcP8g5?%BG9NH-#97OS(?eA#v?lWZ>jFBwFfeYmM4CNwKCp}&@x(k#u= zU(HS=tz^ZR(k;VEy=qLPUP~B>iS((&NZMZ~?T?q|Do<7&w^FOvlUl51-ly?n15&x0 zxRh8ywO2JQ(MYW5Zqm{li5bI6UNsttQp!SVDQzWCxiRjqpW$nkYN)<68`OWT`+!Py z_|ne1Y+@wir<}$4itT4hrE0C}mP-EmNxOm!C1>%BGig`n-DA%gVz0HsFb>o9sj9w=>=S)xPC>8$WJ%115 zC)K%$hBl*J(lCHJ2k>oed?e``95>(K5ZY{cM7KBKVsB#YI6ez&boT3wZ|H=HodA%^je#(ElBCJ zwp!b8U1x2#cHr7?nbuBR*IT=+TW}pfJsVKZZtGUmGbn3lBfxyv?|=H#!=;l?pE?B4 zbu0E1fXVw15rOH}&H2SMn8CBA47}-v7@=Bm%rj-DVwv@tiSp)L#c|5h6{k2!7!Ut( zsySk6{dFf2H(GzPCes$y;!MU$#nxoDLBPs2uv$y`2G?p=BZ;seE4Ew09u6&sl|%Zu z6rT241%!Q8H^Oype?x)i`bP4Nr2XYw!X2=BF98#{8?3%d+REV7#LC92iRDxyWvx4> zzoFR=<`Ui)t$u6$tIRWiJVRF##}WeMn=lnBeXu2cv$ZiuA8JY8g7y zpGs_VOKgjZ*`Gnb@I*^In^<^`Wvnfm5>{eSvwuQj$(9(Ng{kLK&X;fxfCzH@qQt`d zDyH&pwvZa+-xB422l=<~iL|C-D!-(jOWE(BZ(GB@;eMNp-+;B9efVtREj@_uV9T+% z$(qBs)w7&#q%poD7}cE^rEugkjZ84|gSGXz-zBrK3+>#(dadr|bj?6)H)E{9$x1!N zXA`w``1KeGck3J4()}9tH_YpRbt`i|pQvf)6ARDRHe+U0ozGZ!$7wFRWSh0OBb;ovMmoaD4uK4eE8zrh_`?$4-WJ~x z#_gX#PXlOqLtB`|IByH*6C?a95)WoHimN%@reNR=-Qga%Jywb{zdeKllu~h3O484z zf>Jx8Qo5Z(sU1v0J-Ps2i((My~pZ*Aj^rccK7($QdYFh#*((MZo#B7W1wnuk6 zYH748u!?dow7}7Au^wj|S%Lbb{YBT5+0I=8raG2*Lwo7)axRCfw+HWDnAy3q3-~FI zTK5O*>~^H*8o5o0&nB#*%EK86QVs+unExPU^sS7P++ZDyl|1xTQuSxK3!`~Q$0vnG z7e?u?8U=>#Esd^A8p6UQO=k74(5E|D^RtOJ^o2JYUDjQIUfuqN%2@+Rx7^hzNNUo~ zs*;k5daVTVChb#=u0}rW{ka5Ep1_!Gu~3@iBRHno3-caM z)cWv@Yd$G3@FDgwHZRGAO*QrgZ__M;eySdQu2Dd0FnZ?<)bUB_(O9E^KHS%QcAXww zQtuk(ZRyeL7`4NzdUQs5gpqBQY2-PRrJcxse{6Q%&x{8!0|@UEESX&T^$5ZrU>_8g z#O!?*zwfCP0F`Roem6E=`ohO+pKpQWBe8i1A=$8B1t!IgJ+pow(YzCUyW72$Z$S8d z>D-I@a&EZ?*z!t2Jf#~QsM9(@+`j=Zf4mI{0tS9kwU1Oa* zP1(Bj)bcv)-?ul`F&+Kv6-Z1i4Mw9m&R##y_J99;VpV%jOMCr|{-DNvXs81Ix_!IM(~ETtBnZVUvi z%vqbDG(~n$A%6%V3=~=lSwVp%W&&&Nm zYXakc$h~9cPV0r&ne(9aB2w-`%H2%en26F}Li#-|IlmPt_qL>zkn*9Hl#e21ES3_? z9Y%gjY$VD;@!p5L_XThLu0{I#KEUlQOjXZg@BUu&?q{NR|1G@xfV}&FHObT{W;Ob; zPAZM{;R=bKwC0;?^hv&TH^?{b9$vX0zXz@+u4+7Q zb&c9u^0d4`%afKq2TzV#SWh$eZ6Wq~=qhAN=)qB|irB-9iTq(^+~4UyJW#PpitCM- zuccDJl~ruJ0Pe~LW0mkS2f+8&7u|O)#QfB;;}1RasBb(r{>an5@$Au)<9_P+$&*i? z^iz)}ak<`0n%-$Vfu${qP28R&5^X6yx?-!3ea@w@}BeeJ?HL@KGN*?QSUR4&(<+-&I7m!%X%k|leaeAvlVl;ejayxpT&8|j9T8* z+$+MI+nov|We^`(`^}0J9KJ9tV^OQC{1}5Z2f#jvPTHJG`%VUTGiZ~wd%xzd1kg;J2xUTQYlZKRdZ6bDGc-;ZL9T3nQwt-b8gXb%`<=jh)Vzr zMzBY2sic(-bp;7K5Z>57GQb;B;i-@HQ?=IZFW;Fbn$0d$U;!w1$PG$-pd*RIdbR_s9Q)vSJ)Yx2&& zOUY~60q_2B5SwpPG>>w>z;@hB3*Q0CHrqANSKYJdEHG1ScQ#RN9*9Bdy1p@J19eti=y7s zt2!`WN4HrzY$?=}T)&CCMn~_$@K#@k%FH_u4pk>Ar5p?y-n494M4&})c~v)VW*ijw zD6QrP8jxpKHQzD{v34S@(e+2;p3%`!j>dCaFdEM^YnN){(+V)Rh`ntS1p6E)Y|vQs z>(_m+s@uEwG*(blN@@<*vQ?d$st6r6W!L9qCS{aN<|U6}%) zoi!0IELLih;0r$Nja+}656TpJx82M*VN*F%v5VfP@4x;2{}2CrL#F^|7@Gj&21^Ez zc1;^=KE$*(;TsRVohK`03;aYK12|c!0{z4lr$kiVjuT4V+N``eGc5Qn6{#b~>>kpK zROZ`M_hEN=xmsUzN?}Tt?OD}H*Y9otDL)BX&`-`*YJRq+ zns@xvybbNMpAsAgYYG=86MjkvM#Sc4tFE8D5OyOQbtvnFY59+kM?20v!S4@qmLm4T z?+?qw)56lRw3P5mzOjH;_KnFptS0W_=;hS#GBsBbvIh&x-_V?&lI&Vyhv4Ti3%2WU zzw&d(FHBa%{KM}HY(kpi8_uVni{gF~FYYHf5?w?MQqAJL!V`-Y)^rSSFs@L9LKn)~ z#^0zq8ck>kgr$WI+)vh>Ie#5s?&!%!OUEDtdS?8@(TAV#2U`n7`Fnz_M~@vldHmEV ze`6a`i82nU3SvVMHo3JEiiyf?XaS2TSwHLfX{SDUt^&xxE`Am;cV>z7j^7nTC6Y&k z$a5O+W>jlab-xgduUiH8>Gv+qTeQFlN5anoYoQ1Mb!3AwL!n8E{S*UAckFRaN1mAi zfKP$|@-qmgs`i}U9n`2E+V?Vdk`U1)fT{}lT|%D+5rd3`pOO*swThqSq5x>i;^2nh zXJy4QjZh=~MQV4r7oq#MYl~5eh;YO*g-iby{G3-H$ABGAvT4U-WSaNsY+7sn={leA zFZ(y?Oj^rpX{2PeKHM>NNdETeSy%*(>Z95yo*7zJ??aA0+@*t@d8DV2p8k4Iu170q z`g{79^qqOs!avri|F(X6d%dHmH?7^R->uypl)rJkMq9ncZ|etCy#r{+K-*b;OxujV z?Na-oHi#bdyY!4|ceB*a z8VB&lR{2&Y{-Tm!GYTf=1EZ^2{$qVR^RwQx_P0h zJ%|tLyJge|P>;9eI-<}P5etQ56uUi4z@_k6R9_fLpb7VMQ`I8y&NCO z+RtEGWt+CC0I)A2@Qw3jyJmkI<1>t>rpj)4b|fi?-#17=+uy~fZIbbiARCdAr4i}?g}S}YN?OLN zNTZ$EvR=C{_B;g(ti7;ui#-RNXn;7~$`Jl)yQ6p(5SRpv(JF^mS`Z|yu8p)53DZO^ zqS{7k_gh`81JDRPiq!-Cy}ps_z_5A|rc}B>sS7Ag22je@Gge;%ZG_M+8s+3GNvQ`p zUr7p-t{WG4dzaK`%b*Y|MA{n;yJ&B6%h26eS~wI+Z!@B=U6P1HN~V`JwZv2zHN#Ye zysg{!;NojD?XymO$m^JOQSb~Tu07sicGn_vIw^c&x*yflT1uFx(aj0($Omln-3;Dh z)%b9@MK87sH!?om&5^ZWo%$U>c?7m#gpE}-gmYuRi5i-tX@53~Hc1l>tY_FH`T zeF(tL%%EGeRQpH9y5LwdF13*N%3uS9jy0El%O z_W2-pkaB}MgIuvXX@Zdp`)K80)cZryN|c6v|4=;7by|qZ#OkNXUrtQc&|j(lcY^wx z@9;bEJlCy%RsT^k593{d5vyP{g7F+i;}q+$Lb?FO~38j0E@%11N6kw%@L z2*x_z`%lF4U8j|(u2|2-NHgqV(8kXNZN%T@=cJ7rzexlc%{EZ7(~M@Owbk<)t)Ff+ z!%~Y7zG~j1=W2cRw2`;nZ|F-n^0b@{NC&P(oOWnrTS$l8a`qJ%@e6^AJ0TtVt{u8D zfva$H$nHBu`yt?(ci+Rm7_qxUA=EHe6P-Rzv}g`r>l%h&H&Ah2+r*s)Mm3~b&(s$o zQ7)R#INVJHV~_j2t<^cF?fcNE_hM^uvz43MU&0Q=c9$46?Cltq-EWE=QhNT1snoWFQuwUt}PA$`tROWYA$_8ObSXIuqy~ zY}sM*>g7d9lMVY||0~qtZw!Qk?NUcS8}7~>Y4Vp*_D$|fmvLi&2IpSHKk}%3;8AyJ z>GuYb>dtsizqs2V*K8tncT(LM?~$iv8FJ$M%R*WZmK}_h-5ZtN+fue4WlVi5c=uor z99eS@3|~4N_!f$a)Ks(s{vIM&feCxcJ1)#-lMxuOS{Z?n*mE)hx2<9XbgZXjzzE>5 z$%=|+o8_=xbJ3d2V}*6ahuk*HJ{NPU_q?Ki#sK)tiG`_1{PJ9!pK<@1W(zvp^c zuEPb4`vXzqv96!?ws%Snp%CitiR(Wfhu%p{iDZv3=k5B-RSO0u=ELP0c_~P7pe8V* zO5!gLRzu|DB4@dXiBx+-haJKGK|XC0sdz~HGSN3VPzs%!jK%gg#-b-_XdSI9qkhH4 zBz*Z6kSDm)AXX$Se*jzo2IGVh2OtE=i4!Y0gepgi6Ch&{4lSvWr!~LyLfD;!Gg0j% zBw4#*e<8>nq}+7vLH>B{wQoaRSrEyOtnrQu!8;;X_2=!ugxdbV-T7wJol|SHePb*x1tW4Ew0k66zBaI0SYs~O|D0p<0}LpTQSwSp z36>%~V)X7~=HrTIuz#2(euTk~A~@}BTC>L^-HPlM#UrIGF;ZJl>@?Ga1{cb^1Bx9< zH>ElDPcZwv3_j1GlR(?SgRc|&K8G7a?+3{|h~GFcZ#uahcLN}W_X+{6?nprEU(XiQ zvwl3?qcLqDNrg`sD;Ndf@d2u*F6!U(_T4`Qpz|Aelfi)=x{l;oL)*rhx@bVuyA zhG|1b?6v}E{lMKRM%{UPjka%$-DvF}GxCaIw|_wLR8%-ho+6TtlWTVWSYR&JhJ<0K zID=CVkl8~!I8{|vov`{>Mipb`h5J1NWVD3d{az34%Bs__W1Ho(Yubq4+twuTZCgsX zJAo=bvF4+jB#7Hbd88*s3%6nL7Xcc6a;|&gPBHITJwC( z_O*FG2~NWPNl7Yo#OoLEY!AT5nZOOH>VifEeL;hA9=N>+f(nZ6+kd-Zv_Q2#FnaKV zS;BkjmwwN9Z!pyq^9wcJ3A3*;sN&Y8McmKxX+-hV!aKi*UC8?lS3K}ox(n|+q@pir z)7sVKvhEt7e1RZ&8g3@#Y=S!(Al*{R&A9OXfqM<7hM^3&k`T8lbOuBnXDaZ=k∨ zisehUGCf(}x}LYW$pVn~X?l<73y zAcZ%;#=|#|vT*2Pv4wR5Jo&3zz32&@%vbP0S25asl!-uIlh7VDO|l`wav>BnW)N(w_r? zcLfMs{Bu|vc@96O;~iS=dZml`Ex8j~bdz`J;lRYuoLa2O*&QzQfV+&W6BJSqgC|%l zrw-6$%hUHeHmyTeVORQ;A;t7HGM1vk%u>aB8KX)5Xbm;5O=m(l;upAaiiC2sNmK;z zCSox<;HeF$Xu44#aP51VvsEzY1Cj!s_! zX2XiMRSky*qH8v3|7SEA<2Kf6Vur8%Uy#>Z-_k&33YuN_16O}8sy;NAR{a+lwdQ=_ z`e&p11M|x^P#H7FM}}!Ii&oRLu?E6`<_k>}wlFbuzeM78glU{XRQTG(X z``K{J{h?M9-XI;WnPh{4!OOX7t>SC0pR{4c`VkgRtMT-6=)^<6~~nHCK!0RsY@`6VW~#4WVmU>K%qB`61O^RaSdZ8SIYBV z#fQL~3;@wdIviik6jng7lSW1zJuAJ`gNKGYt9q4^Ui}6fV;R_fRId`vUaj318ko6# zbi6eK>VBqwWj!|QB;0oNy@<_@oR>R*9&R8fFvQ+vwj{_KEnGtCpan5jLr1SMD&Utm zMjN-uot48VeI9%WoN29WvO*CUP}B#1`>57JG%I+2@-bl?_dT)uczSO*cF5BgH--X7 zvl-%hf=ir+o*4^#fiq@SskYjxb%|B`vqWgNZ)hM#356IlJ*zlxaBus$W3)M0b` z-!hUcpK3T)t2O;*B9z z+3|^3{ee#7;9*LgaXh90xiStHlVZfkV+`TfF9zevL>T-7xDT2Z)BLz6mgfX`k(iul z_$ER3ojc^;ZyuTCaEJehku&%SMy7EYO(%g(vT)<6Ps6VRMe8uiO%5gCct`_2f~}CN z!O^D%%_u@(T;nRRr3LE-5bVJdrgy;zIQ9%up_LSCZx{oKA-E9%w$oN%^#YkkdWv>O zaCQW&mZe3}mc)m!-tio1>d8v(Dy0|3xGeo9^)`}M;G);S;o&q54gyqb;`0IXRP_xU zY<2uD95|T7Q2?IZ0vs*Q`RU2UIk~!Z`jiOu?s?eMVt1uIm*3;oVc%K`_2_ zfD!ms%y=L6Ud+1gbyA3TuBQd#@Q4z(SuQST52p8xq~~DZ6QzW!VQb_D1i=>EiYmby z+`(eX>GjCK6eVWT;NUVZCaJKUrArdUMXBjUeT`5f)xf{bi` z+7;unVP&DIsae@8#%qb!wAb|4lCK%BrCv+FmU%7vTJE*{YhAAurn<$8Z`SR-3}S*D z+j26;4Le7o0Xm4UVsq=etVDoF!Nv)VP?#IO;&zJXN*G;nbDc4;X(ws4hIEkJUfOw`T8A|8pJl$Le3s0U9qGI4*s5MCn+)o z@s|g$tqkKn=5AIaipg>YCYB+Rl3w2RR6tLXf@#5(RT* zXG6%SW@*+K^`c)v!b}E)4ZNSSA6y%V4ZM{K$jG$xVHaxyDXV(;n^6rLf?NXskP;;2 zJi{#@f_CF~>s6rLnLVXp%;Mgakw)NOS*;e8+bS#<;5ROSZ9wZYA+*9vAmQqR!)ub~ zGS-Gyv~yo<7H9moRI2$g~Izy8UZ--*gQIpUd6w3nw_CL0od%%Km^cSO>-Av9PCy8SoskNLkl!9Q{gVrvEDqn!g_A9z=|N3P3- z_q#gSO7xaDxhAA%@l}zu;a4qST|V%t0An%wMsn$&fU$sKr09LgSR90`jXuuA51DxA zOD?Z_1t&wmSje5o+J?O1DQxC)B0aFH6AwT!od88|bUh%JAH_%P-PmHsg#&n(WR*=e zK&*y3b10PDlJ0eA)ggkTrzv+^(JrU3@1=$k*x5iF!H=0Xkl2{GoWWj*ov$sm>vG|8 zS9>aVOh_U@fL+!vCok*m&o3t~YnM@XTV1)!`OCTXRMs_+P~xn&HF-U_9@lZ)HF@gz z9e(oUiRZ-$*4y2ni)p3Vo>q`gi|G*Rj z^7w=H&&osZV4M8#3#5ziOZ?z)pMmMpHjz&b0U5#K^Sg-lo`ql&M;T9Tw98Y&+*XnBu?d@!!i2$c|YyeOAwv;>9cSyfJ4tD^5^D-!?gd1rG&dAV`={} z19Fx2I}C)OY_gI4F)?44EQAuA)0J@Aot9T(bY*{o8u?qa-Y+cf?pSKH^2 z+n@~t_)P=+#~`?o0i8Ta7Dty0S`l1pVL$B?9ughcH?{3JD&1p1>y=eUUh&xjJW;%C zkH&+~)Rw6OuhfAVDP8Ny9t%qEib@;cd`z^Xj_R@wb?BA{l7~>oJ{%Tottm1YUX%N6 zd&3s~hzxHYKf6uIe zrC@php(L)kElQ7;LHR7=`IRo^+uq8)o?1=<+v}x*+wJy1+z5}TVue-&ztM+UQ!aRm z8CYde`_f32`dv_!&?*J-nGL1EW=iw!k_O5(CzNXnc{Vn5a8W}Ip+FZ7UxqLcd*5dK zwt$}0XSSBMpJT z4tKI&c8x#MGOone98*~dH>szXKmnzv=%gkRT&ECp6GL^v^Rh?1^Ra;+d-O}*t4FvV z@$c(LR&|ePfmq5JL?9b*W|@;H8*WU0gR|4}7I!0?P5!)Xe|ibsmi=jvhrxOQL%wd| zyBuzLa?(#NSP&1%E^uv!c+lXX0`(ejN(Pr^O?~>vEed}U8`>MWOhq`xVN{+&lD2!s zPtU=RMR`UcQ|VX}0|`Q#WPU+t+JeZ_Y{7zmz+}O;f`PxpXcH9*7Tm%%w=&qqU^{~y z3@B7nC{HlDV76zGHj)v{=I36nmV={jqRTL(hxf7+F;oou$oL8N+0b!9Kf=M>i!wUU zk2(`S3RKdMUmwEVKnMds1wJHb_bf~#0&JMd8tFp=;^&H9(S z7H|w~#RSgU31vXr8-c%u(Npl2!P}4nS3I|rgg^h4fQv>8x5C3W+Tj-j8intm{it5z zTMw`74RIjkPb2WNOt0SipppRS3+K@7-ewIbo*{+vlpwP{Telr&Tbu1ClVtWs+CGYKI(7h(Av`4ta_c5T=k$oHP z-u~)uPXLwobe}yv-=o;tx72It5CU*$cpaNHtel#p47G1v)Ta$ z2M~#ZB8*Dm9uR-K1p|)(7xUy6 zNiyYgC7<;6zaOA%!O5faulN+u!vffBaIwKq8vtt92w*|5#>n5bF8F+V`#_3exgUfK+Qv7=m#)K zc4--S1mGnS2XTCAoYvHuO>;9oHKX%6ggoByUU^O@Q?@H{m*u2hcb+@<-7Iqd0h z2)S-mW@uW3G&#)*|CGUA!romB#u!{=nyd(+6+Vq<_;s=-ClbM?eF@J$K`1}%x0CR1fZn2z0Q=%b$7*~g=L!BA{T~DeX5>_hJsnmkB zRVSXNHyR7}T)3u!D-Od;98!9@!URCB&~L~!-xlu*;)USa9b9{Y>-)+teF7xPH4W>x zKD3#!ddZtK07oP1#=To#ym0WLy+_7gc<}a#7hk-+^it!+7hV{>{m4uEUNCPzfbhZF zUwLW7eh{zkYkB(!uGh5gYg+D_HhfLnbxr%|UvowN{7)}(xc>0s=Ys27!S#EC>u12L zJCoI_pRp_Rv-rXpPBhK2QXSs8;NNR*9?}J>+RoSy6E>dbvow4g;ykUNwko^^u48O& zJAPNW=2S}+c+&xwEI<-L3f#QGNq9~CI?|6_ZTiOuHa#eo)$_@M_Q_;6*`G9Y@0JcA z(Oz~HOhnj+hNWWlC;G7!n0wNw8y zr@px7Pr8ch9&6QpSS=3cgqX^(S?WFz`z zM8kOzs$L9_(0tpR*iRti&vP2cQJugEUvEc;!KZUJ_`Q@bGJ>W0GuA$2Z^y;n!N5e| zXG$fjK8bGz^4Nm?Dua(PSYq%p1CN2j-~t2ERrccy1PxJA)Q*cTNfy3Vns@B2y^}!` zv<4X^S7Pr%VB+WON03d>pIGAKiH~QWg;KT;e~)E1WQ#o`+1v7avwcXv74hBK9fJc> zauS82e|!Z_14agVU5pI@ikk;iH4rUag4P_H0XCdvY-qBT{hB-hU;z=qdjc2>?W$%E z1N}g-a8+~mxKLM;1YH2>iTjM)_qz};FsC4`0_sb;-D0`KG^j`UEyPB0rO$#nuBbjy z7H1kzPZbQjJb{7>wnsatqFh;L0c63fWLBWo#2r3?W%b~kM!k{4GolzM3RDWj4+Qjd zBYQZpZ~`ATxevX<=l)=%o3rpC8o7dD0m!dsFYw+FWX#}$A=UX>%qP1+pW@K8fEdZyyb2vP&TqQ$3t(2 z+1nhL3y6@Kv#?j*A!m)Kr+BhYQgn--NdpAZ)`C!I?U&Y0*YXV>Bz&=vS}|#H!O>ZG zD5_DnZs)N%t0!iz2Y`D{bxdo`1^04ydk;NThDa!G&<$iAI9#IDrX!MnF)$HA51$h? z51D(!37xY9aq?X8v^N^KeLu@DXq3%oP9A#%8U>s=j{9(4bebQl5EjGR*YrRPYbdH7 zD&~9{p1=d$R#Xp8d!&kswl(D)rg}lD4je_rT5J2vN9aI1s-@^fUl3p`Yh>@(f#M`E zc=V5iNyJh=h={P%TZJ4-SP*y6a*4+r;QNR%IDd=9$Tonq!!zS*=$R`8fgMO*KMUcx zjDw%2gZ64|5q@`Jk!Dw&IoRJ$is3u4Vs$E)0ACzNv$QHUK~6 z<2|nVEaNK(04C)cjQq)CPZaaV`LUMDEc|A}*9&~QFuhsdWF6mIapvoRm2-tYq8cI9y|!g*Q20~KrW)OGF&1`PvFXab`~z5@qHQ0po4?>&}+f-umiI! z`ZvQ_d??(Ywsp&hu7)e@)JkmqCT?U}G2J3ExI0WxlO4_T*D%k@c=Yb(dTPG<-n$|5 zbt~|ti_hE~q?&CM?#VDhYG4oF&#zZ-NDsmjySU`=_J>BAfnWOQY;CNnr~caRm%Ru522W?Z`_IV`IHypF|_&Fv5S&Ic6 zI0r?dj*;LyQCx9)AXUd67z}JuEuec*N|ax3Yog$58{caj-!bID@!UI@b(F!+GRUwh z9P19mBb0}DN1w$~6W!;*QGA|ZQXJSlPD{R1bOIbv(?oKW3jmGD7IX{Oa!RPRn+OSYzH| zH2sojEckss57{bsuR%x&;I{eHAP?R`ol5Z0{&RIUK zzNBr}m*@>1AEO{#^P}7RI846S{7_8zQL2`YH8pz%FIPv|<=xHE56@Sq6O}Q~h{IlT z7>oBo=8wFxcK)b@I5@{wM#L7GD;N6O__ukSbJh8x_t5p0a7z_%qP6PNFjB|Jt+2kB zoSk2y3-79HKZni<_iaz$&evffYjCgi(^ZEXx}U|=iDPw}${k-nei}9%W!nK850b>L z*Ik=3RX=_FB)y&Z1~+ynsA0K4RXn#uSid5s{KblJ@~_ya$XZLhn`S^Z+s`@k)mjP0 zxa#Be_AfCp<;>#CM1BEMD73d!s!>K@a7vV(p6r=#u%y54iOSNMIyB>t@gr{b;yfIC z^PG>`wy@4edi^XgSBX>${FAfC>K@vu3Tl=25#t16F6F6QcJd!*^fX_lC!Fulap6Yw-WGyFl$c1!Qz=t+e&bOK0Kf zYiT&vra+etv-vX+qJBxIV{BC?)2L3P#vXaDWi!<4BULy4Lho1klq>52)*867rfYKJ z{~B2XZoK6Fe;NI%e*L?9Y^JJo{k^P#*1v(b(X#JlHQV91W=r_c0cO86veSN+33-9j zB8S%|ZRf9V>g{NQ;S>?XP6+e@%Q$tIP=t?24Ep`3r33^}v8N!0Qf0{V`x^ilcg`=} zQ7k(&8gb9Uy)rB+GC{JP<&3+M^TOP05wKpV6pt%X<}`sNB}sl)EX{t29Zz8?;sDs6 zvuqb^&E0Bw7IflS8yp1=E(p|BCmmSP%$})VkmaLfGX!`0d1SJWF<54l>q6uRGD=D| z(j+V8jx>j#Jq$lXi?fvnP%`0gnPp)>(T9l;U*nL;tEpv7{lSd9hjNVOU-lrR7o70- F{vU!q9W4L= diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2_unit_quater.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2_unit_quater.cpython-310.pyc deleted file mode 100644 index 068417d32e277a9dd2694e20481a38c7ddd7e554..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 24940 zcmd6P3y>Vwb=~x5W_o64XJ`M5e*^q123*VnAPJI7@BPO6-W%c+V~PAX-)IAv zl8P!R29k)JdtY}?&tR4WLq%mQ=5_b`-gn=9_uY5zeXr5qpN`?*sbAiH@t3|5i~Tiy zME~jdcpN|bdkCzUrPN|`^i*rgk~*g<^sd#kC4Ej`iqFOQTdyUSl5;Mo?+kzMwEC^=>#@0A71J8Hq>AOr zuB&L@Qcm+PpL-8Ijm_p2PoHgAmAvW=JkzMwoin1eSSdRV(ey^TB2H2|3Zyyyv{P~_ z_|l$hEa0K+Ly&6N<)w;qv0>q(A0HQ1t2Jx6BJ*FKa9LE4p=hsOu*;&l>{J_d5l3ckBvQUuF4bxm zO65ylCYsUfYqxB_i*d0lwRt>K#bUWuvh8B=-Pnysrk^-EeYqhn*~_JJWt#Cj4MZ13 z!#;HDOL3I|TUiBAqOjZ)1At|i@RCE2QFKMcABBk@rxWhhXQ=B@#1_J?X-oD(Sw*Sf zAHCD>N&M^y1P%!6Dp2ewEoD)?ssO1PBUPPs_jAoAA=NfTrCh1LQnAc=(O5D$g^9^T zqFq|3%;a@1zAONWr_%^{I+YUycU~2vc!;eOP@^)~LT`fzcHn2zWHywX;@*90e-?Rg zc^JcCz(`^UdBBTax>#IlSgW-PhBH$n)YW7P%oqW~5E0KA zB8Z3_4d8n$EhujDx$xUa4d#NS%xOFyR7=Br(0L}r=HgbuN@8LsEW=9SnY7YYAD)Jl zvHI~$Sy^iU&$N}R^kM!CT0@vW8Ee=YL2SQur!{7czpl(>tqE%jVg{_O);2tI)^=+L zo`cp-YZsnFmTB$Aa~SoEpq@R}9jIrN=iwNp%3a>j3#Xqfo_gW*A&@`lYaRsYK19sK zbQIID5MUxS3IssRyV*D?ZYz%&*ZlZiJ=60b>^Cb93I3!H*9KN0+;%d*~RLtGw z04Z0Mb!~&@E@m-ivkoq|#fsDSi?RA%idzS=HWaZN-=+h!5qaW$iym{5&Bv738aTPz zj})U7e>?ULh?de{0{32vzZ$QDGtl$0ayhn=1m`fE6t|Sx(9lX6F?~+P!gI0TN-;0V za@>H`hq#=Tk#Wpl23s^ChFah}#o-q5oskyR0^W~sjKgu_+17xSqe1%$^<=^tM!3Zq zLAVwDz6-F95|qw1U|{SjCfr)0mEgX=WsR>TkUqibs52G{k+Xvc`Ovja7zS}9KZGi&Es zirYz1Y}gJsW>-7k>C zwCkE^reeU!9!VAJJ2zr$eXTz0j!WuwRg9xg-vjC2^*ZIb8+mqZ#E!+7NtxE9AHTOF zez%qPz+&6b;bT1e*>D1Z@?kHMFoAFB3NIHT~}62Dr@cE60>5f z%5_D&#^0%~Z=?tDm*QySoA}xtDfKpgw@b-yS!%b1_^{NTNU1+SsXdhHw=KVADUPR7 z;tv3Cp$#v-rs)gM0|ZkrPw;(^pon}QqWXhxOI^#fGN|t+Fn%vEh`Gw6(%ce=y4mchs;ti$w0HQ0ZNP!%+*heAqFo5LX`JU}^Liq$bsf#Kct zZ5Vf|RTWbf`no`K&J5pnhi`l8G+($KlpALHc`yQLwv}Z_HBoc!Ou{-!FoF8im1j8b_gjOlTma1_ z#EZY?zCY0#1hh}K)9`idop=Nz!>wU|M*SPvQjqsZte!)f#QJy9=5cB>f?svzcj`uW z8NjTGL#?5p7SL@BG5Z*rr&>d3|7oPPvD6zHO>B`p4)R8z(Zn5q=2N#o^L`I#ax^MM z;;|nW0EJow$dIl6mIMamaz}3C=CP8prv;QY> zk+7bnvUolxpOoguNP1JP=jAA^jkHE+micu1PW`O^EnE3PDz*??QzA9a`t`g(Pi`xy z@uXklXls;f{4~S)NA+}T6!3nhrL6C5jgm}#U0eA^Sfgs4YAG1E(;?+{LiC@xpJ%8w zl=~{mo%P>e#SFvj!d%Te`)IG?+|x3!u6*XI#_tKNWiRskl=WGx6EE@merpccC^*xL z2dtOBNICag=MZzSBj!9}4t2zQ4l(z3#1s*8Uq{U65p#beCRkf1F&9h32>|P1C&92L ztP2v8{#UtW01H+t1%9Y^q;%%1v=W>z`4Rc%od?z*#P6Yv*aqR(YNbD{1mutz@Y{#2 z1P*sXON0Wr2NxDJz<9 z2-B*}msV@ej5#^I83g4<-4Tsits?x5TN}E`{EX?v-247t%HB|>-A7K;Un$k9R-``# z^QhS!7xV&K4dIxVtIkD$J6UjtkJinK5a>puTwWETV$GPMAEK_vAaIk@*vK_jo%bw! zdGTY%pE&!pr=OU8>IF}K@#v{pFMj;gsTWRp@u!ZSIr>~a{vJum?~PEflY+ez976D4 ze(lTj?_WOllxy9y|NsA=>qJJnBo-=;dEz7q`EsC}jaBn<38RUj$NsNcc?ARkqNp_% z%#zJJf#>D!&o=i0Ih&99!9x!e4&d(~4Wye2KV578&)}c?!Y$ogZeW0kE`$E~+t`oj z$o&Tn+>ahqzleRUI~@>JwZ6RS6wH&g3N}e5$RCrZ^lm7{!R%50iw-*g`{_co{N#7>lEhX#t&BBX>kjo%MV z1PkDu=QC?ij99Ttn#edYTIuT0N&TX_qMnYut9r_nS?GG=^R-6F!KYd{;3?IJR)G`> zUit{>Lx6zgN3T=e)Q}14xpgBRDsBo#Y7MYy`%%0EW!+P4MAsUZiNw&K)nzENWJhhL z=PTx3)-vt4D^TGGCy#zDFVd308-pX!C?v>q@YW6^DyFmk>Zaa zxJ~GqKU3&_>0?6YejMoDqj7X!_%I5sTq%{ETGMncH;5aoLXASS4>kctdZAXTU&`}{ zR7zM?nOF;gw3P1CU9~>P<1NW~Y1MXUnXS?K>Rc?eStBKDQH<{ixrU7Gx`h z8SBC9UZ+71D4}=*ug*BkOA7ss=sIHC0_1n?ja_+r@p7)rOA|3u2q3iFH7%` z&;4&VNm4idWG1^JUa1H-=kKpV)Z}5Xg7|NjFIJW+?qFB)^u8OpX{WqgEH^Mv)jCLqfg?+frp^wNfub zO!$;bobXp3+5gDj!2j;VX-Kv-QRImE!Z}FjhBDJmq2r}b%srENnAEjM%lzUy2y$&sMf~;nOtY%(@DKIdYj{8fSuX6~g zj*Ww!U)d`ah`d#&3G*#6XD?N5*iLb&WMA^)ER%RipIW>e_L;9#8>@CPh%u-=t6Fxv z{tjC6G%UuR2BdfzStiq;At~%Q?c}{UuhGERa;@rk#+9I9BLu;4gE*~LsRImJyF7gb z5PEvK0n6+I)Y(J{i%s3~jJoWg?e)+OGn18%$x`qi8vOt7Bf+n{2n!I7T z(GI+Z(dtFlOW2L_r3z3F^#N8M=R%Vf9Iwy+ZHRxhBN+2is0mjL$TL zn{Y^>ZSs-`=Br}K>-XE2#3yE`9gW7hkEui!^7>dN@jrADou~!~*dV;LL@f1IurZl; zOKef533?}f_6HE|Gm5IJN>)iL3E1$-nx9d#%2vgAO}(l9Z$0N*_J5)$+E#u&*?vzL z36u=}s4n?u{3wx3D5|C^25L1>ufcx_#Zajoicvqfy>u!m%Vqs?3Dlbd%+%Hd{~b^% z&tdhja@fzELyjC;N%%R_ethEnY$^)~)SLQWsk_sth5iUm{ZG{?s&xck45K;&Ol(zl za@`4a2=R9#J%=aN#%11DU(2Nu%I0Z?GDCF($K2+h>3dbpZ|^(my+%?+?FQGEK^__l zYF%PWVvk}H$f4gbk|UfB9R7D>NdHgi9SJqVe?!_3V32jdVSC2Hz_6YeI3i zeaNoAV>eFRm(+w$6XARvfjc7=5S9Xy`8`lds4xktmtta!d^hOps?rCQMNzFM+cESF zwKAFU;5q|kuu7&rQWC!iXQ$K1&uMW>m$gGl9e+n_r|U3aG+%_#9V%pZ(9fN;60Jx* zFQKM2^;MPXlW$~v&l~Q6Pci#~AXmpkq~$W|JivPyW-F#I8Wf~XmAAm7r%7lCQh1w3 zE9jGfoDP}%f|MB{xL~7L_XYaS3eDqJmN7d~(ZMP6!m48~U=!jg1$VqBKbtXU^M)t` z+sxXWqkROubirjYIRc0B2cYT)|QaE*kBoOx$O0ptK zSKNDgG{4y@u_*+CH9@R!VGR6okA{~KcLEKIxA5ZTH*Gdp1cFuOmPToA@VwZL7tl1s z5(^4BP^SVqSEbV!#pE^jfsdxaaN%ovyg<<~7e+^f*#IOE3S>A|mB8_+QiJNd@k-Ug z&enXgRHq%MTvpAnl4#wtpc`J#n6&;jIw5-D%SOU;L^}pv65!%l{CesQ!&Z8L+JQZi z&d6@T*!y$T?S4njdxWt?s5ik|#@Uw-E*GTkBzFr-TTsTq0qT!+=C$38~kUZkClIq?=8h=OuI#~kli?0o5*x~8q^ zs^u{+5mqjzG~GxcwxxzfuZo*Ztac z@rQnHKc=hpflX`o^G9p9atlgJ9S3Yh!vdEr!U@MhkHLM@&F}jzPXYJsvxcPOlf(?T zuSO0}lwZCv_Z@Z*eKh6sG&h6!26rw{MEI{mAwGg=R){BHh-O8&RDw(9@}WB7&!ST1 zH6tA+`sf!GvE@g+cc8xYL_BJnH8>zB2c>r6N5+AEDa zV(xu;<-nA=f;DI5(3Dvy6wDPjaXwEwc6V&bv{s>*hw|fmzP%`kFB0s8e^+iQLs>Y) z$c%KmG$-A@$#Bq2cMGXXV0|S0$%V!$j4TE7tWB$}yKA%VJ;O?)NpGMl!x?u=S4(2 z`d&Z3ekm^Q0}JiQ|2YU$#MET|H3R;6@N@CSxP&YT29gX#HLU%BBW@YePfVP462W&% zy{fK32t=yyMJ6sd@Qjq-VX6_9UE#9)-O^BJNS`Zd4CW%)ZyO@(EDfy?rDOGjB^ z%BisIuX9=c_EWLQP}xhG{dIU&DwLDob-btPUHnPd?>-Vb!L~Jo0mJ6f9Lr@ng-haX z;GKSAkyegaGZ=Wh1#gs=v0#9~rkTz5fz1MQV6&O#Przo=5h1r9Y?iz=d|?@E7U{31 zDZhLpHXFh=%q<};KQj9st+E|76J@2h%cL%cOsW^t++=^>i)p@>{AqyWhTy!rN!uUI zGtWGE5{BSVhgfLy%)?-rWTTxj-6_abwc3=qTy@G95mmY@8Rr8ZmgS>(s@zyS0z|l{ zHUJ_|xL2qkF>YEI5>Q`c6xt+blslEJVBT3Z z2L^rX?@nymSqX9zn|1y{eR5O&5;ygmV{u~cazmBXASU->@0(zj4<>XRlOSn__&jYi z><=(|;WJuv_&kcGIyj3$ocOA;ploPss-u%wdKDXF;;Hsl9Hap6EykUsWAMAOn7XQ7 zRo;y8!2miM0}iwp_dS`@k$ioTe2K1nB(C<0e@fvx399K0WpRLcbNIcUgFh=a@;En> zgm=6~+!P`BC_)m8b&kOI7G z#N~(|Q{5Z~@f~r492b;LbKDTe1!emXH|UVZt|*Y-$@ zJN#S;KUWU9B=osfe=FM>SkSxR9;MNzE!6o(u|*kN8{&F*l7}pM&`;2}27R1K_;*Fp z`oc5`wHZkp3DXEx35A3~>x2Ow(z~$6?cp_U44(L}VwHmrHmh>Rv00w5cDIH*R=GWE z!>bWA4 zF!{si=HePgbxSO|xgdQ4^cfG~A?TZ=qb|nerRqdUa$|K~Dl2 z8Zde-o|^(K{vNf|*7kgjKHm-FLO6aafJ2bL=q9J;`5#OAkyUA^+rTz`1&Et>Z;fyE zh;w0k>tb$jNvze}v0I{fS#?Kli3jsWB~tijyi5n!y`fBb%KR;eAl`TyzaQoh@YYZ# zU#aeq+thXF8$b+i3v;oqh67X3rcH`}fF>ijf3Wj2#61qm#Kb>DqC4Etv&uYtGMrCb z{Wrtv1BOwfWVvRH2L}QI7h~! z2(+h{;E3QUs6i|@y%gGg0vp#_#T%f>dgA%#j~_d6^vv<+UJ@6n;)Lvi_*H6(HH)K-zI*POMZxMIYJwD$ox8}6@ajA znb>x>W|x?CWbP64-U#_zbZ_JT1(EMtNOmWCaNu4g#6jdPlP){>NAA_Mn<7B$zmEXh zLfT*(I1H>Vkj__YsUr}2Ac83^eIz!4!+)foYH1@74EUIxhWvLqileUMbTYPr5EriE zKsD9WN^ps!qb^dKPKWZW_!o7EHHg7CHjU8A;W!@0ru@K|DL8(&rp$=M4_04OUu#+sYJTr<%-*=TBVv9kP)er=c~0! zAstq55)PfP6jy{j6UI^9bx=CY+wd+#2)Btd4cI2kwuPi^?ZvK!G8Q_DJCwZ`2=3-y zc4tL3$9y$a;FvSNTC15$4PN|T#^LY=R88kQt16XovbH!c;P*(5b756a1u?#l4>!qp zFk$I{{}LA@?2z7y99u1%H-_b#BRbm%(AoVwCQlfUHtay*5O7hb6sF8W2kw%UPnn1D zS{AESyCS2xiF_Nqrz*~>sCVI({BH>Xj1-`@bLiM}lW}VHz=)s?=K_BGkC*(X;A&%F zvT&I*`M5ZTG}fkhYMl())jGTHdVN4|8N@=~Hqg9{A{%)a$no>Conz)@I}z`xP6@xV zm#h~#g^J3qdr2pVaw1WT3oqTK%uKrfU0VkeqArKWiN6Ofa9zXZ9Y=~GW{~DyDPjh*Vhb}HF*-Ix@pIan+MDq= zlTKotR9x*8V*?v7#H1{2*2s2nF7c-RX5vjls@WV^4%X9BO|+rCsW>?G62n_#25gl5ZIEDJb|7 zuNzM88a9Hofxw~P!4@_LylG5X$t~Doif==mG<1zrbsPgxJ{gq2;YD+e4)Yq|W@Oh; z7l)zFg6a(Kq+}bRizTeSFUITr9jSB-C`{v{y3%e;$2Z@goa>`FmW6tVbK!f8?aCP_ z-u|f72Nj`eWqwYzjCqs_QZMOnf~Qu-SnWr7zuPk}DfV!{)9QbM%8rLM|1gqU^=sx- zS+k!Tr7hGAIB_I6gruFnv-XLgcI3Y!k{`EGFyx#bsedAX9r+*bnLkoLwxgN#Ev?K3 z9sZrgC~d{oX`7WLrC$mwW&IQ>7PrLK=$I@<2)RCoW7~{ZjCE?|Rc8n2HrtMCWs5tV zzE(euqh%L&6-|1V-xE+=5>0XZZfp|wtl!b%mfEGM#t@2}q=Q8qhKEv$MSb2$Dpq1j?*e+(+mO6<(?!_;^fwgjRU-2H~m|EYD z_0y-?c5cg{gS8P_{d7R9z{K}a?vqoiCYY`_qgEV;-5D9hkDyoNzQ1Sg$e07_kYbVl z`AGgY4T82JG&LU5lqxDc^LN(Yrdd_2Me=_%&8p%{J@ZHEkI<|tz7~#kn`Xa&QR|l+ z1^xS7^cVBGmBNAOf~4FQ9j%>SKLAyi1|`PeIgOMUYs1b#(CQ%^M;@9WIvw5?gQCA5 zM`Qn4ltTTPpo+hQ65`+XqJOAOaGY}o&zjc21N>YY__(uzY7cvNn{$C7ip~a2JQCLN ziDv`M3HM#snk!JXjQFY+;urVhLWq$LRZC`VnIr?XdNY0i%mWlyjj1G?6~X7H4UqqHW9=61=$(L zMU|1*cm?wCR>U$Z2i{G2>dFE94wCz7W!YxsvZr1-C>6|V^Pt!O*q-Vhlu}8!!T&KyAG9G! z#MFfTBa%4A6#XY-y87e9P50r=wjEunl3seB_io}JOKfN0>j^zqUKf8wK;0wn3m(z4Tpp#~-o|BMfSm@i>p0_goC!of8$O%lDG2F~PMgnFgmj`eOX zf0$j1r840xa4LSE0@5Iee@ww&An>whaS76=`3|Hywi2Flifs~}cIxCyQfU4TWlI2^ zrP=~+8*0FfGhPy+)D#xgLJrR2yObwhy;OYedFecwV$DMl%1pL>v@IT?NE=5qbhIaS<-z@PGyBV?!ujpu9=n=~{e)ij$JfOUOe|;_uRjR=QwI-!#^R zuo`4Eq6X>XX;j3?z#?!$ij0GRjqGU$Jsm2HgYd3~r#?FiiGN2mDvO>$Ue!gA6FP+; zIp2V{n`PrhuvMqnm@i7#0`XPK9_NmF{ahs2IC?sLdg}5){0>21Qo2&aW;UYk_zx({ zmn``TgRXh<1DegW$=6{BGT@U*vw16YALMJQlX)nSxM8F+u=Zz_C-BQDA1K@5QJd9a z6f(%C6CR&1CXwgo1b$g%A4-xjN&03|j2?%_r*RB#RC-TXT8HMugcm^uwU7@hdFWEB zR8tQ1kQZvSo?PI)x2bZYt9Knd89n`23YJlxN|NccCuj|A70~e@~f+%WZk)G-81%J{GP1A*0 zX-4=vK-N>@*AaTTZW{4MdLiB&>~grDG4rX2(f41WE&)4HOhAH(iBSr=^sc^#=^Ev1 zn?ZPKZc!YD(>{O~6%&~3y3#>9aFp(TCVrCDeU;^&9u(9CZHKW|LnFkfnQs0BFVa5^ z@{J;WL7^ic-5+WGR5DSJC$t4)9iH%e<|p)i{Dh{DpSU!OpSU24pSa+vuZ3d)K)u2cOvc*v!k1@1Hw&Zh!H7>)gvP zAK3rc`F$^&`==2;zW>$pd2uJY?gkr{Qsi)^@rbBC3OAA z*S_LE-|?Tn>Oa4XHPkLwt6ox6mTR~i4*t+=3c!wr4m&I@W9i2Vv3fz6L=>d1hI$_U z-4>2zK#PQ@?-ecn5vBE&>UOnQfrmGiqfR=(nIjy9uJdsUCm;4ghhfd&9(UNZXkL*pDLqrXM>TPAPQK#FrK;fXLZkm7sVOP-y5 zcJ_rAXWjTz)wb=0Fa7D=|Fy3$_^3Drz-QsULB<*JbqYRFl1xpmHP(b$xijx#D>mj+V zoBuFZ;U1AifB_Y|>8(vwWAw=^Xb;{fk?#^UVsy$T_J!CNj4|jbK~;uf?9AqkeQC|O z6S2@-;&1ENh@f(1{K9`sjrd|pdJBjlN}@FY`VQ5CHcvX`VfJEO;uYzia|$R594Nj> zdPoJ=8BT)l7#8#~_T5g(V$I*mU_FlaBxwOxhMY7}80I{2s|@PXoc_h^`T)o2^4`dn zww|-#_`w=Pl!X%=%2s2rb5II=rLj-nMHdLJ58@(21zMQoI-D->Mwk6%R~6A{rI1Fv zg|6-67-zUe6b{YNk=V*fT)Ou#pvv*}M=1*zli_-%l*Lz9;XX>-UWtB>wDd*nWvx-V zFp8n+b4JPPLGhOq@wPKYU)C7b&bM)~mWdk1DGf0(@l|?*UT_RGOi&H9D)Z%r22b}e2X#Mds;G&tMVw_7G@vf5=RzxZT4PIkW20{yGTdKy`ifQO*mZa4*LVm8~116GI|@-k21Td9cK?+L1kr6 ze`s3oQaEdKhx^3yCHSO81qX16&I91sFJhB{dv8eGz!jk8A@goNkz+TpVOT1h2`^E( zNEcL;%(JJCJ%xQ{Or9lE=_eWG`{ggcXb{X|$C2YKU#RbkD$M`*9f zHDaqsyVyu0n_Qq1)>3f83lFH3O|tg{;8w?W;y1LhvL8pl3X$!+{KhOO!pX#FZ^49> zUuwq}37rrnkfv|=-VXUVEaf=$?itts(SE$*rD49oc6t@3RIz~+ReK4RtTJQ38t`owN!J zg6;bF6r{aO7C{>Q63)HIgWwT=PqXdec?!Osr{L^)$~|9jOP!#-46k|lA1?It|2$Sq z2oa##k|bSdATC#7ilhrcP(S1gx;+4UoVr~DmSJ}!10t@u5io4x^VZR)wT(u+ zIY$u>QqW#d!m;?*7zcS|;BcGR+x_2r7-!s^3XZqJ2z!t;BM0Ey53`}f?hzqwz&<4oxagr!RSUk2*?{>XOvLoTrvaCiGJFa4e33L*!R;ZilJi>I8dhH zD6R%!TQzVrQGJb$6U&3J+o1~4l{@*sA{|W5W+;XBa8f-&r74Dv@+NL1l2pH*(*0QV zo7v}eEenMS9l=#^8am}ve{4V<16V$wHpEHlDDLnkxQoU7ZgDR~q?u$2jDB<`<_))% z7o;$=3kakFk!Gibkt7NIp>w7S^;ENsT7V&ws(d`FV0b3UnEXEM1aTPZ-FZTSS#5F8@^L5)xh*G zN_FM{lR>p?LqA%((6}Owpo|pkiTuR<^a)uZCSIXWqEn_m#*!4Q+vCk|#F{+gh`jgw pQIw3?hY`?? tuple[dict[str, Any], rtb.robot | None]: +def interactive_input() -> tuple[dict[str, Any], rtb.robot]: config = {} + rconf = {} + nconf = {} robot = None 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: - config["urdf"] = input("Enter the absolute path to the URDF file: ") - if os.path.isfile(config["urdf"]): - if not config["urdf"].endswith('.urdf'): + rconf["urdf"] = input("Enter the absolute path to the URDF file: ") + if os.path.isfile(rconf["urdf"]): + if not rconf["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(config["robot_urdf"]) + tree = ET.parse(rconf["urdf"]) root = tree.getroot() - robot = rtb.ERobot.URDF(config["urdf"]) - config["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'] + robot = rtb.ERobot.URDF(rconf["urdf"]) + rconf["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) - config["joint_velocity_limits"] = {} + rconf["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 @@ -57,15 +61,15 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: velocity_limit = limit.get('velocity') if velocity_limit is not None: - config["joint_velocity_limits"][joint_name] = float(velocity_limit) + rconf["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.") - config["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 config["cost_mask_string"]] + rconf["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"]] if sum(cost_mask) <= robot.n and len(cost_mask) == 6: break else: @@ -81,26 +85,26 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: counter = 0 while not(node.joint_names): if counter > 100: - config["joint_names"] = None + rconf["joint_names"] = None break counter+=1 time.sleep(0.1) rclpy.spin_once(node) - config["joint_names"] = node.joint_names + rconf["joint_names"] = node.joint_names node.destroy_node() - if config["joint_names"]: - correct_names = input("The following joint names were found:\n" + "\n".join(config["joint_names"]) + "\nAre these correct? (y/n): ").strip().lower() + if rconf["joint_names"]: + correct_names = input("The following joint names were found:\n" + "\n".join(rconf["joint_names"]) + "\nAre these correct? (y/n): ").strip().lower() while True: if correct_names == 'y': break elif correct_names == 'n': - config["joint_names"] = None + rconf["joint_names"] = None break correct_names = input("Invalid input. Please enter 'y' or 'n'.") - if not(config["joint_names"]): + if not(rconf["joint_names"]): print("Please enter the joint names manually.") while True: - config["joint_names"] = [] + rconf["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.") @@ -110,17 +114,17 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: if joint_name.lower() == 'done': break if joint_name: - config["joint_names"].append(joint_name) + rconf["joint_names"].append(joint_name) print('-+'*50) - correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {config['joint_names']}. (y/n)?: ").strip() + correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {rconf['joint_names']}. (y/n)?: ").strip() if correct_names.lower() == 'y': break else: print("Please re-enter the joint names.") while True: try: - config["joint_velocity_limits"] = {} - for name in config["joint_names"]: + rconf["joint_velocity_limits"] = {} + for name in rconf["joint_names"]: while True: try: print('--'*50) @@ -128,7 +132,7 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: if limit == "": continue else: - config["joint_velocity_limits"][name] = float(limit) + rconf["joint_velocity_limits"][name] = float(limit) break except ValueError: print("Invalid input. Please enter a numeric value or press Enter to skip.") @@ -146,24 +150,24 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: if set_limits == 'y': while True: try: - config["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()] - config["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()] - config["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()] + rconf["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()] + rconf["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()] + rconf["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(config["x_limits"]) != 2 or len(config["y_limits"]) != 2 or len(config["z_limits"]) != 2: + if len(rconf["x_limits"]) != 2 or len(rconf["y_limits"]) != 2 or len(rconf["z_limits"]) != 2: print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") continue - if (config["x_limits"][0] is not None and config["x_limits"][1] is not None and config["x_limits"][0] >= config["x_limits"][1]) or \ - (config["y_limits"][0] is not None and config["y_limits"][1] is not None and config["y_limits"][0] >= config["y_limits"][1]) or \ - (config["z_limits"][0] is not None and config["z_limits"][1] is not None and config["z_limits"][0] >= config["z_limits"][1]): + if (rconf["x_limits"][0] is not None and rconf["x_limits"][1] is not None and rconf["x_limits"][0] >= rconf["x_limits"][1]) or \ + (rconf["y_limits"][0] is not None and rconf["y_limits"][1] is not None and rconf["y_limits"][0] >= rconf["y_limits"][1]) or \ + (rconf["z_limits"][0] is not None and rconf["z_limits"][1] is not None and rconf["z_limits"][0] >= rconf["z_limits"][1]): print("Invalid input. Lower limit must be less than upper limit for each axis.") continue print("Current limits:") - print(f"x: {config['x_limits']}") - print(f"y: {config['y_limits']}") - print(f"z: {config['z_limits']}") + print(f"x: {rconf['x_limits']}") + print(f"y: {rconf['y_limits']}") + print(f"z: {rconf['z_limits']}") con = True while con: confirm = input("Do you want your TCP to move in this range? (y/n): ").strip().lower() @@ -180,9 +184,9 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: print("Invalid input. Please enter numeric values only.") break elif set_limits == 'n': - config["x_limits"] = [None, None] - config["y_limits"] = [None, None] - config["z_limits"] = [None, None] + rconf["x_limits"] = [None, None] + rconf["y_limits"] = [None, None] + rconf["z_limits"] = [None, None] break print("Invalid input. Please enter 'y' or 'n'.") @@ -192,24 +196,24 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: if set_limits == 'y': while True: try: - config["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()] - config["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()] - config["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()] + rconf["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()] + rconf["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()] + rconf["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(config["x_limits_workspace"]) != 2 or len(config["y_limits_workspace"]) != 2 or len(config["z_limits_workspace"]) != 2: + if len(rconf["x_limits_workspace"]) != 2 or len(rconf["y_limits_workspace"]) != 2 or len(rconf["z_limits_workspace"]) != 2: print("Invalid input. Please enter exactly two values (or leave blank) for each limit.") continue - if (config["x_limits_workspace"][0] is not None and config["x_limits_workspace"][1] is not None and config["x_limits_workspace"][0] >= config["x_limits_workspace"][1]) or \ - (config["y_limits_workspace"][0] is not None and config["y_limits_workspace"][1] is not None and config["y_limits_workspace"][0] >= config["y_limits_workspace"][1]) or \ - (config["z_limits_workspace"][0] is not None and config["z_limits_workspace"][1] is not None and config["z_limits_workspace"][0] >= config["z_limits_workspace"][1]): + if (rconf["x_limits_workspace"][0] is not None and rconf["x_limits_workspace"][1] is not None and rconf["x_limits_workspace"][0] >= rconf["x_limits_workspace"][1]) or \ + (rconf["y_limits_workspace"][0] is not None and rconf["y_limits_workspace"][1] is not None and rconf["y_limits_workspace"][0] >= rconf["y_limits_workspace"][1]) or \ + (rconf["z_limits_workspace"][0] is not None and rconf["z_limits_workspace"][1] is not None and rconf["z_limits_workspace"][0] >= rconf["z_limits_workspace"][1]): print("Invalid input. Lower limit must be less than upper limit for each axis.") continue print("Current limits:") - print(f"x: {config['x_limits_workspace']}") - print(f"y: {config['y_limits_workspace']}") - print(f"z: {config['z_limits_workspace']}") + print(f"x: {rconf['x_limits_workspace']}") + print(f"y: {rconf['y_limits_workspace']}") + print(f"z: {rconf['z_limits_workspace']}") con = True while con: confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower() @@ -226,9 +230,9 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: print("Invalid input. Please enter numeric values only.") break elif set_limits == 'n': - config["x_limits_workspace"] = [None, None] - config["y_limits_workspace"] = [None, None] - config["z_limits_workspace"] = [None, None] + rconf["x_limits_workspace"] = [None, None] + rconf["y_limits_workspace"] = [None, None] + rconf["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 @@ -236,15 +240,15 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: 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(config["joint_names"])): + for i in range(len(rconf["joint_names"])): while True: try: lim = robot.qlim.copy() # Find the link corresponding to the joint name print("-" * 50) - print(f"Current position limits for joint '{config['joint_names'][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") - lower_limit = input(f"Enter the new lower limit for joint '{config['joint_names'][i]}' (or press Enter to keep current): ").strip() - upper_limit = input(f"Enter the new upper limit for joint '{config['joint_names'][i]}' (or press Enter to keep current): ").strip() + print(f"Current position limits for joint '{rconf['joint_names'][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") + lower_limit = input(f"Enter the new lower limit for joint '{rconf['joint_names'][i]}' (or press Enter to keep current): ").strip() + upper_limit = input(f"Enter the new upper limit for joint '{rconf['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.") @@ -278,7 +282,7 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: else: lim[1][i] = float(upper_limit) robot.qlim = lim - print(f"New limits for joint '{config['joint_names'][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") + print(f"New limits for joint '{rconf['joint_names'][i]}': [{robot.qlim[0][i]} {robot.qlim[1][i]}] rad") print("-" * 50) break except ValueError: @@ -297,10 +301,10 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: while True: print('+-' * 50) update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower() - n_joints = len(config["joint_names"]) + n_joints = len(rconf["joint_names"]) if update_limits == 'y': - config["joint_lim"] = [[]*n_joints,[]*n_joints] - for i, joint in enumerate(config["joint_names"]): + rconf["joint_lim"] = [[]*n_joints,[]*n_joints] + for i, joint in enumerate(rconf["joint_names"]): while True: try: print("-" * 50) @@ -311,39 +315,113 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot | None]: print('--' * 50) print(" !!! Invalid input. Lower limit must be less than upper limit. !!! ") continue - config["joint_lim"][0][i] = float(lower_limit) if lower_limit!="" else None - config["joint_lim"][1][i] = float(upper_limit) if upper_limit!="" else None + rconf["joint_lim"][0][i] = float(lower_limit) if lower_limit!="" else None + rconf["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: {config["joint_lim"][0]}\n upper: {config["joint_lim"][1]}') + print(f'New limits for joint:\n lower: {rconf["joint_lim"][0]}\n upper: {rconf["joint_lim"][1]}') break elif update_limits == 'n': - config["joint_lim"] = None + rconf["joint_lim"] = None break print("Invalid input. Please enter 'y' or 'n'.") while True: try: print('+-' * 50) - config["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 config["trajectory_topic_name"] == "": - config["trajectory_topic_name"] = '/scaled_joint_trajectory_controller/joint_trajectory' + rconf["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 rconf["trajectory_topic_name"] == "": + rconf["trajectory_topic_name"] = '/scaled_joint_trajectory_controller/joint_trajectory' break - elif config["trajectory_topic_name"].startswith("/"): + elif rconf["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}") + print('--' * 50) + while True: + try: + print('+-' * 50) + rconf["hz"] = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ") + if rconf["hz"] == "": + rconf["hz"] = 100 + else: + rconf["hz"] = float(rconf["hz"]) + break + except ValueError: + print("Invalid input. Please enter a valid number.") + continue + while True: + try: + print('+-' * 50) + nconf["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 nconf["log_ip"] == "": + nconf["log_ip"] = "127.0.0.1" + print('--' * 50) + nconf["log_port"] = input("Enter the target port for the log messages (or press Enter for default: 5005): ") + if nconf["log_port"] == "": + nconf["log_port"] = 5005 + else: + nconf["log_port"] = int(nconf["log_port"]) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + nconf["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 nconf["state_ip"] == "": + nconf["state_ip"] = "127.0.0.1" + print('--' * 50) + nconf["state_port"] = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ") + if nconf["state_port"] == "": + nconf["state_port"] = 7000 + else: + nconf["state_port"] = int(nconf["state_port"]) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + nconf["commands_port"] = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") + if nconf["commands_port"] == nconf["state_port"]: + print("The commands port must be different from the state port.") + continue + if nconf["commands_port"] == "": + nconf["commands_port"] = 8000 + else: + nconf["commands_port"] = int(nconf["commands_port"]) + break + except ValueError: + print("Invalid input. Please enter a valid port.") + continue + print() + print('=-=' * 50) + print() + print(f"Sending joint state's' to {nconf['state_ip']}:{nconf['state_port']}") + print() + print('=-=' * 50) + print() + print(f"Sending log messages to {nconf['log_ip']}:{nconf['log_port']}") + print() + print('=-=' * 50) + print() + print(f"Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{nconf['commands_port']}") + print() + print('=-=' * 50) + print() + config["robot"] = rconf + config["network"] = nconf while True: save_config = input("Do you want to save this interface? (y/n): ").strip().lower() if save_config == "y": config_path = input("Where do you want to save the config?: ").strip() - if not os.path.isdir(config_path): - print("Warning: Path does not exist") - print("Making Directory...") - os.mkdir(config_path) - print(f"[TODO] osc_ros2.py:1091:0: Write config to {config_path}/config.toml") + with open(f"{config_path}.toml", "w") as f: + toml.dump(config, f) break elif save_config == "n": print("Will not be saving config") diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 5d64fdf..0c378cb 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -1,5 +1,7 @@ #ruff: noqa: F403, F405 import rclpy +import argparse +import toml from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sensor_msgs.msg import JointState @@ -10,6 +12,7 @@ import numpy as np import spatialmath as sm import roboticstoolbox as rtb from osc4py3 import oscbuildparse +from osc_ros2.interactive_input import interactive_input import time import re import socket @@ -62,62 +65,9 @@ class OSC_ROS2_interface(Node): 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 @@ -127,34 +77,6 @@ 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) - 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}') @@ -764,9 +686,15 @@ class OSC_ROS2_interface(Node): 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]") + parser.add_argument("-c", "--config", type=argparse.FileType("r"), help="A robot TOML config, if the config is provided then the interface will not show the iteractive input") + args = parser.parse_args() rclpy.init() - # TODO: if no config use interactive - config, robot = interactive_input() + if not args.config: + config, robot = interactive_input() + else: + config = toml.load(args.config) + robot = rtb.ERobot.URDF(config["robot"]["urdf"]) node = OSC_ROS2_interface(robot, config) # Run ROS 2 spin, and osc_process will be handled by the timer diff --git a/workspace/ur10e.toml b/workspace/ur10e.toml new file mode 100644 index 0000000..e345f8a --- /dev/null +++ b/workspace/ur10e.toml @@ -0,0 +1,27 @@ +[robot] +urdf = "/workspace/ur10e.urdf" +joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint",] +cost_mask_string = "111111" +x_limits = [ "None", "None",] +y_limits = [ "None", "None",] +z_limits = [ "None", "None",] +x_limits_workspace = [ "None", "None",] +y_limits_workspace = [ "None", "None",] +z_limits_workspace = [ "None", "None",] +trajectory_topic_name = "/scaled_joint_trajectory_controller/joint_trajectory" +hz = 100 + +[network] +log_ip = "127.0.0.1" +log_port = 5005 +state_ip = "127.0.0.1" +state_port = 7000 +commands_port = 8000 + +[robot.joint_velocity_limits] +shoulder_pan_joint = 2.0943951023931953 +shoulder_lift_joint = 2.0943951023931953 +elbow_joint = 3.141592653589793 +wrist_1_joint = 3.141592653589793 +wrist_2_joint = 3.141592653589793 +wrist_3_joint = 3.141592653589793 From 4ab2ff3c3b6e8c2398e36ad5e607e634a99fe2d3 Mon Sep 17 00:00:00 2001 From: Ali Date: Sun, 8 Jun 2025 22:13:18 +0200 Subject: [PATCH 4/4] [AN] (feat) Interface with config fully works --- examples/log.py | 19 +++ examples/move.py | 45 ++++++ examples/oscpy/__init__.py | 138 ++++++++++++++++++ examples/state.py | 27 ++++ examples/test.py | 7 + .../osc_ros2/osc_ros2/interactive_input.py | 10 +- workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 102 +++++-------- 7 files changed, 275 insertions(+), 73 deletions(-) create mode 100644 examples/log.py create mode 100644 examples/move.py create mode 100644 examples/oscpy/__init__.py create mode 100644 examples/state.py create mode 100644 examples/test.py diff --git a/examples/log.py b/examples/log.py new file mode 100644 index 0000000..15dc442 --- /dev/null +++ b/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) + diff --git a/examples/move.py b/examples/move.py new file mode 100644 index 0000000..59336a7 --- /dev/null +++ b/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") + diff --git a/examples/oscpy/__init__.py b/examples/oscpy/__init__.py new file mode 100644 index 0000000..fb3dc20 --- /dev/null +++ b/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 diff --git a/examples/state.py b/examples/state.py new file mode 100644 index 0000000..d9d5a17 --- /dev/null +++ b/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) + + diff --git a/examples/test.py b/examples/test.py new file mode 100644 index 0000000..72797f5 --- /dev/null +++ b/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) + diff --git a/workspace/src/osc_ros2/osc_ros2/interactive_input.py b/workspace/src/osc_ros2/osc_ros2/interactive_input.py index d32e3b0..83cb4d7 100644 --- a/workspace/src/osc_ros2/osc_ros2/interactive_input.py +++ b/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 = [int(i) for i in rconf["cost_mask_string"]] - if sum(cost_mask) <= robot.n and len(cost_mask) == 6: + cost_mask_string = input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ") + rconf["cost_mask"] = [int(i) for i in cost_mask_string] + 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: diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 0c378cb..e854cf7 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/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_velocity_limits = joint_velocity_limits - self.cost_mask = cost_mask + self.joint_names = config["robot"]["joint_names"] + self.joint_velocity_limits = config["robot"]["joint_velocity_limits"] + 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(log_ip, log_port, "osc_log_client") - osc_udp_server('0.0.0.0', commands_port, "osc_server") - + osc_udp_client(self.state_ip, self.state_port, "osc_client") + osc_udp_client(self.log_ip, self.log_port, "osc_log_client") + 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'Sending joint states to {state_ip}:{state_port}') - self.get_logger().info(f'Sending log messages to {log_ip}:{log_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 {self.state_ip}:{self.state_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: - 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("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 + 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("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}") @@ -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]) @@ -190,7 +194,6 @@ class OSC_ROS2_interface(Node): 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.""" @@ -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]")