From 09644709f9894cccf9a003e910db4c2dfa7cd64a Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Mon, 26 May 2025 23:38:12 +0200 Subject: [PATCH] AS: commit before final presentation --- .DS_Store | Bin 8196 -> 8196 bytes workspace/build/.built_by | 1 - workspace/build/COLCON_IGNORE | 0 .../osc_ros2/build/lib/osc_ros2/__init__.py | 0 .../osc_ros2/build/lib/osc_ros2/osc_ros2.py | 1048 ----------------- workspace/build/osc_ros2/colcon_build.rc | 1 - .../colcon_command_prefix_setup_py.sh | 1 - .../colcon_command_prefix_setup_py.sh.env | 19 - workspace/build/osc_ros2/install.log | 14 - .../build/osc_ros2/osc_ros2.egg-info/PKG-INFO | 12 - .../osc_ros2/osc_ros2.egg-info/SOURCES.txt | 16 - .../osc_ros2.egg-info/dependency_links.txt | 1 - .../osc_ros2.egg-info/entry_points.txt | 3 - .../osc_ros2/osc_ros2.egg-info/requires.txt | 6 - .../osc_ros2/osc_ros2.egg-info/top_level.txt | 1 - .../build/osc_ros2/osc_ros2.egg-info/zip-safe | 1 - .../__pycache__/sitecustomize.cpython-310.pyc | Bin 283 -> 0 bytes .../osc_ros2/prefix_override/sitecustomize.py | 4 - workspace/install/.colcon_install_layout | 1 - workspace/install/COLCON_IGNORE | 0 workspace/install/_local_setup_util_ps1.py | 407 ------- workspace/install/_local_setup_util_sh.py | 407 ------- workspace/install/local_setup.bash | 121 -- workspace/install/local_setup.ps1 | 55 - workspace/install/local_setup.sh | 137 --- workspace/install/local_setup.zsh | 134 --- .../install/osc_ros2/lib/osc_ros2/interface | 33 - .../osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO | 12 - .../SOURCES.txt | 16 - .../dependency_links.txt | 1 - .../entry_points.txt | 3 - .../requires.txt | 6 - .../top_level.txt | 1 - .../osc_ros2-1.0.0-py3.10.egg-info/zip-safe | 1 - .../site-packages/osc_ros2/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 173 -> 0 bytes .../__pycache__/osc_ros2.cpython-310.pyc | Bin 33124 -> 0 bytes .../site-packages/osc_ros2/osc_ros2.py | 1048 ----------------- .../resource_index/packages/osc_ros2 | 0 .../share/colcon-core/packages/osc_ros2 | 1 - .../share/osc_ros2/hook/ament_prefix_path.dsv | 1 - .../share/osc_ros2/hook/ament_prefix_path.ps1 | 3 - .../share/osc_ros2/hook/ament_prefix_path.sh | 3 - .../share/osc_ros2/hook/pythonpath.dsv | 1 - .../share/osc_ros2/hook/pythonpath.ps1 | 3 - .../share/osc_ros2/hook/pythonpath.sh | 3 - .../osc_ros2/share/osc_ros2/package.bash | 31 - .../osc_ros2/share/osc_ros2/package.dsv | 6 - .../osc_ros2/share/osc_ros2/package.ps1 | 116 -- .../osc_ros2/share/osc_ros2/package.sh | 87 -- .../osc_ros2/share/osc_ros2/package.xml | 20 - .../osc_ros2/share/osc_ros2/package.zsh | 42 - workspace/install/setup.bash | 31 - workspace/install/setup.ps1 | 29 - workspace/install/setup.sh | 45 - workspace/install/setup.zsh | 31 - workspace/log/COLCON_IGNORE | 0 .../log/build_2025-05-25_15-14-38/events.log | 52 - .../build_2025-05-25_15-14-38/logger_all.log | 126 -- .../osc_ros2/command.log | 2 - .../osc_ros2/stderr.log | 0 .../osc_ros2/stdout.log | 35 - .../osc_ros2/stdout_stderr.log | 35 - .../osc_ros2/streams.log | 37 - workspace/log/latest | 1 - workspace/log/latest_build | 1 - .../__pycache__/osc_ros2.cpython-310.pyc | Bin 33165 -> 34230 bytes workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 87 +- 68 files changed, 69 insertions(+), 4271 deletions(-) delete mode 100644 workspace/build/.built_by delete mode 100644 workspace/build/COLCON_IGNORE delete mode 100644 workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py delete mode 100644 workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py delete mode 100644 workspace/build/osc_ros2/colcon_build.rc delete mode 100644 workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh delete mode 100644 workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env delete mode 100644 workspace/build/osc_ros2/install.log delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt delete mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe delete mode 100644 workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc delete mode 100644 workspace/build/osc_ros2/prefix_override/sitecustomize.py delete mode 100644 workspace/install/.colcon_install_layout delete mode 100644 workspace/install/COLCON_IGNORE delete mode 100644 workspace/install/_local_setup_util_ps1.py delete mode 100644 workspace/install/_local_setup_util_sh.py delete mode 100644 workspace/install/local_setup.bash delete mode 100644 workspace/install/local_setup.ps1 delete mode 100644 workspace/install/local_setup.sh delete mode 100644 workspace/install/local_setup.zsh delete mode 100755 workspace/install/osc_ros2/lib/osc_ros2/interface delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc delete mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py delete mode 100644 workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 delete mode 100644 workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.bash delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.dsv delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.ps1 delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.sh delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.xml delete mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.zsh delete mode 100644 workspace/install/setup.bash delete mode 100644 workspace/install/setup.ps1 delete mode 100644 workspace/install/setup.sh delete mode 100644 workspace/install/setup.zsh delete mode 100644 workspace/log/COLCON_IGNORE delete mode 100644 workspace/log/build_2025-05-25_15-14-38/events.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/logger_all.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/osc_ros2/command.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/osc_ros2/stderr.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout_stderr.log delete mode 100644 workspace/log/build_2025-05-25_15-14-38/osc_ros2/streams.log delete mode 120000 workspace/log/latest delete mode 120000 workspace/log/latest_build diff --git a/.DS_Store b/.DS_Store index 885449cf83d908b5243728d504a2145a5b07636e..65bef6e5dff87cf850de8ac577bca959bd57145d 100644 GIT binary patch delta 470 zcmZp1XmOa}FDk;oz`)4BAi%)j$l&Celb@WFlb^J)a2or>2Hwr=94s95AQfy3zCig9 z1|MWqKyjcphV?+K_a6*^ECvQXhJ1!%hGd3#h9ZVkAe{*$%Yayqp@5+hW&=ARXTL~9xNLE*y7#Qm)7@AsG)aocyTN)YY zD3}2Hwr=94s7+{F4L~CO;8) zK3Pzxo{b>~s9bRJG$DD$9g{Of3e_d5tBuS}brejEO>1=&sx6HSbQDaC&1!2oIYgE9 zt%KsTb8_?YyC(Y!%QJRQ&KLHd{7$$Eq1V_FsMo~Om= 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 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 = [None, None] - self.y_limits = [None, None] - self.z_limits = [None, None] - break - print("Invalid input. Please enter 'y' or 'n'.") - # Ask the user if they want to set new joint limits - while True: - 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 to keep current): ").strip() - upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").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.") - break - elif update_limits == 'n': - self.joint_lim = None - break - print("Invalid input. Please enter 'y' or 'n'.") - - print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}') - - - while True: - try: - print('+-' * 50) - self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() - if self.trajectory_topic_name == "": - self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' - break - elif self.trajectory_topic_name.startswith("/"): - break - else: - print("Invalid topic name. A valid topic name should start with '/'.") - except Exception as e: - print(f"An error occurred: {e}") - - # ROS2 Publisher - self.publisher = self.create_publisher( - JointTrajectory, - self.trajectory_topic_name, - 1 - ) - - while True: - try: - print('+-' * 50) - log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if log_ip == "": - log_ip = "127.0.0.1" - print('--' * 50) - log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") - if log_port == "": - log_port = 5005 - else: - log_port = int(log_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if state_ip == "": - state_ip = "127.0.0.1" - print('--' * 50) - state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ") - if state_port == "": - state_port = 7000 - else: - state_port = int(state_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") - if commands_port == state_port: - print("The commands port must be different from the state port.") - continue - if commands_port == "": - commands_port = 8000 - else: - commands_port = int(commands_port) - break - except ValueError: - print("Invalid input. Please enter a valid port.") - continue - - - osc_startup() - - osc_udp_client(state_ip, state_port, "osc_client") - - osc_udp_client(log_ip, log_port, "osc_log_client") - - osc_udp_server('0.0.0.0', commands_port, "osc_server") - - # Register OSC handler - osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_ADDRESS+osm.OSCARG_DATAUNPACK) - osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK) - print('--' * 50) - while True: - try: - print('+-' * 50) - self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ") - if self.hz == "": - self.hz = 100 - else: - self.hz = float(self.hz) - break - except ValueError: - print("Invalid input. Please enter a valid number.") - continue - print() - print('=-=' * 50) - print() - print(f'Sending joint states to {state_ip}:{state_port}') - print() - print('=-=' * 50) - print() - print(f'Sending log messages to {log_ip}:{log_port}') - print() - print('=-=' * 50) - print() - print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') - print() - print('=-=' * 50) - print() - - self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}") - self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') - self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}') - self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}') - self.create_timer(1/self.hz, self.update_position) # Timer to update the position - self.create_timer(3, self.reset_prev) # reset the previous desired position - - def reset_prev(self): self.previous_desired = None - - def speed_scaling_handler(self, *args): - """Handles incoming OSC messages for speed scaling.""" - try: - if len(args) == 1: - if args[0] < 0: - self.speed_scaling = -float(args[0]) - else: - self.speed_scaling = float(args[0]) - if self.speed_scaling > 1: - self.get_logger().warn(f"speed_scaling_handler: Attention! Speed scaling {self.speed_scaling} is greater than 1!") - self.get_logger().info(f"Speed scaling set to {self.speed_scaling}") - else: - self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.") - except Exception as e: - self.get_logger().fatal(f"speed_scaling_handler: {e}") - - def joint_trajectory_handler(self, *args): - try: - if len(args[0]) == 6: - points = [[float(j) for j in i] for i in args] - elif len(args[0]) >= 7: - points = [[float(j) for j in i[:6]] for i in args] - self.get_logger().warn(f"joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.") - else: - self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.") - return - - self.desired = ["joint_trajectory"] + points - self.new = True - except Exception as e: - self.get_logger().fatal(f"joint_trajectory_handler: {e}") - - def joint_position_handler(self, address, *args): - """Handles incoming OSC messages for joint positions.""" - try: - joint_name = address.split("/")[-1] - - if joint_name in self.joint_names: - if len(args) == 1: - position = float(args[0]) - index = self.joint_names.index(joint_name) - if self.robot: - if position < self.robot.qlim[0][index]: - position = self.robot.qlim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}." - ) - elif position > self.robot.qlim[1][index]: - position = self.robot.qlim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}." - ) - else: - if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]: - position = self.joint_lim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}." - ) - elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]: - position = self.joint_lim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}." - ) - desired_joint_positions = self.current_joint_positions - desired_joint_positions[index] = position - self.desired = ["joint_positions"] + desired_joint_positions - self.new = True - elif len(args) == 2: - position = float(args[0]) - duration = float(args[1]) - index = self.joint_names.index(joint_name) - if self.robot: - if position < self.robot.qlim[0][index]: - position = self.robot.qlim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}." - ) - elif position > self.robot.qlim[1][index]: - position = self.robot.qlim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}." - ) - else: - if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]: - position = self.joint_lim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}." - ) - elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]: - position = self.joint_lim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}." - ) - desired_joint_positions = self.current_joint_positions - desired_joint_positions[index] = position - self.desired = ["joint_positions"] + desired_joint_positions + [duration] - self.new = True - else: - self.get_logger().warn(f"joint_position_handler: Invalid number of arguments for joint position. Expected 1, but got {len(args)}.") - else: - self.get_logger().warn(f"joint_position_handler: Joint '{joint_name}' not found in the robot model.") - except Exception as e: - self.get_logger().fatal(f"joint_position_handler: {e}") - - - def cartesian_trajectory_handler(self, *args): - """Handles incoming OSC messages for cartesian trajectory.""" - if self.robot: - try: - if len(args[0]) == 6: - points = [[float(j) for j in i] for i in args] - elif len(args[0]) >= 7: - points = [[float(j) for j in i[:6]] for i in args] - self.get_logger().warn(f"cartesian_trajectory_handler: Duration is not supported for cartesian trajectory yet. Ignoring duration.") - else: - self.get_logger().warn(f"cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.") - return - - self.desired = ["cartesian_trajectory"] + points - self.new = True - except Exception as e: - self.get_logger().fatal(f"cartesian_trajectory_handler: {e}") - else: - self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.") - return - - def joint_positions_handler(self, *args): - """Handles incoming OSC messages for joint positions.""" - try: - if len(args) == len(self.joint_names): - desired_joint_positions = [float(i) for i in list(args)] - elif len(args) == len(self.joint_names) + 1: - desired_joint_positions = [float(i) for i in list(args)] - else: - self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.") - return - - # Check if joint positions exceed limits - if self.robot: - for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present - if position < self.robot.qlim[0][i]: - desired_joint_positions[i] = self.robot.qlim[0][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}." - ) - elif position > self.robot.qlim[1][i]: - desired_joint_positions[i] = self.robot.qlim[1][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}." - ) - else: - for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): - if self.joint_lim[0][i] is not None and position < self.joint_lim[0][i]: - desired_joint_positions[i] = self.joint_lim[0][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}." - ) - elif self.joint_lim[1][i] is not None and position > self.joint_lim[1][i]: - desired_joint_positions[i] = self.joint_lim[1][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}." - ) - - self.desired = ["joint_positions"] + desired_joint_positions - self.new = True - except Exception as e: - self.get_logger().fatal(f"joint_positions_handler: {e}") - return - - def tcp_coordinates_handler(self, *args): - # Ensure the desired joint positions are within the specified limits - if self.robot: - try: - if len(args) == 6: - x, y, z, r, p, yaw = [float(i) for i in list(args)] - duration = None - elif len(args) >= 7: - x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)] - else: - self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.") - return - - if self.x_limits[0] is not None: - x = max(self.x_limits[0], x) - if self.x_limits[1] is not None: - x = min(self.x_limits[1], x) - if self.y_limits[0] is not None: - y = max(self.y_limits[0], y) - if self.y_limits[1] is not None: - y = min(self.y_limits[1], y) - if self.z_limits[0] is not None: - z = max(self.z_limits[0], z) - if self.z_limits[1] is not None: - z = min(self.z_limits[1], z) - - if x != args[0] or y != args[1] or z != args[2]: - self.get_logger().warn( - f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: " - f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})" - ) - - self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration] - self.new = True - except Exception as e: - self.get_logger().fatal(f"tcp_coordinates_handler: {e}") - else: - self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.") - return - - - def joint_states_callback(self, msg: JointState): - """Callback function to handle incoming joint states.""" - try: - msg_time = oscbuildparse.OSCMessage(f"/time", ',s', [str(time.time())]) - osc_send(msg_time, "osc_client") - if not(self.joint_names): self.joint_names = msg.name - joint_position_dict = dict(zip(msg.name, msg.position)) - self.current_joint_positions = [float(joint_position_dict[name]) for name in self.joint_names] - joint_position_dict = dict(zip(msg.name, msg.velocity)) - self.current_joint_velocities = [float(joint_position_dict[name]) for name in self.joint_names] - - if self.robot: - tcp_position = self.robot.fkine(self.current_joint_positions).t - tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy() - - msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]]) - #msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]]) - #msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]]) - #msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]]) - #msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]]) - #msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]]) - #msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]]) - #bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw]) - #osc_send(bun, "osc_client") - osc_send(msg_tcp, "osc_client") - - msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position]) - msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity]) - msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort]) - msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name]) - bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort]) - osc_send(bun, "osc_client") - - #for i, name in enumerate(msg.name): - # msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]]) - # msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]]) - # msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]]) - # bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort]) - # osc_send(bun, "osc_client") - - except Exception as e: - self.get_logger().fatal(f"joint_states_callback: {e}") - - def send_joint_positions(self): - self.previous_desired = None - self.new = False - try: - if len(self.desired) == len(self.joint_names) + 2: - desired_joint_positions = [float(i) for i in self.desired[1:-1]] - duration = self.desired[-1] - msg = JointTrajectory() - msg.joint_names = self.joint_names - point = JointTrajectoryPoint() - point.positions = desired_joint_positions - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - self.publisher.publish(msg) - elif len(self.desired) == len(self.joint_names) + 1: - desired_joint_positions = [float(i) for i in self.desired[1:]] - msg = JointTrajectory() - msg.joint_names = self.joint_names - point = JointTrajectoryPoint() - point.positions = desired_joint_positions - duration = 0 - for p1, p2, max_vel in zip(desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()): - duration = max(duration, abs(p1 - p2) / max_vel) - duration = duration + min(2, 0.2*duration) - duration /= self.speed_scaling - if duration == 0: - self.get_logger().warn("send_joint_positions: Duration is 0.") - return - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - self.publisher.publish(msg) - else: - self.get_logger().warn(f"send_joint_positions: Invalid number of arguments for joint positions. Expected {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]) or {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]), but got {len(self.desired)}.") - return - except Exception as e: - self.get_logger().fatal(f"send_joint_positions: {e}") - return - - def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3): - - if num_points == 2: - return [0, total_duration] - n = int(num_points*(1-flat_ratio)/2) - start = np.cos(np.linspace(0, np.pi, n))+2 - end = np.cos(np.linspace(-np.pi, 0, n))+2 - flat = np.ones(num_points-2*n) - - timestamps = np.concatenate((start, flat, end)) - timestamps *= total_duration / timestamps.sum() - timestamps = np.cumsum(timestamps) - - return timestamps.tolist() - - - def send_tcp_coordinates(self): - """Send the desired TCP coordinates to the robot.""" - try: - self.new = False - msg = JointTrajectory() - msg.joint_names = self.joint_names - steps_per_m = 100 - if self.previous_desired == None: - [x,y,z] = self.robot.fkine(self.current_joint_positions).t - [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() - else: - [x,y,z] = self.previous_desired[1:4] - [roll, pitch, yaw] = self.previous_desired[4:-1] - x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] - self.previous_desired = self.desired - steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m) - if steps < 2: steps = 2 - cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)] - '''if self.previous_desired: - [x,y,z] = self.previous_desired[1:4] - q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5]) - else: - [x, y, z] = self.robot.fkine(self.current_joint_positions).t - q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R) - - x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] - q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1) - - steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m) - if steps < 2: - steps = 2 - - cart_traj = [] - for i in range(steps): - alpha = i / (steps - 1) - - # Convert to arrays for robust interpolation if needed - q0_array = q0.vec - q1_array = q1.vec - dot = np.dot(q0_array, q1_array) - dot = np.clip(dot, -1.0, 1.0) - - if abs(dot) > 0.9995: - # Linear interpolation + normalization - q_interp_array = (1 - alpha) * q0_array + alpha * q1_array - q_interp_array = q_interp_array / np.linalg.norm(q_interp_array) - q_interp = sm.UnitQuaternion(q_interp_array) - else: - q_interp = q0.interp(q1, alpha) - - # Interpolate translation - pos_interp = [ - x + (x1 - x) * alpha, - y + (y1 - y) * alpha, - z + (z1 - z) * alpha - ] - - # Compose SE3 transform - cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())''' - - - - if self.desired[-1]: - timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8) - for j in range(steps): - sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan') - if sol[1] == 1: - fowards = self.robot.fkine_all(sol[0]) - out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) - if np.any(out_of_bounds): - #print(fowards.t) - #indices = np.where(out_of_bounds)[0] - #print(f"indices: {indices}") - self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!") - ''' - for i in indices: - try: - print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}") - except IndexError: - print(f"index {i} is out of bounds, but no corresponding joint found.") - self.previous_desired_tcp_position = self.desired_tcp_position - ''' - break - duration = timestamps[j] - if duration == 0: - prev_sol = list(sol[0]) - continue - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - prev_sol = list(sol[0]) - else: - self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!") - prev_sol = self.current_joint_positions - if len(msg.points) == 0: - self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.") - self.previous_desired = self.desired - return - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = self.desired - - - - else: - prev_duration = 0 - ''' - if self.previous_desired == None: - [x,y,z] = self.robot.fkine(self.current_joint_positions).t - [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() - else: - [x,y,z] = self.previous_desired[:3] - [roll, pitch, yaw] = self.previous_desired[3:] - ''' - for j in range(steps): - sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan') - if sol[1] == 1: - fowards = self.robot.fkine_all(sol[0]) - out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) - if np.any(out_of_bounds): - #print(fowards.t) - #indices = np.where(out_of_bounds)[0] - #print(f"indices: {indices}") - self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!") - ''' - for i in indices: - try: - print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}") - except IndexError: - print(f"index {i} is out of bounds, but no corresponding joint found.") - self.previous_desired_tcp_position = self.desired_tcp_position - ''' - break - duration = 0 - prev = self.current_joint_positions if j == 0 else prev_sol - for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()): - duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun - prev_sol = list(sol[0]) - if duration == 0: - continue - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - duration *= 1.2 - duration /= self.speed_scaling - duration += prev_duration - prev_duration = duration - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - else: - self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!") - prev_sol = self.current_joint_positions - if len(msg.points) == 0: - self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.") - self.previous_desired = self.desired - return - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = self.desired - except Exception as e: - self.get_logger().fatal(f"send_tcp_coordinates: {e}") - - def send_joint_trajectory(self): - - try: - self.new = False - viapoints = np.array([i for i in self.desired[1:]]) - msg = JointTrajectory() - msg.joint_names = self.joint_names - traj = rtb.mstraj(viapoints, q0 = self.current_joint_positions ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=[self.speed_scaling*i for i in self.joint_velocity_limits.values()]) - msg.points = [] - for i in range(len(traj.q)): - point = JointTrajectoryPoint() - point.positions = list(traj.q[i]) - point.time_from_start.sec = int(traj.t[i]) - point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9) - msg.points.append(point) - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = None - except Exception as e: - print(f'Error in joint_angles_handler: {e}') - - def send_cartesian_trajectory(self): - - try: - self.new = False - viapoints = np.array([i[:6] for i in self.desired[1:]]) - msg = JointTrajectory() - msg.joint_names = self.joint_names - x,y,z = self.robot.fkine(self.current_joint_positions).t - r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy() - q0 = [x, y, z, r, p, yaw] - traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=2*self.speed_scaling) - msg.points = [] - prev_sol = self.current_joint_positions - n = max(int(len(traj.q)/200), 1) - for i in range(len(traj.q)): - T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz') - sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True) - if sol[1] == 1: - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - point.time_from_start.sec = int(traj.t[i]) - if traj.t[i] == 0: - continue - point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9) - msg.points.append(point) - prev_sol = list(sol[0]) - else: self.get_logger().warn(f"send_cartesian_trajectory: IK could not find a solution for (x,y,z) = {traj.q[i][:3]} and (r,p,y) = {traj.q[i][3:]}!") - msg.header.stamp = self.get_clock().now().to_msg() - msg.points = msg.points[::n] - self.publisher.publish(msg) - self.previous_desired = None - except Exception as e: - print(f'Error in joint_angles_handler: {e}') - - self.previous_desired = None - - def update_position(self): - """Calls the appropriate function to update the robot's position.""" - try: - if self.desired is None or not(self.new): - return - - if self.desired[0] == "joint_positions": - self.new = False - self.send_joint_positions() - return - elif self.desired[0] == "tcp_coordinates": - self.new = False - self.send_tcp_coordinates() - return - elif self.desired[0] == "joint_trajectory": - self.new = False - self.send_joint_trajectory() - return - elif self.desired[0] == "cartesian_trajectory": - self.new = False - self.send_cartesian_trajectory() - return - else: - self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.") - return - - - except Exception as e: - self.get_logger().fatal(f'update_position: {e}') - - - def clean_log_string(self, s): - - s = str(s) - - # Remove ANSI escape sequences (e.g., \x1b[31m) - ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])') - s = ansi_escape.sub('', s) - - # Replace tabs/newlines with spaces - s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ') - - # Strip leading/trailing whitespace - s = s.strip() - - # Optionally enforce ASCII only (replace non-ASCII chars with '?') - s = s.encode('ascii', 'replace').decode('ascii') - - return s - - - def log_callback(self, msg: Log): - """Callback function to handle incoming log messages.""" - - # Send the log message as an OSC message - msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)]) - osc_send(msg_log, "osc_log_client") - - -def main(): - """Main function to get joint names and start the ROS 2 & OSC system.""" - rclpy.init() - while True: - use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower() - if use_urdf == 'y': - while True: - robot_urdf = input("Enter the absolute path to the URDF file: ") - if os.path.isfile(robot_urdf): - if not robot_urdf.endswith('.urdf'): - print("The file is not a URDF file. Please enter a valid URDF file.") - continue - break - else: - print("Invalid path. Please enter a valid path to the URDF file.") - tree = ET.parse(robot_urdf) - root = tree.getroot() - robot = rtb.ERobot.URDF(robot_urdf) - joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic'] - print(robot) - joint_velocity_limits = {} - - # Iterate over all joints in the URDF - for joint in root.findall('.//joint'): - joint_name = joint.get('name') # Get the name of the joint - - # Look for the tag under each joint - limit = joint.find('limit') - - if limit is not None: - # Extract the velocity limit (if it exists) - velocity_limit = limit.get('velocity') - - if velocity_limit is not None: - joint_velocity_limits[joint_name] = float(velocity_limit) - - while True: - try: - print('-+'*50) - print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].") - print("The cost mask 111000 means that the IK will only consider translation and no rotaion.") - cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")] - if sum(cost_mask) <= robot.n and len(cost_mask) == 6: - break - else: - print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.") - except ValueError: - print("Invalid input. Please enter integers only.") - print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}") - break - elif use_urdf == 'n': - node = JointNameListener() - print("Wainting 10 sec for JointState messages to extract joint names...") - rclpy.spin_once(node) - counter = 0 - while not(node.joint_names): - if counter > 100: - joint_names = None - break - counter+=1 - time.sleep(0.1) - rclpy.spin_once(node) - joint_names = node.joint_names - node.destroy_node() - if joint_names: - correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower() - while True: - if correct_names == 'y': - break - elif correct_names == 'n': - joint_names = None - break - correct_names = input("Invalid input. Please enter 'y' or 'n'.") - if not(joint_names): - print("Please enter the joint names manually.") - while True: - joint_names = [] - print('-+'*50) - print("Enter the joint names manually one by one. Type 'done' when you are finished:") - print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") - while True: - print('-'*50) - joint_name = input("Enter joint name (or 'done' to finish): ").strip() - if joint_name.lower() == 'done': - break - if joint_name: - joint_names.append(joint_name) - print('-+'*50) - correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() - if correct_names.lower() == 'y': - break - else: - print("Please re-enter the joint names.") - while True: - try: - joint_velocity_limits = {} - for name in joint_names: - while True: - try: - print('--'*50) - limit = input(f"Enter the velocity limit for joint '{name}': ").strip() - if limit == "": - continue - else: - joint_velocity_limits[name] = float(limit) - break - except ValueError: - print("Invalid input. Please enter a numeric value or press Enter to skip.") - break - except ValueError: - print("Invalid input. Please enter numeric values or leave blank to skip.") - robot = None - cost_mask = None - break - print("Invalid input. Please enter 'y' or 'n'.") - - - node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask) - - # Run ROS 2 spin, and osc_process will be handled by the timer - try: - rclpy.spin(node) - except KeyboardInterrupt: - print("") - finally: - node.destroy_node() - osc_terminate() - -if __name__ == '__main__': - main() diff --git a/workspace/build/osc_ros2/colcon_build.rc b/workspace/build/osc_ros2/colcon_build.rc deleted file mode 100644 index 573541a..0000000 --- a/workspace/build/osc_ros2/colcon_build.rc +++ /dev/null @@ -1 +0,0 @@ -0 diff --git a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh deleted file mode 100644 index f9867d5..0000000 --- a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh +++ /dev/null @@ -1 +0,0 @@ -# generated from colcon_core/shell/template/command_prefix.sh.em diff --git a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env deleted file mode 100644 index c73f6a0..0000000 --- a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env +++ /dev/null @@ -1,19 +0,0 @@ -AMENT_PREFIX_PATH=/opt/ros/humble -COLCON=1 -HOME=/root -HOSTNAME=0e38e264ac6b -LANG=C.UTF-8 -LC_ALL=C.UTF-8 -LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib -LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36: -OLDPWD=/BA -PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin -PWD=/BA/workspace/build/osc_ros2 -PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages -ROS_DISTRO=humble -ROS_LOCALHOST_ONLY=0 -ROS_PYTHON_VERSION=3 -ROS_VERSION=2 -SHLVL=1 -TERM=xterm -_=/usr/bin/colcon diff --git a/workspace/build/osc_ros2/install.log b/workspace/build/osc_ros2/install.log deleted file mode 100644 index 8ab07e3..0000000 --- a/workspace/build/osc_ros2/install.log +++ /dev/null @@ -1,14 +0,0 @@ -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc -/BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 -/BA/workspace/install/osc_ros2/share/osc_ros2/package.xml -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt -/BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt -/BA/workspace/install/osc_ros2/lib/osc_ros2/interface diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO b/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO deleted file mode 100644 index 275623c..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO +++ /dev/null @@ -1,12 +0,0 @@ -Metadata-Version: 2.1 -Name: osc-ros2 -Version: 1.0.0 -Summary: Creates an interface for communication between OSC and Ros2 -Home-page: UNKNOWN -Maintainer: Alexander Schaefer -Maintainer-email: a.schaefer@tuhh.de -License: Apache-2.0 -Platform: UNKNOWN - -UNKNOWN - diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt deleted file mode 100644 index 650a016..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt +++ /dev/null @@ -1,16 +0,0 @@ -package.xml -setup.cfg -setup.py -../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO -../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt -../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt -../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt -../../build/osc_ros2/osc_ros2.egg-info/requires.txt -../../build/osc_ros2/osc_ros2.egg-info/top_level.txt -../../build/osc_ros2/osc_ros2.egg-info/zip-safe -osc_ros2/__init__.py -osc_ros2/osc_ros2.py -resource/osc_ros2 -test/test_copyright.py -test/test_flake8.py -test/test_pep257.py \ No newline at end of file diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt deleted file mode 100644 index 8b13789..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt deleted file mode 100644 index 008581c..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt +++ /dev/null @@ -1,3 +0,0 @@ -[console_scripts] -interface = osc_ros2.osc_ros2:main - diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt deleted file mode 100644 index c78337e..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt +++ /dev/null @@ -1,6 +0,0 @@ -numpy==1.22.4 -osc4py3 -roboticstoolbox-python==1.1.1 -scipy==1.7.3 -setuptools -spatialmath-python==1.1.14 diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt deleted file mode 100644 index 99ee4eb..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt +++ /dev/null @@ -1 +0,0 @@ -osc_ros2 diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe b/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe deleted file mode 100644 index 8b13789..0000000 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe +++ /dev/null @@ -1 +0,0 @@ - diff --git a/workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc b/workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc deleted file mode 100644 index 2f82eebe0694db39782da446e3f7c7f2ee26d9d3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 283 zcmY*Vu};H44D}_aDk`eP!ph#IC$%e}wjaRKjpY<_Mm>7vB0HgN{(w*6mol(0@e2$U z4sB7f-%vI2GUl5T2DwRta5o48bNY)v~o6 zPn}xG-2#UCOFVGjXj3ya*JeI5n@B4z^8$^3;VKWoL@q2v9uIY!@{ZeHN%-Wq?FC^T z>`CAji=%i4>PhS`eBb^LeXM*(r$cS;hu N`$PtPm8nam_dg0GO+5er diff --git a/workspace/build/osc_ros2/prefix_override/sitecustomize.py b/workspace/build/osc_ros2/prefix_override/sitecustomize.py deleted file mode 100644 index e2319f7..0000000 --- a/workspace/build/osc_ros2/prefix_override/sitecustomize.py +++ /dev/null @@ -1,4 +0,0 @@ -import sys -if sys.prefix == '/usr': - sys.real_prefix = sys.prefix - sys.prefix = sys.exec_prefix = '/BA/workspace/install/osc_ros2' diff --git a/workspace/install/.colcon_install_layout b/workspace/install/.colcon_install_layout deleted file mode 100644 index 3aad533..0000000 --- a/workspace/install/.colcon_install_layout +++ /dev/null @@ -1 +0,0 @@ -isolated diff --git a/workspace/install/COLCON_IGNORE b/workspace/install/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/workspace/install/_local_setup_util_ps1.py b/workspace/install/_local_setup_util_ps1.py deleted file mode 100644 index 3c6d9e8..0000000 --- a/workspace/install/_local_setup_util_ps1.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' -FORMAT_STR_USE_ENV_VAR = '$env:{name}' -FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/workspace/install/_local_setup_util_sh.py b/workspace/install/_local_setup_util_sh.py deleted file mode 100644 index f67eaa9..0000000 --- a/workspace/install/_local_setup_util_sh.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' -FORMAT_STR_USE_ENV_VAR = '${name}' -FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/workspace/install/local_setup.bash b/workspace/install/local_setup.bash deleted file mode 100644 index 03f0025..0000000 --- a/workspace/install/local_setup.bash +++ /dev/null @@ -1,121 +0,0 @@ -# generated from colcon_bash/shell/template/prefix.bash.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -else - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_bash_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" - unset _colcon_prefix_bash_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_bash_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/workspace/install/local_setup.ps1 b/workspace/install/local_setup.ps1 deleted file mode 100644 index 6f68c8d..0000000 --- a/workspace/install/local_setup.ps1 +++ /dev/null @@ -1,55 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix.ps1.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# check environment variable for custom Python executable -if ($env:COLCON_PYTHON_EXECUTABLE) { - if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { - echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" - exit 1 - } - $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" -} else { - # use the Python executable known at configure time - $_colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { - if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { - echo "error: unable to find python3 executable" - exit 1 - } - $_colcon_python_executable="python3" - } -} - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_powershell_source_script { - param ( - $_colcon_prefix_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_powershell_source_script_param'" - } - . "$_colcon_prefix_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" - } -} - -# get all commands in topological order -$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 - -# execute all commands in topological order -if ($env:COLCON_TRACE) { - echo "Execute generated script:" - echo "<<<" - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output - echo ">>>" -} -if ($_colcon_ordered_commands) { - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression -} diff --git a/workspace/install/local_setup.sh b/workspace/install/local_setup.sh deleted file mode 100644 index 5c15896..0000000 --- a/workspace/install/local_setup.sh +++ /dev/null @@ -1,137 +0,0 @@ -# generated from colcon_core/shell/template/prefix.sh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/BA/workspace/install" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX - return 1 - fi -else - _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_sh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" - unset _colcon_prefix_sh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_sh_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "_colcon_prefix_sh_source_script() { - if [ -f \"\$1\" ]; then - if [ -n \"\$COLCON_TRACE\" ]; then - echo \"# . \\\"\$1\\\"\" - fi - . \"\$1\" - else - echo \"not found: \\\"\$1\\\"\" 1>&2 - fi - }" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/workspace/install/local_setup.zsh b/workspace/install/local_setup.zsh deleted file mode 100644 index b648710..0000000 --- a/workspace/install/local_setup.zsh +++ /dev/null @@ -1,134 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix.zsh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -else - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -_colcon_prefix_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_zsh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # workaround SH_WORD_SPLIT not being set - _colcon_prefix_zsh_convert_to_array _values - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" - unset _colcon_prefix_zsh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_zsh_prepend_unique_value -unset _colcon_prefix_zsh_convert_to_array - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/workspace/install/osc_ros2/lib/osc_ros2/interface b/workspace/install/osc_ros2/lib/osc_ros2/interface deleted file mode 100755 index 61d20a1..0000000 --- a/workspace/install/osc_ros2/lib/osc_ros2/interface +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/python3 -# EASY-INSTALL-ENTRY-SCRIPT: 'osc-ros2==1.0.0','console_scripts','interface' -import re -import sys - -# for compatibility with easy_install; see #2198 -__requires__ = 'osc-ros2==1.0.0' - -try: - from importlib.metadata import distribution -except ImportError: - try: - from importlib_metadata import distribution - except ImportError: - from pkg_resources import load_entry_point - - -def importlib_load_entry_point(spec, group, name): - dist_name, _, _ = spec.partition('==') - matches = ( - entry_point - for entry_point in distribution(dist_name).entry_points - if entry_point.group == group and entry_point.name == name - ) - return next(matches).load() - - -globals().setdefault('load_entry_point', importlib_load_entry_point) - - -if __name__ == '__main__': - sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0]) - sys.exit(load_entry_point('osc-ros2==1.0.0', 'console_scripts', 'interface')()) diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO deleted file mode 100644 index 275623c..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO +++ /dev/null @@ -1,12 +0,0 @@ -Metadata-Version: 2.1 -Name: osc-ros2 -Version: 1.0.0 -Summary: Creates an interface for communication between OSC and Ros2 -Home-page: UNKNOWN -Maintainer: Alexander Schaefer -Maintainer-email: a.schaefer@tuhh.de -License: Apache-2.0 -Platform: UNKNOWN - -UNKNOWN - diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt deleted file mode 100644 index 650a016..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt +++ /dev/null @@ -1,16 +0,0 @@ -package.xml -setup.cfg -setup.py -../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO -../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt -../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt -../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt -../../build/osc_ros2/osc_ros2.egg-info/requires.txt -../../build/osc_ros2/osc_ros2.egg-info/top_level.txt -../../build/osc_ros2/osc_ros2.egg-info/zip-safe -osc_ros2/__init__.py -osc_ros2/osc_ros2.py -resource/osc_ros2 -test/test_copyright.py -test/test_flake8.py -test/test_pep257.py \ No newline at end of file diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt deleted file mode 100644 index 8b13789..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt +++ /dev/null @@ -1 +0,0 @@ - diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt deleted file mode 100644 index 008581c..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt +++ /dev/null @@ -1,3 +0,0 @@ -[console_scripts] -interface = osc_ros2.osc_ros2:main - diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt deleted file mode 100644 index c78337e..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt +++ /dev/null @@ -1,6 +0,0 @@ -numpy==1.22.4 -osc4py3 -roboticstoolbox-python==1.1.1 -scipy==1.7.3 -setuptools -spatialmath-python==1.1.14 diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt deleted file mode 100644 index 99ee4eb..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt +++ /dev/null @@ -1 +0,0 @@ -osc_ros2 diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe deleted file mode 100644 index 8b13789..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe +++ /dev/null @@ -1 +0,0 @@ - diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 030e934c8974a61184b337d29722d9f727eb138b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 173 zcmd1j<>g`kf-hG@GeGoX5P=LBfgA@QE@lA|DGb33nv8xc8Hzx{2;x_OzLTSVd45rL zaY15os(xl(aYxfqrpjNvbYTWp-kEYB54p he0*kJW=VX!UP0w84x8Nkl+v73JCIGqOhAH#0RRXHDv|&I diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc deleted file mode 100644 index 5fd4a3963643aace1c1e9c275ba8b1a65ec4a9a9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 33124 zcmd6Q3v?XUdEU=TP80T6r-A0h}omLMhTK~thAiIVk#Xp%B5Nb-7#S%3>Hc7d6J zNK6)r63T~UQ`bryC$Sx+V49>>n|_gZMdL zMqtG(trDB$Rj+9C`lPP$K3<8>8Yxc4~*cNe;=_rtu_|tCDE#WEtRBZ+q zZ7+gM&6%1nx#w#Z9=h>xZmC?c7K*l0nqqU&fAQeK*^8S}Y*MpglltY@v}S3R{%U$M zZpBN+v~C$z;#FfZaXDthCX=UQV@bbX+Mg)Sm!2v+ZmC+br?g1TyielC2BdN~b}6=s zYOiTptR7p{-MFRKW3z@8zh=~9g@lFJLeh$%a$~|@H_O*7R8f7Q64Zaa`=E+-_|nc> zY+@|sC!D2)lI^Dpg>tp*77G5lDZ7LW1!w7;Gi8?-+;XjICy?16jFc}-6)Tl<#i=>beuxrjo9^*2Om0q@IuX=a~6tIrGw=vdQ_<##84OP znsfMIrF`z-!ZK!{dT0L7_(7-amJT52Tydu4G;=kw=NFdkUUY9AhpG=jPBSzee+-lV zC!6#(KEct5j&(kK+xWaQGo_acEx&qI2_4sSL{ckfdchKs+ z1VrHuS$&tZ)!}Qg)%Dk6D~Wo->YvkJ)9eQ`G4G4kfVJ*b<{3nuk!!I>VglqFFcm6( zxFvq0wLXX+X^G#2_G7afE@{?=xw!LjdD__Wv`IbLpG3cqqQ%a}7C*=`)}{?HE4HNB zKOs-?mM5e&V&@Xhmv9e&2y*yGCzXGrh1ekfrZE4T$iImx(wcfw`6YHPVZVvK zZEo}p_giHA2Cc2^!>41f>%sFjwj6mjS##sr>RCzFlNjGIjOuobQe)&(^;9tO!_{@T z-yyTG1MTc&y;k>1vTERI7oS*zla_k&XJgfV{CbR-yZJS3`F;)CALez?x`R2Ni&eD? zvBl@A8!@x0&Sx#W;|!sRo36U!wyW+oRTGU?79Rtg3|YG)y-e5B(o5a`F?Wm1W9AYd z>yg-N+KWe5G8tUG-FV;J?AFz7fTt|#8V$x}58^ZR%!b&fW7e3;!_g07_69MSfgonz z>nSNYWbKcXJn(v4^=G9Eeam-D$<@0s!oN|^F?7v(*Ch>M?vgf_keEU1AZtF0-Y(kp zE^B-)j`-hGIjh*ASGwxCAofpGNr^?hRt$M{dtbe)o^AAgE{2$$GFFG`U3k00%``mS z`FbKUg5CA*U<3zLdyTwDW7R&SagKC*Gsf*M_OY65%m(`UH`R1|8T3>2$gSs4Q!sjS z2I{CtkKR+yp$~UA({9qE=heG5^0xG-gnr!9)+2rKi1Y{}+bmPhawZE~k^kPv?7Wj1 z4`2pR-6v43FMlS4>igIS1=Sc42ftc12dGrz_R}rn^?dcSEtu{|WF8uj9JikWI%~Z0*<1VVC>?{k9Hh8 zeeug^=Z~ws!Pv!Z;054>?U51G7k>f0y1&sb-t+;vM;pLr278O~xC40_d&x01!l-8_ za_+<$((#U$tOpr`KJ8=d>MrZ}H4X9q7%{|f52^RAZp3Wg@mhTOUuvm;V;)@mRox9ypX)%6{d*dGTYc%W^aqT0-Q!9P-zMxOP zW}R5+Lk>0eL)N26?XUL-slN5tNq_KfA-9oqYAX^$h<826`8YOp?7 zUx)EF?E8`ObO1}n;%I#^Xyt@8u`(#FU_W0OjI{Dx*vd1(TcO;An7fb6l)Jw^fS#Vb z6qoxxjMDq%{(yA~b)S~|ymbcSf6yJDJ!C!mQRd8BA3)4u#N5f)^|3JiEaLBK$;rL` zJuNZMA?EIum`TLk6Nw4t4kN!QG7`@Vg+k18Z}8Ut!6H3$FW{Ox@x3ZN^6tMMzWWQ| zyZujt;ldIZ1sUW;ARq{QIO zN&wQ2f>yiVI)``J$(0eXdn$H~+FJ6oyg^G!OP_<32P~|o*?Tt;`y9Il5dnJefHj4u z2l*uIq&4AhcfjqHtb*cFLSlKj5Kz%2+s=U+w!0B{se@qRYD?~07M}dXBPSku_A%dh zeB#k(eBd-RNF z?K|-I|L2WAQZtupOXh`Q)g_bQlw7k?o-e!Rbj>y|?l+gsV%0LeW9FXagVnJkrmrox zuhYJE!P zTY>ZZlH;1^00ZD|0Sv`z6b72r6)%>Zyx05il5J!90|+_d#V;N)z4$T%55aq)s|L%* zHtpKEnk=CC8ai37qKUF&+QsTjDYA5`-lpJ9?a~1SUILd~a8CaEXj}hVMrCw))Wj%^ zR!8&R_{}m_mmr`iPc??FR;?`Oz5UT12kX3Ax)2BhWQd%wqt_1~c*p;|osBVDs6l)M z30ib{DVf8lXN(>(pTDk|UgAXzj^*9aT%7EUTA)q*(u7+z*1+N~;JYYH$sQ?fAmx9mdn%{z2qzJ{*L66nx{k!PEz z(QIjVc!Dz|pxQE??{6E=7N~6p#+GHw4JSH@LG5{OPX}mK)7K8E);k8&ze54m`L=qzt)0g7NLDQbKtU#}&=hVz(OfESZ2{|>!cdN&0xeFZ8lon# zVmjw*OBIV~&jGeE_e2*E_xAw&j+mnd*P?f(YE{>+RVpPrkfk@PALXLE>+e$Xx^~dJ zzcGl-x5=BwiM6mTG~*iY0D-yfnitCMd2|*iaNY5$2_g(HQJWGRV4*W^WQ>aT3}~6h zPg1O@hA2ws*rlm*37A@~*cr3vi0t#}K=ye9x@jG)Lx=ClkK^x<3_&J*^q#q)0}C~D zo245qHN>koZ{j`UPfuT>0{Y+t91A+nFQD?`NsUnW~gaRoCy~ zeWf-NJbNjE75-Evr)0lWvc0}w={0s%XAJ5kz_&AXzBFI*`rDEZ?!Df3(4AT+Ox0?( zRjyL^6=rHKUvKE(6xLM9DHp4SNOaFZXQ2evhZvz;o$;P(j<# zRH{yaK7F4zcJpyQBva_^yPa{uu5qqp=e_T_|G@qKAO817P6N&`HUY*BmJA^6x^|?Q zh-q!YHxj+=r%FW&bZrd-I8`bG{Y3RGL{#3klS)g|th_lhEch1nFJs5;9$b_jjQT&N ztQ?YsRZzVNwUsvYUN-e!eog?bp~YMukWK-Q8v>Uo_xzx;mr7u%%I-3Bs-#i<%v8;B z3-d*1&QA!<>!$*s@cXd4yi~3&IfX_{n(bNTDcA3A0VzKYV$Y9PN>x8yRn0qoV!?*e z%1;Q6gO1jPBGyj`MTn<`O4;?(7aQG3haF0LjkxRw$Sa)So`AlQSn`cUblW$kYEYQI zha-?tL&VrjNoW%+4u7aQ#s%4VL@NO`+Qc*TrGK}hxga3j)s#`0QF~K6ZYdZXWs7z+#Emk zSm6=K6P}$odHmrg{o&REVg8;V>+we(Id$UnX@7kiVhA&is0t!O(P(mWCo~b38)})_ zFj_zF`AMfXHCMvyV<$cjxH-2>n#J!59_1m62O_{3yqi(3PS^ZgFuraXe2m|_v|v#; z-xvu$3(SKe1h%murGch4+J2e=B?a~brz6Wu0no-l#P}%$(`9?!?+$8IiT1tB9VbL| z39zX`ewPr@!Gl3+-%rR0`C7?O60iZD0^mgIep*%?;|MjaH%m)vN0?7j?HXS+#`L&q)A)jZm!48@ zvr%eijf41Ot8CZAUs&?1Mvf(us@*ruO*+OfE%hcP7y6Ai->9Wu(cd=y*60mPB)*|1 znJF! zgUEGj@I0*Vl2IQ-J>I692)|l{QWS?!(DE?Bi_%8Xg@|<11`QP((`=FQPzKp9p!zn% zy$=dT1&j`FHmH5xnV9RtAB0(JrX?-qc54#i);^~ss z1%Y8fucn$Ye1p>tI8x* z`zV!CrBu2RbCb*Gp_YNj*Xs{*!{DGEsRzoL%!>Z9&id3H)iYyRZ~T3Niyer0u)eud zV}zPJJ&Aq6iDp1So-xC@IYN=>$#r%8P^%9`FfN)2YjK#5EQ1XqW) zdqb_6D77Vl@oIlvLLO9gm6)@&U*?I63osf(Ylrrm=ayV^266^p%X{nB z=;g#%+I|+(Dx0ZIk*~diz&9=w?W+AUpJ(`-m@dM;X)G@2**8e4+TX#{Hd*LJ#63Y! zqYaU+=dd;gv>rVR9JX0A^&V}9tXywT2N3Mo#TIC!SekT_K;u_%uDO$ zh!a7$Y8$cLuXnKyKqDmQRu2@m`g*Pd!|FwtQ1Jq#E}%3XKq*^KS$%c1)qr+UA;w>h zOFhW>a$KOae?s8xEz*50gF>tj%Ii7oBCpS^Kw58UjiFGI`Z0a&l0+O*GP$CuC8o-# z8K&_24c)#67hjugpLI&WU&O47NPjSP{fQ2r|+NXjPBWp!bb$SG1Ln6;eP*L^WMZ*q@XF^3%Y zmN~?HzZi~qIE#oqjGU2~y-ANFxguloPr_QmF+i;^1hocnx2W}ok*HEXAI<7KNih*lUBmIB0WDOWrMbZHa-`$5q+1>Mf2UXjT_zrB|FV%YDT+pUc>d%t!7wy zDul0^_b^?pubwvYw)-`G8AldY(gEqfwFqm(4n%gB2sP|&8ibR zlJ^3+Nu!!(^JBgC^XQ+5)0$K>1zc9?qLNe;ncmz}rug_2gAN~ZYcLO7!V?D(KMgZ2}tLi!3 zEQj@)3D;y6E37L*hpCxI}{FdBtrG~Bckd2A$V<2PXXoW~xj@N#IcGo3r}s zi58+byOMZ05p1=%6QX#){$ap*3-GsWjcwAJwPY{9(ZJ{410D9DRzlWkEV)j*o>1BE zx;wuacIWgOZQmMuOTmzg2hAP|m9G!pD5NnL?Ntn#A17&jT;in~FOz_#sP-6lzpHw|P1I0oi3j~ETvOr=OIdBWPMdCla^4|*q|aPTVUqzg4MnjqLEHmZ4HBij##ZDQB5X} zLiKk3Vl5>5^{_i{tkL$ZvD%r3Pr?{1G@puKwZBJlR8%%fej<{MQ)_nrk-!kE4f(J zja>;;@X0lkZj%6RALX&02<_X3xnBST`0@GTMFi!l{UQP4Lkv2h@2&{?zLS~8Sz7aa z&GxkgKMu~p{&9&abj0UNNc$@B`6O;gMdvgsbSYN2`PMCd-K@~p>E#iG1Kq88#*4_Cd^w#?wS3G7|x(ja^L`9dh z8SPqpMRyHQxCdc@`FEs?+~~PXvx(d3 zfWYOzytSSOts^?!k(I8OyO`gSJE1{0c!wVjjQ7mxrRtPu=djQNZZfn+P)I?{kYEd( zI7mY(Pv7s@4+BtcK=yR#EA+EsfN+^V*wVz^1 z#l(pk%AMF!MBg|P^Ny}b$*AszR*Ri>hc&xy{{otc=t}<`AOC$A7xv^1^u+!g<4F;D zsr}RPuO00U1@^SkrGt%EyRPl`wdouBz{SVS>8pJRWcUo8eTxp8C~Vi-tnO}%PG1ZL zxr(M$4L3|L*KE@MJerK~|7$gI4`2Hik=I+-(m-h%nq2o?SATCB^%Tti<=C}hiz)tPR*rs8_Bl>thB^jp{{2Xt> zeDW0bKg+bBLsGNMM*Q4THL~By5|P#M1bVO;VA4KV0(6PI8H!&3El!Elp!0)<{T3f$_j#np}3Od-pA z^*jRJWDtl>;^B#AHn$3joisA)=vnQh-n(I_b3?BZ(yQNrry2v>kLp#d*{iktLW455 zkDhR5LEX;|tggdm9fx0rz8A3Bk@Io~(ZeBf0we5QYEz87(c&e<4qFgg)phh5qXK@3 zW3+yY+*uin(r3Ykj6jR9!D@)SfTG_0+lRI0!db!llaFaUzR{Fvhj=$@koo)0?ZBqx?$6cK|)GE>+&wD33=em_R zzKy4dus-%rvhlvCK%Le$UhW3-8gICB+hd!R-Kp5jh=g0HchVm=cT$_Y9!%*2rLbeq z@wu01vB0x8%9b7m_N6I5RV`L)xUqkTd6Gq(TLgwL^C1=3;@iK5C#6xPdAOfc$BFHK z&4+mLoMZnRX5juEkrB=C@#6>rm8%i}kzEiP948ehlJfP1L--vAJ}zD?0P#i;YiOe7 z4|W=d$1tT1t<-7OtxUGXgcwicF-Gtk5HoIN5=>L(t#}>~bK{;!o|E83B66b9+XUS= z@05SPdu)os-S|h0oWf5qGWAz9l>|0P!|k3vgOmFdrNam}J`#gx84dUdwn452$9x(z zp$LI-jcdS`7OWXSum>rO?}E{9Bn?ub)dXv=8-uYC_=y0v(^g^S0+~m0ns!F;W&^C2 zrUlWa*nL>QoVf4Kj zGv0^27e?l~*GU21xtCBhOLns5CmKB^QQ!FaNme2 zr&}8XQxuy`f`d!F5WkwZnz~ADKp4B4z7|8gel>PAQyoMK6zEohTVEu1xJfs1A|Fg| ztd!(SUQJ$2U(H>OU)8SCViuONg}9q_ySN3rU8_A;l2_7Kax(ykXv?!Hf{bi`+EwF< zVWpv&saol)#^u;$?XrG3e%ZL3xSYJ4x}3h8xtzV+bvZZPEnYX%ZtoQk6Xe)d;u&t( z84?Z9J$wb5Ti+EW0z?cpPH2D{x#0|KySS2rkrg-BSp%DPoJMN6r|?oZNUQgwi7MRF zMN(VRV1$*TF`8wFsn!oM=PF&J96A)}h5 zS!39X0R;)ODG)YrN63C~ZQyC}^+Z5MCZ!KMSQ|)L)x+NnYZwZ03H(D!5SQ}{J3$2P z!tai2K)bWM3!|9DJ*#8&z#pnwEh@K_TgkyOSOD9A)@K{g3b$#5t9K8tah}6i8(z`Q zdn2=W3y4MTQyp_h=DY=Fb$d3F|JE?8+uzvXi&3I35|7+WhKAMG3j`@p;0J#t;iz0=jfR>HTu z%{3uCi>`{e{h_dqcfBgWSd7E@+INAmfFY#ld&yWFf~<{Bti;ijxL}K~^uLUgAYd%y zPGoH(Uj8&Ta~!y#lMZ$2xlLM489S53(j)(}U{~}j z@hf_J`jyxf?F#B{t1ELQdnMBz%en?*N}TnECa(uyz&Z}0#!sKP(~qAz`JA{pdZQb3 zG0k#d^v=s6-=M7yPYG8lhe#lmKndN6clGs)hm<^CUp{32JYx*V;}6+CBZ=OjHu>Re zq>J#2{m4z2vo@i5C+2KMIoEpYm{5#oa4Z;t(=o%>7sq`=PBiC~*djPJnp8(|y`w_H zw>5CYe5P6=^CQkwKxfW5xP1wlmlg}w?e-s_Y7I15D3U(fH!u*Wl8oqigVD4@hk+H+J@O8q-5wsPr z_YCj8z(7n}f0K8=#Xwj;GI;i1A;6(pDxzlKVY-G>x{0D~7nki1Gx_UGPL$^gPd%*; z_hv-=R)8?vrdp_}3<6VEU5>DFo0%%B@eg_TM-1RdK4$+3gCAn>B?e?g{S1#HPvN}# z91f`0F8E2eR)84IPoBrIeH=w5kw1@9>MZc541~KRV`={x19Fx2ml>R9&}1VAB4WNS zSqKHVA1mMtIxVfl$jZLXBA-Oyr+Kuw05|}9f*>_r!|6K9fn)Y1w@{ldDBEaJU)g`o z0^L#|WKIk|+JC|N-emBX48DVbu%)1xlvC`0H*K57Z{=$HEOHyPU4U(Z0sdnc+{mC# zo+OO}#W^hxt~Ixx_6ZM(4(yxSRvcjNF`)HIt0S)PLyi>1%l2qIj!bQtI^aqjn32%6 zp7bL@=^bHd1DubEcGS^Z)}apB@&NJ(>e!3Jfvq)#CcAZY2e#J>Ik(&Gfw&PaFU9IC4}PN$wI*Ef7PGx|a0;M86NLd6R3)@Z z!Sn1;VYnI7yt|}Af_-nKvOMYm;t8s_1z9AL=TEjY?|i&Im6V$p(l zNOpni+r&2o4-u%>fQt%v18eHj$95|GNo;5ja+wN-;7Tq zi18M_J@Qc~5?0tsa&|qq3QBoKs1g6DvC>=bZAdKaUw1bEJN1Fb=(TzUH}w@ZL4}b8 zCyJJUtNMsl8qf0ek{?~1YUXI(39jlBT*We5fscBCiG(+5*1yuVh+|)?CUDkvCdmSo`Saw-i92w;<=^d4wMYIXtZ!gVK>{sh``zOM`$;y7x>nrt9u$a5b~!nIMSut z=Dko!YzsJtZg-zGsCb454g}92Zkur>1{C$x7}XPFLmhA-wd`E14) z=*t7FBF{k3q##G3(F6+;L)r(K`eELElfl~zAW(|gLkuXQZ^xjW@TMqqP=+ymlomtf!{z{NbNMUqVUSjon{{qF=QTW~UI zh7c$JA3S&;#`?qxS&BLIqM$iE@`^7ZNi4A!Wz}2wiID;%qCA2nirQ$71ajoV9Pti0 zlE`r^%wcrMkwT7>VU9#J$7&KdhIUNsCwc@7)I9WsegKoCmsfyCKs$*9g8A`LlR8{u zMV)~<888yWaqfTgvomZCCVlDls z{nG@VA7t`*kzYd*Xo!!j29w z*VgG4%Q-Y98sdj>b$%OxIwbV_1tW>H~sRe1PPCQNTGZyT*a7_eP9DbK)Na^JY z69BnFzaiIb+jCd&oC~hq!L=v2zN7roCqSZHldyj4Lz^k9m%K?Ga5UCG;ob4V#X}G6 zId^@WBHwzc^+;h}ZYEtbGjE>st49EpuHP zy{_%Ju6_7#xg!7LPcLz}{`8ga53a8V*Y69ip9HV&OqI)i$}TNbpl!nmrdd|1!)F$} zZp|-1x;Mw&Wp99q5c_-x6^hlL=*L!J?v3w` z>DR4I2l7GkSiNXcE(W}p^@xkoo_FcY){wD>`^ z)NNYx!PQvkTZq=qlgRinP6L^Z zlQ_}qZR;@jbeRT^lJW&LuvA}R?IZS9TM zH9ZQYY#;s}PYG>Tw1!HUubc9#GXl zv~USpb8H6Ka8|IP$yWATk^;a2B7%1XFc#W1%^n5%fned9=InN%t|SS%2+|YxDY+kT zAzolkL0UP~7k9hGa*1(JkMiSW_4sO^1#?_ceWEPRG@za`7KkW|l$r{F{`)Ls4CTd#GG{nU0 zALbpDn!~7Jgf(!(uXbabUBCQu8pc*y#Fx1m-_%)LMEQ+k)Vv8jNV_95F>CV=ipb@X zY{Ja@8op0}IoKkKS;gpdq1;xcVb(S(1)#NU1>e4z-Q;`&U#l^3VDG=;>sN_tKc2Qj zZIW|0uWqq+P?^^(!~FK2GXz`MwYrt_Ur#`dxesHp)7k~L2Q#}qX5B&55kf>al+)dW z3FO>T?ZLh3+a1zk5@qoftCemjP}Z|H$3t&N+1m`53y6@Kv#>|rA!CiHRHW>c7~R^Z z+M}%nq0rhdt(~c6>uI$5LOro+(&B=nv-nV0qi!AGaXBk5->m`!uiy(%QeO4e#dk?E z>U&fucPC)th(H-k$zQxvzBjZL>El5k4$1e14u^FS*6x&kd`iA%_3eN#e6I))))RgM zXgyf79}nkRzA6=dKj{+Md<77}?<_Ij=fiyOVT)nDZ^1YkR?pR#tRsvi)>OjXF6V9x zz@M?w4@ZC3`~U3!}tBn!{%;rI_E4yoIIaD6Mo0zJin1pG@m{7$fM9G z;EZw9JMxm#{1}0-7~bBdZ(mqLQT0$V7mDz)9q6{gdT`n!kzcZ{Y40f23sQC97%I|Q z+lL$)uBpRX@?PkCpRKHsy<-RRH?RsJOZ^}atgzIZg&c}m5O>gWiN_q^>xfTq_7-^} z+W^uIX~wmNC$0npb|88EG=%3e4t|!d&&$;%c(jE@nq7A0VShU%hVR(F9>+cjlJ*|7 zZT}+%&E16TmVFSb|>s#XPWg7@FAFe z-YVyn-hEB#!oUaMt0_2W1MpMc(<7&$;8d_=0+^I5F!CplKbg;-;O96>6?mkEZx{G* zVSKZ`sT#hh;Vjewm)u1hg%5mBN6uN9H#z7JS7NO_Jah;yrN==Vfm}pmMYuqep5V|^ zr2=Ql_<{&#(80lb=(XSv*nwFV{h8tHJrr(G+qy+O-UwHuVnkr@y||HS#dOc{UL?RB z=J|7&XJtHkcXK^8UwzNrkomeLc(}!fOb$`aHV$`W7$G&VNAHK9e;Lw4@Nq6KIK2H0 zBhA3$dw8ZcQdL-i*ULKP!xEgBLwRp+Tk)1pGK76mUk168qdE;GZS4Zph0$r2fyBoH z+NQeg1308_IA-hcb4&!X7IQdg4vIt_Bf*z|xZ?Ccs{RdRQUe=T3+P@(d=kMMni9Im zAcyPdmpQ&ikc?xwcQWfZgP&nwU>VvVsRQxIQN(%2&tj>G?(@(%K724G4(pzvCEsbf z<_)Q7BDvBSh4SCPj|OW48b>ABmCFno45RW%hOG;>XZS%UNX-u^O85u z2;*+yeiQ&!s>%*PxPXEgpMNb`N4%xb+vpLKFC?P9#*&o=;M)}c=jredABKWsKluL( zza_zbI{f$qwyhh#tUxi7tVGq*(3CfCKfqVsqh6)?8sas6RP`4VqI!vifD;;c2X8`+ zAq;&Zl?L0Q!w*;hMTui3@%sM+XZei!w5?rRrk{0u=7Dg{ui)~#D%pJV8!L^^4z+xY zr`a=jwK~o&?`n>I<9vlWQ5o@sIO@emv3MV3{?H3+=g&2W+wsQtdAM?+uZ@1z#+fTG zb=OG97D|UHgORoN(XvB<_427P1EST0dEKxS{)L zOr5wpx6ew36K7!4!LNY(DUc*~t>)U4srtzir|8|pH@LAwK@H0Vs^YmN!ul0Y%0Da@ zqy0AwMAlm1oy8!Fz|S}f`+vYhRwQXUYkM;U#V6FnG7I+P3kJUZ2Qzg_Y?<2+u z#9YdMxa{OV%E$NfWqKO(-S7i0{eetliEYxoH#W5?_0@4lc5X!moI+vYCuvwxu70o3 z0f|e1wR2OMJLs1sJu2JeU~`o zg2V3rwJS&MJOyNQWUaLI%u5&E$?Hit(xyO{uDSW+*TGT0#M2SBs*`b4r%_{%JlE4H z>h%$;8-J}2sC>$G^&o2vTvyX!xbeS@v;ntWa{oV!0ad^Lojo>VRlNRo+Cb~yz}sl) zx6_)vz;VS#zBw+1!q|5E3?s4vr+E&qO@hf^*VNn52E)lCh@24U1(tDYFrjE9kr?y` zP)qa6*QyM8et!snao57~o%y0eqY?K!+$qDNA{9j2Y0mhfxGg1h|z zRAC=y;PH{`LgWZCN=i1;Bsao6(N7;$A4Y!wC1VaZMt;(~4-+B2#UYbdQ_Cmy2Q%^> Q$}t*$=@rN>INfjmU$iSCZ~y=R diff --git a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py deleted file mode 100644 index 6eb5678..0000000 --- a/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py +++ /dev/null @@ -1,1048 +0,0 @@ -import rclpy -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint -from sensor_msgs.msg import JointState -from rcl_interfaces.msg import Log -from osc4py3.as_allthreads import * -from osc4py3 import oscmethod as osm -import xml.etree.ElementTree as ET -import numpy as np -import spatialmath as sm -import roboticstoolbox as rtb -from osc4py3 import oscbuildparse -import time -import os -import re -import socket - -class JointNameListener(Node): - def __init__(self): - super().__init__('joint_name_listener') - self.subscription = self.create_subscription( - JointState, - '/joint_states', - self.joint_state_callback, - 1 - ) - self.joint_names = None - - def joint_state_callback(self, msg: JointState): - self.joint_names = list(msg.name) - -class OSC_ROS2_interface(Node): - """Node to publish joint trajectories based on OSC messages.""" - - def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask): - super().__init__('scaled_joint_trajectory_publisher') - - - self.subscription = self.create_subscription( - JointState, - '/joint_states', - self.joint_states_callback, - 1 - ) - - self.subscription = self.create_subscription( - Log, - '/rosout', - self.log_callback, - 100 - ) - - # Store received joint positions - self.current_joint_positions = None - self.joint_names = joint_names - self.joint_velocity_limits = joint_velocity_limits - self.cost_mask = cost_mask - self.robot = robot - self.desired = None - self.previous_desired = None - self.log_dict = { - 10: "DEBUG", - 20: "INFO", - 30: "WARN", - 40: "ERROR", - 50: "FATAL", - } - self.speed_scaling = 0.2 - self.new = False - self.n_joints = len(joint_names) - - if robot: - while True: - print('+-' * 50) - set_limits = input("Do you want to set limit 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 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 = [None, None] - self.y_limits = [None, None] - self.z_limits = [None, None] - break - print("Invalid input. Please enter 'y' or 'n'.") - # Ask the user if they want to set new joint limits - while True: - 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 to keep current): ").strip() - upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").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.") - break - elif update_limits == 'n': - self.joint_lim = None - break - print("Invalid input. Please enter 'y' or 'n'.") - - print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}') - - - while True: - try: - print('+-' * 50) - self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() - if self.trajectory_topic_name == "": - self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' - break - elif self.trajectory_topic_name.startswith("/"): - break - else: - print("Invalid topic name. A valid topic name should start with '/'.") - except Exception as e: - print(f"An error occurred: {e}") - - # ROS2 Publisher - self.publisher = self.create_publisher( - JointTrajectory, - self.trajectory_topic_name, - 1 - ) - - while True: - try: - print('+-' * 50) - log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if log_ip == "": - log_ip = "127.0.0.1" - print('--' * 50) - log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") - if log_port == "": - log_port = 5005 - else: - log_port = int(log_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if state_ip == "": - state_ip = "127.0.0.1" - print('--' * 50) - state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ") - if state_port == "": - state_port = 7000 - else: - state_port = int(state_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") - if commands_port == state_port: - print("The commands port must be different from the state port.") - continue - if commands_port == "": - commands_port = 8000 - else: - commands_port = int(commands_port) - break - except ValueError: - print("Invalid input. Please enter a valid port.") - continue - - - osc_startup() - - osc_udp_client(state_ip, state_port, "osc_client") - - osc_udp_client(log_ip, log_port, "osc_log_client") - - osc_udp_server('0.0.0.0', commands_port, "osc_server") - - # Register OSC handler - osc_method("/joint_positions", self.joint_positions_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_ADDRESS+osm.OSCARG_DATAUNPACK) - osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) - osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK) - print('--' * 50) - while True: - try: - print('+-' * 50) - self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ") - if self.hz == "": - self.hz = 100 - else: - self.hz = float(self.hz) - break - except ValueError: - print("Invalid input. Please enter a valid number.") - continue - print() - print('=-=' * 50) - print() - print(f'Sending joint states to {state_ip}:{state_port}') - print() - print('=-=' * 50) - print() - print(f'Sending log messages to {log_ip}:{log_port}') - print() - print('=-=' * 50) - print() - print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') - print() - print('=-=' * 50) - print() - - self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}") - self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') - self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}') - self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}') - self.create_timer(1/self.hz, self.update_position) # Timer to update the position - self.create_timer(3, self.reset_prev) # reset the previous desired position - - def reset_prev(self): self.previous_desired = None - - def speed_scaling_handler(self, *args): - """Handles incoming OSC messages for speed scaling.""" - try: - if len(args) == 1: - if args[0] < 0: - self.speed_scaling = -float(args[0]) - else: - self.speed_scaling = float(args[0]) - if self.speed_scaling > 1: - self.get_logger().warn(f"speed_scaling_handler: Attention! Speed scaling {self.speed_scaling} is greater than 1!") - self.get_logger().info(f"Speed scaling set to {self.speed_scaling}") - else: - self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.") - except Exception as e: - self.get_logger().fatal(f"speed_scaling_handler: {e}") - - def joint_trajectory_handler(self, *args): - try: - if len(args[0]) == 6: - points = [[float(j) for j in i] for i in args] - elif len(args[0]) >= 7: - points = [[float(j) for j in i[:6]] for i in args] - self.get_logger().warn(f"joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.") - else: - self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.") - return - - self.desired = ["joint_trajectory"] + points - self.new = True - except Exception as e: - self.get_logger().fatal(f"joint_trajectory_handler: {e}") - - def joint_position_handler(self, address, *args): - """Handles incoming OSC messages for joint positions.""" - try: - joint_name = address.split("/")[-1] - - if joint_name in self.joint_names: - if len(args) == 1: - position = float(args[0]) - index = self.joint_names.index(joint_name) - if self.robot: - if position < self.robot.qlim[0][index]: - position = self.robot.qlim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}." - ) - elif position > self.robot.qlim[1][index]: - position = self.robot.qlim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}." - ) - else: - if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]: - position = self.joint_lim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}." - ) - elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]: - position = self.joint_lim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}." - ) - desired_joint_positions = self.current_joint_positions - desired_joint_positions[index] = position - self.desired = ["joint_positions"] + desired_joint_positions - self.new = True - elif len(args) == 2: - position = float(args[0]) - duration = float(args[1]) - index = self.joint_names.index(joint_name) - if self.robot: - if position < self.robot.qlim[0][index]: - position = self.robot.qlim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[0][index]}." - ) - elif position > self.robot.qlim[1][index]: - position = self.robot.qlim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.robot.qlim[1][index]}." - ) - else: - if self.joint_lim[0][index] is not None and position < self.joint_lim[0][index]: - position = self.joint_lim[0][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[0][index]}." - ) - elif self.joint_lim[1][index] is not None and position > self.joint_lim[1][index]: - position = self.joint_lim[1][index] - self.get_logger().warn( - f"joint_position_handler: Joint '{joint_name}' position {position} is out of bounds. Using {self.joint_lim[1][index]}." - ) - desired_joint_positions = self.current_joint_positions - desired_joint_positions[index] = position - self.desired = ["joint_positions"] + desired_joint_positions + [duration] - self.new = True - else: - self.get_logger().warn(f"joint_position_handler: Invalid number of arguments for joint position. Expected 1, but got {len(args)}.") - else: - self.get_logger().warn(f"joint_position_handler: Joint '{joint_name}' not found in the robot model.") - except Exception as e: - self.get_logger().fatal(f"joint_position_handler: {e}") - - - def cartesian_trajectory_handler(self, *args): - """Handles incoming OSC messages for cartesian trajectory.""" - if self.robot: - try: - if len(args[0]) == 6: - points = [[float(j) for j in i] for i in args] - elif len(args[0]) >= 7: - points = [[float(j) for j in i[:6]] for i in args] - self.get_logger().warn(f"cartesian_trajectory_handler: Duration is not supported for cartesian trajectory yet. Ignoring duration.") - else: - self.get_logger().warn(f"cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.") - return - - self.desired = ["cartesian_trajectory"] + points - self.new = True - except Exception as e: - self.get_logger().fatal(f"cartesian_trajectory_handler: {e}") - else: - self.get_logger().warn("cartesian_trajectory_handler: No robot model provided. Cannot handle cartesian trajectory.") - return - - def joint_positions_handler(self, *args): - """Handles incoming OSC messages for joint positions.""" - try: - if len(args) == len(self.joint_names): - desired_joint_positions = [float(i) for i in list(args)] - elif len(args) == len(self.joint_names) + 1: - desired_joint_positions = [float(i) for i in list(args)] - else: - self.get_logger().warn(f"joint_positions_handler: Invalid number of arguments for joint positions. Expected {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]) or {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]), but got {len(args)}.") - return - - # Check if joint positions exceed limits - if self.robot: - for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present - if position < self.robot.qlim[0][i]: - desired_joint_positions[i] = self.robot.qlim[0][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[0][i]}." - ) - elif position > self.robot.qlim[1][i]: - desired_joint_positions[i] = self.robot.qlim[1][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}." - ) - else: - for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): - if self.joint_lim[0][i] is not None and position < self.joint_lim[0][i]: - desired_joint_positions[i] = self.joint_lim[0][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}." - ) - elif self.joint_lim[1][i] is not None and position > self.joint_lim[1][i]: - desired_joint_positions[i] = self.joint_lim[1][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}." - ) - - self.desired = ["joint_positions"] + desired_joint_positions - self.new = True - except Exception as e: - self.get_logger().fatal(f"joint_positions_handler: {e}") - return - - def tcp_coordinates_handler(self, *args): - # Ensure the desired joint positions are within the specified limits - if self.robot: - try: - if len(args) == 6: - x, y, z, r, p, yaw = [float(i) for i in list(args)] - duration = None - elif len(args) >= 7: - x, y, z, r, p, yaw, duration, *_ = [float(i) for i in list(args)] - else: - self.get_logger().warn(f"tcp_coordinates_handler: Invalid number of arguments for TCP coordinates. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args)}.") - return - - if self.x_limits[0] is not None: - x = max(self.x_limits[0], x) - if self.x_limits[1] is not None: - x = min(self.x_limits[1], x) - if self.y_limits[0] is not None: - y = max(self.y_limits[0], y) - if self.y_limits[1] is not None: - y = min(self.y_limits[1], y) - if self.z_limits[0] is not None: - z = max(self.z_limits[0], z) - if self.z_limits[1] is not None: - z = min(self.z_limits[1], z) - - if x != args[0] or y != args[1] or z != args[2]: - self.get_logger().warn( - f"tcp_coordinates_handler: Desired joint positions adjusted to fit within limits: " - f"x={x}, y={y}, z={z} (original: x={args[0]}, y={args[1]}, z={args[2]})" - ) - - self.desired = ["tcp_coordinates", x, y, z, r, p, yaw, duration] - self.new = True - except Exception as e: - self.get_logger().fatal(f"tcp_coordinates_handler: {e}") - else: - self.get_logger().warn("tcp_coordinates_handler: No robot model provided. Cannot handle TCP coordinates.") - return - - - def joint_states_callback(self, msg: JointState): - """Callback function to handle incoming joint states.""" - try: - msg_time = oscbuildparse.OSCMessage(f"/time", ',s', [str(time.time())]) - osc_send(msg_time, "osc_client") - if not(self.joint_names): self.joint_names = msg.name - joint_position_dict = dict(zip(msg.name, msg.position)) - self.current_joint_positions = [float(joint_position_dict[name]) for name in self.joint_names] - joint_position_dict = dict(zip(msg.name, msg.velocity)) - self.current_joint_velocities = [float(joint_position_dict[name]) for name in self.joint_names] - - if self.robot: - tcp_position = self.robot.fkine(self.current_joint_positions).t - tcp_orientation = self.robot.fkine(self.current_joint_positions).rpy() - - msg_tcp = oscbuildparse.OSCMessage(f"/tcp_coordinates", ',ffffff', [tcp_position[0], tcp_position[1], tcp_position[2], tcp_orientation[0], tcp_orientation[1], tcp_orientation[2]]) - #msg_x = oscbuildparse.OSCMessage(f"/tcp_coordinates/x", ',f', [tcp_position[0]]) - #msg_y = oscbuildparse.OSCMessage(f"/tcp_coordinates/y", ',f', [tcp_position[1]]) - #msg_z = oscbuildparse.OSCMessage(f"/tcp_coordinates/z", ',f', [tcp_position[2]]) - #msg_roll = oscbuildparse.OSCMessage(f"/tcp_coordinates/roll", ',f', [tcp_orientation[0]]) - #msg_pitch = oscbuildparse.OSCMessage(f"/tcp_coordinates/pitch", ',f', [tcp_orientation[1]]) - #msg_yaw = oscbuildparse.OSCMessage(f"/tcp_coordinates/yaw", ',f', [tcp_orientation[2]]) - #bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_tcp, msg_x, msg_y, msg_z, msg_roll, msg_pitch, msg_yaw]) - #osc_send(bun, "osc_client") - osc_send(msg_tcp, "osc_client") - - msg_position = oscbuildparse.OSCMessage(f"/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position]) - msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity]) - msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort]) - msg_name = oscbuildparse.OSCMessage(f"/joint_state/name", f',{"s"*self.n_joints}', [i for i in msg.name]) - bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_name, msg_position, msg_velocity, msg_effort]) - osc_send(bun, "osc_client") - - #for i, name in enumerate(msg.name): - # msg_position = oscbuildparse.OSCMessage(f"/joint_state/position/{name}", ',f', [msg.position[i]]) - # msg_velocity = oscbuildparse.OSCMessage(f"/joint_state/velocity/{name}", ',f', [msg.velocity[i]]) - # msg_effort = oscbuildparse.OSCMessage(f"/joint_state/effort/{name}", ',f', [msg.effort[i]]) - # bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY, [msg_position, msg_velocity, msg_effort]) - # osc_send(bun, "osc_client") - - except Exception as e: - self.get_logger().fatal(f"joint_states_callback: {e}") - - def send_joint_positions(self): - self.previous_desired = None - self.new = False - try: - if len(self.desired) == len(self.joint_names) + 2: - desired_joint_positions = [float(i) for i in self.desired[1:-1]] - duration = self.desired[-1] - msg = JointTrajectory() - msg.joint_names = self.joint_names - point = JointTrajectoryPoint() - point.positions = desired_joint_positions - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - self.publisher.publish(msg) - elif len(self.desired) == len(self.joint_names) + 1: - desired_joint_positions = [float(i) for i in self.desired[1:]] - msg = JointTrajectory() - msg.joint_names = self.joint_names - point = JointTrajectoryPoint() - point.positions = desired_joint_positions - duration = 0 - for p1, p2, max_vel in zip(desired_joint_positions, self.current_joint_positions, self.joint_velocity_limits.values()): - duration = max(duration, abs(p1 - p2) / max_vel) - duration = duration + min(2, 0.2*duration) - duration /= self.speed_scaling - if duration == 0: - self.get_logger().warn("send_joint_positions: Duration is 0.") - return - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - self.publisher.publish(msg) - else: - self.get_logger().warn(f"send_joint_positions: Invalid number of arguments for joint positions. Expected {len(self.joint_names)+1} ([q0, q1, q2, ... q{len(self.joint_names)}, duration]) or {len(self.joint_names)} ([q0, q1, q2, ... q{len(self.joint_names)}]), but got {len(self.desired)}.") - return - except Exception as e: - self.get_logger().fatal(f"send_joint_positions: {e}") - return - - def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3): - - if num_points == 2: - return [0, total_duration] - n = int(num_points*(1-flat_ratio)/2) - start = np.cos(np.linspace(0, np.pi, n))+2 - end = np.cos(np.linspace(-np.pi, 0, n))+2 - flat = np.ones(num_points-2*n) - - timestamps = np.concatenate((start, flat, end)) - timestamps *= total_duration / timestamps.sum() - timestamps = np.cumsum(timestamps) - - return timestamps.tolist() - - - def send_tcp_coordinates(self): - """Send the desired TCP coordinates to the robot.""" - try: - self.new = False - msg = JointTrajectory() - msg.joint_names = self.joint_names - steps_per_m = 100 - if self.previous_desired == None: - [x,y,z] = self.robot.fkine(self.current_joint_positions).t - [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() - else: - [x,y,z] = self.previous_desired[1:4] - [roll, pitch, yaw] = self.previous_desired[4:-1] - x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] - self.previous_desired = self.desired - steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m) - if steps < 2: steps = 2 - cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)] - '''if self.previous_desired: - [x,y,z] = self.previous_desired[1:4] - q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5]) - else: - [x, y, z] = self.robot.fkine(self.current_joint_positions).t - q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R) - - x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] - q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1) - - steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m) - if steps < 2: - steps = 2 - - cart_traj = [] - for i in range(steps): - alpha = i / (steps - 1) - - # Convert to arrays for robust interpolation if needed - q0_array = q0.vec - q1_array = q1.vec - dot = np.dot(q0_array, q1_array) - dot = np.clip(dot, -1.0, 1.0) - - if abs(dot) > 0.9995: - # Linear interpolation + normalization - q_interp_array = (1 - alpha) * q0_array + alpha * q1_array - q_interp_array = q_interp_array / np.linalg.norm(q_interp_array) - q_interp = sm.UnitQuaternion(q_interp_array) - else: - q_interp = q0.interp(q1, alpha) - - # Interpolate translation - pos_interp = [ - x + (x1 - x) * alpha, - y + (y1 - y) * alpha, - z + (z1 - z) * alpha - ] - - # Compose SE3 transform - cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())''' - - - - if self.desired[-1]: - timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8) - for j in range(steps): - sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan') - if sol[1] == 1: - fowards = self.robot.fkine_all(sol[0]) - out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) - if np.any(out_of_bounds): - #print(fowards.t) - #indices = np.where(out_of_bounds)[0] - #print(f"indices: {indices}") - self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!") - ''' - for i in indices: - try: - print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}") - except IndexError: - print(f"index {i} is out of bounds, but no corresponding joint found.") - self.previous_desired_tcp_position = self.desired_tcp_position - ''' - break - duration = timestamps[j] - if duration == 0: - prev_sol = list(sol[0]) - continue - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - prev_sol = list(sol[0]) - else: - self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!") - prev_sol = self.current_joint_positions - if len(msg.points) == 0: - self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.") - self.previous_desired = self.desired - return - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = self.desired - - - - else: - prev_duration = 0 - ''' - if self.previous_desired == None: - [x,y,z] = self.robot.fkine(self.current_joint_positions).t - [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() - else: - [x,y,z] = self.previous_desired[:3] - [roll, pitch, yaw] = self.previous_desired[3:] - ''' - for j in range(steps): - sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan') - if sol[1] == 1: - fowards = self.robot.fkine_all(sol[0]) - out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) - if np.any(out_of_bounds): - #print(fowards.t) - #indices = np.where(out_of_bounds)[0] - #print(f"indices: {indices}") - self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!") - ''' - for i in indices: - try: - print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}") - except IndexError: - print(f"index {i} is out of bounds, but no corresponding joint found.") - self.previous_desired_tcp_position = self.desired_tcp_position - ''' - break - duration = 0 - prev = self.current_joint_positions if j == 0 else prev_sol - for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()): - duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun - prev_sol = list(sol[0]) - if duration == 0: - continue - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - duration *= 1.2 - duration /= self.speed_scaling - duration += prev_duration - prev_duration = duration - point.time_from_start.sec = int(duration) - point.time_from_start.nanosec = int((duration - int(duration)) * 1e9) - msg.points.append(point) - else: - self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!") - prev_sol = self.current_joint_positions - if len(msg.points) == 0: - self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.") - self.previous_desired = self.desired - return - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = self.desired - except Exception as e: - self.get_logger().fatal(f"send_tcp_coordinates: {e}") - - def send_joint_trajectory(self): - - try: - self.new = False - viapoints = np.array([i for i in self.desired[1:]]) - msg = JointTrajectory() - msg.joint_names = self.joint_names - traj = rtb.mstraj(viapoints, q0 = self.current_joint_positions ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=[self.speed_scaling*i for i in self.joint_velocity_limits.values()]) - msg.points = [] - for i in range(len(traj.q)): - point = JointTrajectoryPoint() - point.positions = list(traj.q[i]) - point.time_from_start.sec = int(traj.t[i]) - point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9) - msg.points.append(point) - msg.header.stamp = self.get_clock().now().to_msg() - self.publisher.publish(msg) - self.previous_desired = None - except Exception as e: - print(f'Error in joint_angles_handler: {e}') - - def send_cartesian_trajectory(self): - - try: - self.new = False - viapoints = np.array([i[:6] for i in self.desired[1:]]) - msg = JointTrajectory() - msg.joint_names = self.joint_names - x,y,z = self.robot.fkine(self.current_joint_positions).t - r,p,yaw = self.robot.fkine(self.current_joint_positions).rpy() - q0 = [x, y, z, r, p, yaw] - traj = rtb.mstraj(viapoints, q0 = q0 ,dt=0.01, tacc=1.5/self.speed_scaling, qdmax=2*self.speed_scaling) - msg.points = [] - prev_sol = self.current_joint_positions - n = max(int(len(traj.q)/200), 1) - for i in range(len(traj.q)): - T = sm.SE3(traj.q[i][:3]) * sm.SE3.RPY(traj.q[i][3:], order='xyz') - sol = self.robot.ik_LM(T, q0=prev_sol, mask = self.cost_mask, joint_limits = True) - if sol[1] == 1: - point = JointTrajectoryPoint() - point.positions = list(sol[0]) - point.time_from_start.sec = int(traj.t[i]) - if traj.t[i] == 0: - continue - point.time_from_start.nanosec = int((traj.t[i] - int(traj.t[i])) * 1e9) - msg.points.append(point) - prev_sol = list(sol[0]) - else: self.get_logger().warn(f"send_cartesian_trajectory: IK could not find a solution for (x,y,z) = {traj.q[i][:3]} and (r,p,y) = {traj.q[i][3:]}!") - msg.header.stamp = self.get_clock().now().to_msg() - msg.points = msg.points[::n] - self.publisher.publish(msg) - self.previous_desired = None - except Exception as e: - print(f'Error in joint_angles_handler: {e}') - - self.previous_desired = None - - def update_position(self): - """Calls the appropriate function to update the robot's position.""" - try: - if self.desired is None or not(self.new): - return - - if self.desired[0] == "joint_positions": - self.new = False - self.send_joint_positions() - return - elif self.desired[0] == "tcp_coordinates": - self.new = False - self.send_tcp_coordinates() - return - elif self.desired[0] == "joint_trajectory": - self.new = False - self.send_joint_trajectory() - return - elif self.desired[0] == "cartesian_trajectory": - self.new = False - self.send_cartesian_trajectory() - return - else: - self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.") - return - - - except Exception as e: - self.get_logger().fatal(f'update_position: {e}') - - - def clean_log_string(self, s): - - s = str(s) - - # Remove ANSI escape sequences (e.g., \x1b[31m) - ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])') - s = ansi_escape.sub('', s) - - # Replace tabs/newlines with spaces - s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ') - - # Strip leading/trailing whitespace - s = s.strip() - - # Optionally enforce ASCII only (replace non-ASCII chars with '?') - s = s.encode('ascii', 'replace').decode('ascii') - - return s - - - def log_callback(self, msg: Log): - """Callback function to handle incoming log messages.""" - - # Send the log message as an OSC message - msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)]) - osc_send(msg_log, "osc_log_client") - - -def main(): - """Main function to get joint names and start the ROS 2 & OSC system.""" - rclpy.init() - while True: - use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower() - if use_urdf == 'y': - while True: - robot_urdf = input("Enter the absolute path to the URDF file: ") - if os.path.isfile(robot_urdf): - if not robot_urdf.endswith('.urdf'): - print("The file is not a URDF file. Please enter a valid URDF file.") - continue - break - else: - print("Invalid path. Please enter a valid path to the URDF file.") - tree = ET.parse(robot_urdf) - root = tree.getroot() - robot = rtb.ERobot.URDF(robot_urdf) - joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic'] - print(robot) - joint_velocity_limits = {} - - # Iterate over all joints in the URDF - for joint in root.findall('.//joint'): - joint_name = joint.get('name') # Get the name of the joint - - # Look for the tag under each joint - limit = joint.find('limit') - - if limit is not None: - # Extract the velocity limit (if it exists) - velocity_limit = limit.get('velocity') - - if velocity_limit is not None: - joint_velocity_limits[joint_name] = float(velocity_limit) - - while True: - try: - print('-+'*50) - print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].") - print("The cost mask 111000 means that the IK will only consider translation and no rotaion.") - cost_mask = [int(i) for i in input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")] - if sum(cost_mask) <= robot.n and len(cost_mask) == 6: - break - else: - print(f"Invalid input. Expected 6 integers of which {robot.n if robot.n < 6 else 6} or less are 1.") - except ValueError: - print("Invalid input. Please enter integers only.") - print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}") - break - elif use_urdf == 'n': - node = JointNameListener() - print("Wainting 10 sec for JointState messages to extract joint names...") - rclpy.spin_once(node) - counter = 0 - while not(node.joint_names): - if counter > 100: - joint_names = None - break - counter+=1 - time.sleep(0.1) - rclpy.spin_once(node) - joint_names = node.joint_names - node.destroy_node() - if joint_names: - correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower() - while True: - if correct_names == 'y': - break - elif correct_names == 'n': - joint_names = None - break - correct_names = input("Invalid input. Please enter 'y' or 'n'.") - if not(joint_names): - print("Please enter the joint names manually.") - while True: - joint_names = [] - print('-+'*50) - print("Enter the joint names manually one by one. Type 'done' when you are finished:") - print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") - while True: - print('-'*50) - joint_name = input("Enter joint name (or 'done' to finish): ").strip() - if joint_name.lower() == 'done': - break - if joint_name: - joint_names.append(joint_name) - print('-+'*50) - correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() - if correct_names.lower() == 'y': - break - else: - print("Please re-enter the joint names.") - while True: - try: - joint_velocity_limits = {} - for name in joint_names: - while True: - try: - print('--'*50) - limit = input(f"Enter the velocity limit for joint '{name}': ").strip() - if limit == "": - continue - else: - joint_velocity_limits[name] = float(limit) - break - except ValueError: - print("Invalid input. Please enter a numeric value or press Enter to skip.") - break - except ValueError: - print("Invalid input. Please enter numeric values or leave blank to skip.") - robot = None - cost_mask = None - break - print("Invalid input. Please enter 'y' or 'n'.") - - - node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask) - - # Run ROS 2 spin, and osc_process will be handled by the timer - try: - rclpy.spin(node) - except KeyboardInterrupt: - print("") - finally: - node.destroy_node() - osc_terminate() - -if __name__ == '__main__': - main() diff --git a/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 b/workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 deleted file mode 100644 index e69de29..0000000 diff --git a/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 b/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 deleted file mode 100644 index aff3120..0000000 --- a/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 +++ /dev/null @@ -1 +0,0 @@ -rclpy \ No newline at end of file diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv deleted file mode 100644 index 79d4c95..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv +++ /dev/null @@ -1 +0,0 @@ -prepend-non-duplicate;AMENT_PREFIX_PATH; diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 deleted file mode 100644 index 26b9997..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em - -colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX" diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh deleted file mode 100644 index f3041f6..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_core/shell/template/hook_prepend_value.sh.em - -_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX" diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv deleted file mode 100644 index 257067d..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv +++ /dev/null @@ -1 +0,0 @@ -prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 deleted file mode 100644 index caffe83..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em - -colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages" diff --git a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh deleted file mode 100644 index 660c348..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_core/shell/template/hook_prepend_value.sh.em - -_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages" diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.bash b/workspace/install/osc_ros2/share/osc_ros2/package.bash deleted file mode 100644 index 92f5b25..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.bash +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_bash/shell/template/package.bash.em - -# This script extends the environment for this package. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)" -else - _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source sh script of this package -_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/osc_ros2/package.sh" - -unset _colcon_package_bash_source_script -unset _colcon_package_bash_COLCON_CURRENT_PREFIX diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.dsv b/workspace/install/osc_ros2/share/osc_ros2/package.dsv deleted file mode 100644 index ece85b1..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.dsv +++ /dev/null @@ -1,6 +0,0 @@ -source;share/osc_ros2/hook/pythonpath.ps1 -source;share/osc_ros2/hook/pythonpath.dsv -source;share/osc_ros2/hook/pythonpath.sh -source;share/osc_ros2/hook/ament_prefix_path.ps1 -source;share/osc_ros2/hook/ament_prefix_path.dsv -source;share/osc_ros2/hook/ament_prefix_path.sh diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.ps1 b/workspace/install/osc_ros2/share/osc_ros2/package.ps1 deleted file mode 100644 index ed891bc..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.ps1 +++ /dev/null @@ -1,116 +0,0 @@ -# generated from colcon_powershell/shell/template/package.ps1.em - -# function to append a value to a variable -# which uses colons as separators -# duplicates as well as leading separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_append_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - $_duplicate="" - # start with no values - $_all_values="" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -eq $_value) { - $_duplicate="1" - } - if ($_all_values) { - $_all_values="${_all_values};$_" - } else { - $_all_values="$_" - } - } - } - } - # append only non-duplicates - if (!$_duplicate) { - # avoid leading separator - if ($_all_values) { - $_all_values="${_all_values};${_value}" - } else { - $_all_values="${_value}" - } - } - - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_prepend_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - # start with the new value - $_all_values="$_value" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -ne $_value) { - # keep non-duplicate values - $_all_values="${_all_values};$_" - } - } - } - } - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -function colcon_package_source_powershell_script { - param ( - $_colcon_package_source_powershell_script - ) - # source script with conditional trace output - if (Test-Path $_colcon_package_source_powershell_script) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_package_source_powershell_script'" - } - . "$_colcon_package_source_powershell_script" - } else { - Write-Error "not found: '$_colcon_package_source_powershell_script'" - } -} - - -# a powershell script is able to determine its own path -# the prefix is two levels up from the package specific share directory -$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName - -colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/osc_ros2/hook/pythonpath.ps1" -colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/osc_ros2/hook/ament_prefix_path.ps1" - -Remove-Item Env:\COLCON_CURRENT_PREFIX diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.sh b/workspace/install/osc_ros2/share/osc_ros2/package.sh deleted file mode 100644 index 2631417..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.sh +++ /dev/null @@ -1,87 +0,0 @@ -# generated from colcon_core/shell/template/package.sh.em - -# This script extends the environment for this package. - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prepend_unique_value_IFS=$IFS - IFS=":" - # start with the new value - _all_values="$_value" - # workaround SH_WORD_SPLIT not being set in zsh - if [ "$(command -v colcon_zsh_convert_to_array)" ]; then - colcon_zsh_convert_to_array _values - fi - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - # restore the field separator - IFS=$_colcon_prepend_unique_value_IFS - unset _colcon_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_package_sh_COLCON_CURRENT_PREFIX="/BA/workspace/install/osc_ros2" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_package_sh_COLCON_CURRENT_PREFIX - return 1 - fi - COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX" -fi -unset _colcon_package_sh_COLCON_CURRENT_PREFIX - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source sh hooks -_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/osc_ros2/hook/pythonpath.sh" -_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/osc_ros2/hook/ament_prefix_path.sh" - -unset _colcon_package_sh_source_script -unset COLCON_CURRENT_PREFIX - -# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.xml b/workspace/install/osc_ros2/share/osc_ros2/package.xml deleted file mode 100644 index 086e035..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - osc_ros2 - 1.0.0 - Creates an interface for communication between OSC and ROS2 - Alexander Schaefer - Apache-2.0 - - rclpy - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/workspace/install/osc_ros2/share/osc_ros2/package.zsh b/workspace/install/osc_ros2/share/osc_ros2/package.zsh deleted file mode 100644 index b8ae59f..0000000 --- a/workspace/install/osc_ros2/share/osc_ros2/package.zsh +++ /dev/null @@ -1,42 +0,0 @@ -# generated from colcon_zsh/shell/template/package.zsh.em - -# This script extends the environment for this package. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)" -else - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -colcon_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# source sh script of this package -_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/osc_ros2/package.sh" -unset convert_zsh_to_array - -unset _colcon_package_zsh_source_script -unset _colcon_package_zsh_COLCON_CURRENT_PREFIX diff --git a/workspace/install/setup.bash b/workspace/install/setup.bash deleted file mode 100644 index 10ea0f7..0000000 --- a/workspace/install/setup.bash +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_bash/shell/template/prefix_chain.bash.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script diff --git a/workspace/install/setup.ps1 b/workspace/install/setup.ps1 deleted file mode 100644 index 558e9b9..0000000 --- a/workspace/install/setup.ps1 +++ /dev/null @@ -1,29 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix_chain.ps1.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_chain_powershell_source_script { - param ( - $_colcon_prefix_chain_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_chain_powershell_source_script_param'" - } - . "$_colcon_prefix_chain_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" - } -} - -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" - -# source this prefix -$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) -_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/workspace/install/setup.sh b/workspace/install/setup.sh deleted file mode 100644 index fa9641d..0000000 --- a/workspace/install/setup.sh +++ /dev/null @@ -1,45 +0,0 @@ -# generated from colcon_core/shell/template/prefix_chain.sh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/BA/workspace/install -if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX - return 1 -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_sh_source_script -unset COLCON_CURRENT_PREFIX diff --git a/workspace/install/setup.zsh b/workspace/install/setup.zsh deleted file mode 100644 index 54799fd..0000000 --- a/workspace/install/setup.zsh +++ /dev/null @@ -1,31 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix_chain.zsh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/humble" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script diff --git a/workspace/log/COLCON_IGNORE b/workspace/log/COLCON_IGNORE deleted file mode 100644 index e69de29..0000000 diff --git a/workspace/log/build_2025-05-25_15-14-38/events.log b/workspace/log/build_2025-05-25_15-14-38/events.log deleted file mode 100644 index 52f7dda..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/events.log +++ /dev/null @@ -1,52 +0,0 @@ -[0.000000] (-) TimerEvent: {} -[0.000769] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()} -[0.001046] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'} -[0.098566] (-) TimerEvent: {} -[0.200450] (-) TimerEvent: {} -[0.304259] (-) TimerEvent: {} -[0.405472] (-) TimerEvent: {} -[0.507304] (-) TimerEvent: {} -[0.609513] (-) TimerEvent: {} -[0.668851] (osc_ros2) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'egg_info', '--egg-base', '../../build/osc_ros2', 'build', '--build-base', '/BA/workspace/build/osc_ros2/build', 'install', '--record', '/BA/workspace/build/osc_ros2/install.log', '--single-version-externally-managed', 'install_data'], 'cwd': '/BA/workspace/src/osc_ros2', 'env': {'HOSTNAME': '0e38e264ac6b', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/root', 'OLDPWD': '/BA', 'ROS_PYTHON_VERSION': '3', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', 'LS_COLORS': 'rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'PWD': '/BA/workspace/build/osc_ros2', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages', 'COLCON': '1'}, 'shell': False} -[0.710418] (-) TimerEvent: {} -[0.812364] (-) TimerEvent: {} -[0.916461] (-) TimerEvent: {} -[0.930157] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'} -[0.931065] (osc_ros2) StdoutLine: {'line': b'creating ../../build/osc_ros2/osc_ros2.egg-info\n'} -[0.931536] (osc_ros2) StdoutLine: {'line': b'writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO\n'} -[0.932359] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt\n'} -[0.932742] (osc_ros2) StdoutLine: {'line': b'writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt\n'} -[0.933289] (osc_ros2) StdoutLine: {'line': b'writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt\n'} -[0.933622] (osc_ros2) StdoutLine: {'line': b'writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt\n'} -[0.934053] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} -[0.936202] (osc_ros2) StdoutLine: {'line': b"reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} -[0.937431] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} -[0.937733] (osc_ros2) StdoutLine: {'line': b'running build\n'} -[0.937983] (osc_ros2) StdoutLine: {'line': b'running build_py\n'} -[0.938382] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build\n'} -[0.938896] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build/lib\n'} -[0.939373] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} -[0.939598] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} -[0.940779] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} -[0.941875] (osc_ros2) StdoutLine: {'line': b'running install\n'} -[0.942656] (osc_ros2) StdoutLine: {'line': b'running install_lib\n'} -[0.943416] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'} -[0.944159] (osc_ros2) StdoutLine: {'line': b'copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'} -[0.945268] (osc_ros2) StdoutLine: {'line': b'copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'} -[0.946403] (osc_ros2) StdoutLine: {'line': b'byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc\n'} -[0.964085] (osc_ros2) StdoutLine: {'line': b'byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc\n'} -[0.965778] (osc_ros2) StdoutLine: {'line': b'running install_data\n'} -[0.966332] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index\n'} -[0.966697] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index\n'} -[0.967204] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'} -[0.967648] (osc_ros2) StdoutLine: {'line': b'copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'} -[0.968293] (osc_ros2) StdoutLine: {'line': b'copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2\n'} -[0.969222] (osc_ros2) StdoutLine: {'line': b'running install_egg_info\n'} -[0.971222] (osc_ros2) StdoutLine: {'line': b'Copying ../../build/osc_ros2/osc_ros2.egg-info to /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info\n'} -[0.977345] (osc_ros2) StdoutLine: {'line': b'running install_scripts\n'} -[1.001801] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'} -[1.002943] (osc_ros2) StdoutLine: {'line': b"writing list of installed files to '/BA/workspace/build/osc_ros2/install.log'\n"} -[1.017483] (-) TimerEvent: {} -[1.023489] (osc_ros2) CommandEnded: {'returncode': 0} -[1.050567] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0} -[1.051948] (-) EventReactorShutdown: {} diff --git a/workspace/log/build_2025-05-25_15-14-38/logger_all.log b/workspace/log/build_2025-05-25_15-14-38/logger_all.log deleted file mode 100644 index 0849fcd..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/logger_all.log +++ /dev/null @@ -1,126 +0,0 @@ -[0.226s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] -[0.227s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=8, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, mixin_files=None, mixin=None, verb_parser=, verb_extension=, main=>, mixin_verb=('build',)) -[0.381s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters -[0.382s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta' -[0.382s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta' -[0.382s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters -[0.382s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters -[0.382s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters -[0.382s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover -[0.382s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover -[0.382s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/workspace' -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] -[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' -[0.394s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored -[0.395s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] -[0.396s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' -[0.396s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored -[0.396s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] -[0.396s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' -[0.396s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install'] -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore' -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install' -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg'] -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg' -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta'] -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta' -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros'] -[0.397s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros' -[0.398s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python'] -[0.398s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake' -[0.398s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python' -[0.398s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py'] -[0.398s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py' -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install'] -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore' -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install' -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg'] -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg' -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta'] -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta' -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros'] -[0.399s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros' -[0.405s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2' -[0.405s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults -[0.405s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover -[0.405s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults -[0.405s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover -[0.405s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None' -[0.426s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False' -[0.426s] DEBUG:colcon.colcon_core.verb:Building package 'osc_ros2' with the following arguments: {'ament_cmake_args': None, 'build_base': '/BA/workspace/build/osc_ros2', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/BA/workspace/install/osc_ros2', 'merge_install': False, 'path': '/BA/workspace/src/osc_ros2', 'symlink_install': False, 'test_result_base': None} -[0.426s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor -[0.429s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete -[0.430s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/BA/workspace/src/osc_ros2' with build type 'ament_python' -[0.430s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path') -[0.434s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems -[0.434s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1' -[0.436s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv' -[0.437s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh' -[0.438s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell -[0.438s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment -[0.704s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/BA/workspace/src/osc_ros2' -[0.705s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell -[0.705s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment -[1.112s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/BA/workspace/src/osc_ros2': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data -[1.455s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/BA/workspace/src/osc_ros2' returned '0': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data -[1.461s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake module files -[1.462s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake config files -[1.465s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib' -[1.465s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' -[1.465s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc' -[1.466s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/python3.10/site-packages' -[1.466s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath') -[1.466s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1' -[1.468s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv' -[1.469s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh' -[1.471s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' -[1.472s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2) -[1.472s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.ps1' -[1.474s] INFO:colcon.colcon_core.shell:Creating package descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/package.dsv' -[1.475s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.sh' -[1.476s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.bash' -[1.477s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.zsh' -[1.479s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/BA/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2) -[1.481s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop -[1.481s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed -[1.481s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' -[1.482s] DEBUG:colcon.colcon_core.event_reactor:joining thread -[1.512s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' -[1.512s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems -[1.512s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems -[1.512s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' -[1.514s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.NotSupported: Unable to autolaunch a dbus-daemon without a $DISPLAY for X11 -[1.514s] DEBUG:colcon.colcon_core.event_reactor:joined thread -[1.515s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.ps1' -[1.517s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_ps1.py' -[1.519s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.ps1' -[1.521s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.sh' -[1.523s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_sh.py' -[1.524s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.sh' -[1.525s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.bash' -[1.525s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.bash' -[1.526s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.zsh' -[1.527s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.zsh' diff --git a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/command.log b/workspace/log/build_2025-05-25_15-14-38/osc_ros2/command.log deleted file mode 100644 index 2b1b253..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/command.log +++ /dev/null @@ -1,2 +0,0 @@ -Invoking command in '/BA/workspace/src/osc_ros2': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data -Invoked command in '/BA/workspace/src/osc_ros2' returned '0': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data diff --git a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stderr.log b/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stderr.log deleted file mode 100644 index e69de29..0000000 diff --git a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout.log b/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout.log deleted file mode 100644 index 4bb3204..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout.log +++ /dev/null @@ -1,35 +0,0 @@ -running egg_info -creating ../../build/osc_ros2/osc_ros2.egg-info -writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO -writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt -writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt -writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt -writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt -writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -running build -running build_py -creating /BA/workspace/build/osc_ros2/build -creating /BA/workspace/build/osc_ros2/build/lib -creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -running install -running install_lib -creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc -byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc -running install_data -creating /BA/workspace/install/osc_ros2/share/ament_index -creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index -creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2 -running install_egg_info -Copying ../../build/osc_ros2/osc_ros2.egg-info to /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info -running install_scripts -Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 -writing list of installed files to '/BA/workspace/build/osc_ros2/install.log' diff --git a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout_stderr.log b/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout_stderr.log deleted file mode 100644 index 4bb3204..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/stdout_stderr.log +++ /dev/null @@ -1,35 +0,0 @@ -running egg_info -creating ../../build/osc_ros2/osc_ros2.egg-info -writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO -writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt -writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt -writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt -writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt -writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -running build -running build_py -creating /BA/workspace/build/osc_ros2/build -creating /BA/workspace/build/osc_ros2/build/lib -creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -running install -running install_lib -creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc -byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc -running install_data -creating /BA/workspace/install/osc_ros2/share/ament_index -creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index -creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2 -running install_egg_info -Copying ../../build/osc_ros2/osc_ros2.egg-info to /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info -running install_scripts -Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 -writing list of installed files to '/BA/workspace/build/osc_ros2/install.log' diff --git a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/streams.log b/workspace/log/build_2025-05-25_15-14-38/osc_ros2/streams.log deleted file mode 100644 index d2168de..0000000 --- a/workspace/log/build_2025-05-25_15-14-38/osc_ros2/streams.log +++ /dev/null @@ -1,37 +0,0 @@ -[0.680s] Invoking command in '/BA/workspace/src/osc_ros2': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data -[0.930s] running egg_info -[0.930s] creating ../../build/osc_ros2/osc_ros2.egg-info -[0.931s] writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO -[0.931s] writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt -[0.932s] writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt -[0.932s] writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt -[0.933s] writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt -[0.933s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -[0.935s] reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -[0.936s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' -[0.937s] running build -[0.937s] running build_py -[0.938s] creating /BA/workspace/build/osc_ros2/build -[0.938s] creating /BA/workspace/build/osc_ros2/build/lib -[0.938s] creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -[0.939s] copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -[0.940s] copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 -[0.941s] running install -[0.942s] running install_lib -[0.943s] creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -[0.943s] copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -[0.944s] copying /BA/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py -> /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 -[0.945s] byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc -[0.964s] byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc -[0.965s] running install_data -[0.965s] creating /BA/workspace/install/osc_ros2/share/ament_index -[0.966s] creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index -[0.966s] creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -[0.967s] copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages -[0.967s] copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2 -[0.970s] running install_egg_info -[0.970s] Copying ../../build/osc_ros2/osc_ros2.egg-info to /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info -[0.977s] running install_scripts -[1.001s] Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 -[1.002s] writing list of installed files to '/BA/workspace/build/osc_ros2/install.log' -[1.023s] Invoked command in '/BA/workspace/src/osc_ros2' returned '0': PYTHONPATH=/BA/workspace/build/osc_ros2/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 -W ignore:setup.py install is deprecated -W ignore:easy_install command is deprecated setup.py egg_info --egg-base ../../build/osc_ros2 build --build-base /BA/workspace/build/osc_ros2/build install --record /BA/workspace/build/osc_ros2/install.log --single-version-externally-managed install_data diff --git a/workspace/log/latest b/workspace/log/latest deleted file mode 120000 index b57d247..0000000 --- a/workspace/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_build \ No newline at end of file diff --git a/workspace/log/latest_build b/workspace/log/latest_build deleted file mode 120000 index 7b3fb52..0000000 --- a/workspace/log/latest_build +++ /dev/null @@ -1 +0,0 @@ -build_2025-05-25_15-14-38 \ No newline at end of file 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 index 5816b90259a07eefb03e379986e1706bb1e716d1..574fd9e23862a1e274ec780c0f324d9312a0cbaf 100644 GIT binary patch delta 6514 zcmb7I2~=CxnSS@_NeILuKw@8nF<=2^F}AURm|$Z&;1qC-<2Vjc1W(3dk>KiKBh8aV z4o-JslItXm9kj^fnI_X7ot!$0+a_+4nQ79Vo-WD!|9wva zIke{#9sS(<{ri9ad*6Lpm=~YRfjbAVr=-bgW zQ=J&G$yxJ+2#Q^fjgfMEvNLUnC&83uJuI4|n+atz~~5J>%Nb#lPlDNcoS}t4Ew?jpNbu#rH&YUnl0`A- zA_9ICSpO;%JuLXh2`uuYuNc-|Gdn9owxdu$Sn2qvEHNtgnc|K>??@_U8SkaKRUw;I zGY{E7|66hJ4oICR^Q0TR4Rx^F%o&l2rFGDAX`o-PS7##>BUMc;6wV291B}zhDsU}y zq)|s;HFTuud8d3Hb^cIIN2*k+mU#j+eV*_I{SboM zjHaOj#p$!EMcx9fE7HGel0G5}VT+p1Q3Ew=nj8hgvH-FMdO)?PY4PyK1wi&evF!3z z)dK0Ys*wZ}rrXXNSJf^<&CqMtra6oo>JWTZ;NdzMC@%kY406?q7^Vw-7!IVfq62|y zAw8g`1FF|AGA@CUGd!j?H64OzUt#{8kmj{)t zwQ=XFrHHq&L{Gt{fa7m0*Cwot_%>OuRACC(+(YBHh~d<5CLEuGkeb%m*q)hC(QA_$ zGtzPu2(7ay(N1VdpxPR0rG}P7#3pxPR`7o({)g6-rY(AFO7uES1DgHHy=YQ!PzD870=SkE71iE!L>fA zEiXews=!A4sMtNfpC4}^SPiDCVJnG{=3(r1E|?l zo0OPFl^&);0YhLt#WgF!p%V~FL2X;LQop!@$u29=0K=7 z&KvmH3UxZf$4&B05iow3PnzYMVF@-VEn}PIBMT_nEZ+ji7C^QlnlB{qA;4RcByR;| zTN2^|WP1{F8z60~5L`aYABtbhi=oxR&2KI3Al$C!{jO|;u4sn_twdDgugd=8ME1jp z>|cfKZ9IFMJc4LqCaXEHAv|g>ZiPq^D_QWUYUP62@ja6;kt{c=bSzy1-yO@sGT|Dw zzo!B1-U%1DYvp5*Or#n_#bt;u0sjqiNis<}Byp00);V1=?5eT$VmxVfE{oh(m+Xbv zt~8v=!U4fK#%{J=DQfQaJA(ch#|gI&t^s~Wz@s=Oypvuf5FN@cHP~BL^Ffe026qjx zyKLuncdzE?nkW2pJTT=R@#r=jUY}#O-Vt=TeX=9ukiFD1qImtjPRE8|qtDsqVC0%2 z#&T**D%^)@8}oOR@@Y6}ntGB?fv}*hHCku?xo8NL*09A=yVU3Rd7du#lZkXk|6JxE z?K!h9Eq+%bm{?A54J`BtEL!AvLL?{Ig?0Crn~9aA5fMHG{L>_wiHU3#x00;{My$57 zDo=V-tVjbX{D^=`uZfLj!bU2m4KIsZ*v~2(tPX<+Oq0g`l~@+Fuir_Gry!Hr=q*)F zv33rIG3kVx`snR2q+>WY9(61338#VDIoC}`1N2VTm78n)Cjdgx7i)e`$O3!bsbnk= zLHIxfv`7ebGPf`H?#(84sP;3~vw+b8B%6_(1NHpu(;!XH)W=?A@d$2HzFB8GKd5NqxS-NRN`UL`(PY1P5He(B_3T6pwCBNsIM`Cwcj-V zY=?|Wb4a+!qd2-A$r^T@sam=zZds6YVnD}08>DuMGk|B%y)mecSIKskg(f zG{bAt9@f>C&o;Lg6=tatVzLD;0Pp~Zt0|$Gh_GPWMs}v%=6sd&@tz1@IgP(|?bfw) z6fTd1?q;vF?7yVjgd8YV+~(-TUU1aVZy^6MB##3b3>9)S zc{^-5M4 zVOMt5v?jKI)3Pe678Ga#5+k7)p)oQQGX&iy=+hi>X-Vy6_XZ2^#32uZJLi6T0qc(r?0Zers%4I#EjTyW z{~al2{yrypl-=J~t(WrioXLLM_i6GkEYQE+gslr;D4j`FB!W(F1F|>0- z>yD?Hf1m=6i@OFkkWlp5fx`sc+irM{d^LLUhM%OE9!347=)*TVNc!_Ij!Cdo&GcFJ z%8@+s0(Az zcs;Z#u!nEmVEQY953rx!TDKirkG=yWmN7*=$6fLab>m|e! zDkD!vZ*(0HwJtK>YygQDVdbCNo5$XF-(NKZKmb-;$ipWA-0g*!C@V2ZagU6|jMMU@ zdzM~ej}O=(n05gs~EkC*BoaAIUys|2sf8z{>w%By@;VJ;)L7+ld3B&A5o zkdz}?snvdz<0Ux+BWH^CKuJB$zBp27!g|1Fd~C!C8}PLe#oP;7z&R|j{qi&QO17Fd zmZSXwGIS~o?<-4oMAQuYZAnhMLo9^Lo?;ubkL9s;Px*m-H4}b75XK7R3^{W_3TLTV zvIMfyxJX}83g-oQUnv4Cs}O&cv$~2EE7(g#dpI8}ah1mBau(=3K0U&|>8YqHah0j! zSmsi>YQ@{!ijA;kdIau_4!BBIun#=V`*8-p&@5HLcm1*eT2=6_2D|&s)gk(V`wF7QYrrJC?Q%M(_R*M?FAdhS^}qNn%Dnl|6J2?lRaa)QSWg z3jE1I`Ci+@Exs|@P?C!b8a7Kawmgrloc!^mkb zo0{q@J!BTmR`~Ro?dFC|r@1a|gV`!X%vCU7AANc1St+-6w@>j z?BjXSM`p4~#`O@aQ>3kEaRKksO$c08vZ~vd5RGKKkQm*@<*w zKR5^$NBjZBG3gGBJDQuDo0^)~zS$!gXVF>#5GSENmOWZx`T&*^t|qU|?u89l8hlF> z2qPg|@dF_ADAC##6)1mJZ->#JSX0Pu<$aWb1vMgho*fK*k+etO3jJIpP3*aFaTO*I zla6}f3A_aj@V5F8M|_*Oa5T#P94;gmnEhlqxy-hllr5MZeFDiKd+20`^&nbD-!W;@ z?e$%V{_jZ($qArsv&)5$%jK+~hginkXNkh@oogH&LZ;C-GdUHcxSeAbcuQ2g?ukja za*X1pjAc>(uwU_x1Qfr2V%R@R`Myp?w-=BcMz7sS!Z=MsvJMG4re!!PfX99=>%4t; NYBQ`Vymv_-|3BgHh^GJm delta 5712 zcmb7IdvH|M8NcV=&1RF$W0T#y6S7IjW62vZfe;AKfEdCfi8KN(o7|9Olk8^C-hhdB zS!Jcu5pjY?i;8HXmTHTjU5gg9YKzrT6#T>D?D#@$ZR<=O>$KyDm44s3o82UnIwO!@ z&iS45J#j)R;q(NY4f1_2pttnp_ef z6X}z}M8>4xGpGhRWk4Jk=_w_TO~mK<=S}1TAFo*C(f}a>{4P@kXPg?BB^(oE8?+Og z^W>Z?ApKOV~^)8vY2l9k6L&92X=q{&rtjwGl?|2Ux+)C5Gusu}|XFd;w& zVmM=wt5NlKVVroVYLsgRB;eoGX1a(h_>5{onEhCjdL#tL~Jb0K4k(MgvcTJy05&VJ=G z%$5O1HJ21M0W^j4=O564PlMd2>(m5rqAoi411{~*qMe#M!=)Z@q<)r5qNjno1i6jM zsPU>F*4I7{1eQ!&p&zsA;kwxlInfD9?zlMm1QGBr=v<0UbP6uA4|C#$AGa$3Dp1SZ zvzRe|BxO5XPeT?gaxx0R3N=hyim9@dXp1cL5^&XQ5u&YLQ5Eb3N+E_2AZvwP13N`u zoG&R(7;6WOT47^(CP7AL7N);y773*@F3}7qNkDB4wNyh%B4d%4aR)7OBmRcc6g{7Y zOH-m&O#+J$oGH)>uTcmWsYGiuU6FCgkK3pb)P&O~jW*AjKGE|T82ZSS98RA^JJ5u^ zU|QLUo)^Hqu0wYrBgNBl+!WZ#&jwIb1XGJ(D~S+gKwgOqI8~19 z#C&|UMKwcKb#yrlZ3@?n&Qq#d!e-XVtv(AkW5M~;%ydS~ zY}Vo$2f2NMQXatlRiUPWt6KsRZ<`=bTY0-u-sT78ZM;(@x5E-tD>cqqx#KWSt&(p9 zW+5<(kj)Sx{C40M&zSiWV6L0NbOKX1gSiQq`e_UI>M%#v{2 zugEdTKml8k)yAhqe>oEU&PenpA$kjs-XiZpHqyIUO@)HDt7a^9|BRuT6L+g7J~3r# zPefGAoaa!MPBg&JvPof*a0$x~NkF+b!X>+0?t)HT7av&04p0)IDO|=Wr=j z#clQWDAvLLp?)QJ%A6;$M>6Z!wRAQ3=giBZ_^r&|$#Dd$bMMuYRl)OxRuZ(%KP8g+ z>|)6S><^_L=`C?Ft1GKAiBde#6OkB*?sIF7CGZ7Xe z+|RD8%Sf>kXg-MjV*su6J~nSp0kJ=Jw3?73?EadDC1{-9gK#6l7KC;L^nx;gCw1Hh zdM~s?`eAp!OPO+IFHkoH3u{YbK- zeKz?O8>=rRT5w)a;0&Iv|AvtJ*w)NE@^kji()>At(?8FHV|fi8BX2$gvSVx5?Hy)z zvZ0n72YE|^9>_NI>R~}(*6O@DRi&Jbw zp{H=t5`?D^`0B^GXT)>WA- zy}5!fXkIvutJ=`v=F{XzaQXUlqSxGIof}H)qXlQ2=FuYbqXr;E#zKUK$Z$v+b?l?3 zIc3*e)Q>>#YZAP)p;;nDETg5X@&%NOZmbuv`}+u&g{`Fk#jx1)Wp<+FZc@jt+q8`Q zk$Ep}L)wq|mI8QMz1^mdKj(e_jF8}>=teEks+97|{8ZRxDA zy%q|my}hXSaUer_$MA5EOQ!wUi5(v48tnJ>(Knz+TM#z!NqZfGBRyXFTkJ@VE-yWU zV|<@Q%O<)E+P>Fn;xzlPy?`9Lr=yIl5866930M!@c#gP(P1`TV8c(32;|R-e{{}1V zu_WOv+R?#c)zatK%H8Q?oNeFzqW*oL$5t_qBc0VazF6@VN^vLO#@0myUZw-s`T`)N zAMAHI275y~mzxfSVjPq@M(J)GzREhga#rue<}QSr0U$Kj=(OW37YB3*A0T{)@Dakt z2$(*4g`MfDslw#ZPY`(GFlY2r1WXwH41woL%Mi14m+8Mn@&UHGyL=56B>f9OC~=tf z?6u1y)S>jdT_GKI>0fd9bA-PkMC$^sQ-NY#ge;H~3-JeE?QRpbDl?QyFnAu%z?O8j zYtIA43xM!KT?pxT!Y6=iNR*Y3PH}X1hhjYPkmDBmKFjH?&|~JPg>CAs_VXZkpu3Um z00_aSrw%(jJ3KKw-+T>brRWOI%j9k%WFzDtS#qMBbXez0R+D8N%+d{r z^rDg-5a3as16*>J5GW<8$)2m2KxG`7f>)c#nP;Cj)t8e&=xI+cvp5Ti^X&Pm=uDch zswRB1M3fMp(bEerqXKv^6|$S0wSMdsL2o|%6i*5uRbnqiSqKHbo(|&OAlj1@Gun|Y zXczREAqg-ClYl)wAI6+{&Uwf{NM#c?mA}LI$I=3LUE()O`8XMEfNz!*{>@^R)7bmY z;H>AP4U!bK3%W#p(ur;4u7|2e;`l5*l zNeJ*GKy_UmXz`n{50(VVw|)h*Y!%uQL@PWT;@cSP2-J+gFQmD)dnHb;Lg4$0S1R9g zyd<$)X*G^wS!w&;|2;OZA;3q2K+_Ofu{*1%=PG3%&f|}#A5+w}RP9!(K~o#EQO6<# zyqoBA2+t$@4gpU-zBy?v&dx_@L=Lak5NW4YWWrnG&Y?>E65Q4;?8TuRVqlkt@}(q_ z6#RPVQIhf`NHo)DQTS1WXV?k%mYTm~vlx{uM5sfEh7~=+Ma^gJm3A8l?GCOOzCe~b zp&1fsPbjwAJv7`u*b|bxBVDu!^>pHxzQ@(=mU}{axd(rTU$pd9wgy7-2S?xlAb(;< zylFPRMyO9a4x%S(9&+&A4<`n;US}7*6@;tWqgzxgF_S5b#Axy$JXWqr(Uz z2r&p75qQK`u{A5~+mLH!4=GJ~r3TSpg1^@r(hTKEHbZ%QnZYFZ4aLwe3x1=V)x{O8 zb16OW9~ynZ^P^=XaRaz#6KOr_xD(&raE+(jT3mQ15Mzd_>S`xGoGFG9is2s>t@KSC zlUa?qo1MG$@d8JCUyrrh?NzKp4)1_JAuJZST$=W0mAr|Q8&F(7yXth9VfZkDs$={2-$m+! zM*n+a9`31-e%&^Amm98!j%{n3L$Q1MU9w|vkZ!o=fMvl~sGaXEJGNwmcz{rxcevkW zce}cKv~BwuTX7(tTwymKkd2rZdJX)D_0mzQqH(eFAI%nBGusvB(^>~^H= zc3S~$V|91@jCj~fcU13!&wt3JYh-A6RJ+}c-eI^X9fLy-rH|si4kc6cy1(12xZQ(Y n?pr87AmY&MYX}`^btS?WcF}8Eh~Q>79PEk{VaZ86(Ovr=1;VL+ diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 6eb5678..0b28d5b 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -72,7 +72,7 @@ class OSC_ROS2_interface(Node): if robot: while True: print('+-' * 50) - set_limits = input("Do you want to set limit for x, y and z? (y/n): ").strip().lower() + 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: @@ -96,7 +96,7 @@ class OSC_ROS2_interface(Node): print(f"z: {self.z_limits}") con = True while con: - confirm = input("Do you want your robot to move in this range? (y/n): ").strip().lower() + confirm = input("Do you want your TCP to move in this range? (y/n): ").strip().lower() if confirm == 'y': break elif confirm == 'n': @@ -114,8 +114,56 @@ class OSC_ROS2_interface(Node): 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)): @@ -183,8 +231,8 @@ class OSC_ROS2_interface(Node): while True: try: print("-" * 50) - lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter to keep current): ").strip() - upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").strip() + 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) @@ -195,13 +243,13 @@ class OSC_ROS2_interface(Node): 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'.") - print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}') while True: @@ -478,17 +526,20 @@ class OSC_ROS2_interface(Node): f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.robot.qlim[1][i]}." ) else: - for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): - if self.joint_lim[0][i] is not None and position < self.joint_lim[0][i]: - desired_joint_positions[i] = self.joint_lim[0][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}." - ) - elif self.joint_lim[1][i] is not None and position > self.joint_lim[1][i]: - desired_joint_positions[i] = self.joint_lim[1][i] - self.get_logger().warn( - f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}." - ) + if self.joint_lim: + for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): + if self.joint_lim[0][i] is not None: + if position < self.joint_lim[0][i]: + desired_joint_positions[i] = self.joint_lim[0][i] + self.get_logger().warn( + f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[0][i]}." + ) + elif self.joint_lim[1][i] is not None: + if position > self.joint_lim[1][i]: + desired_joint_positions[i] = self.joint_lim[1][i] + self.get_logger().warn( + f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}." + ) self.desired = ["joint_positions"] + desired_joint_positions self.new = True @@ -704,7 +755,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[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) + out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False) if np.any(out_of_bounds): #print(fowards.t) #indices = np.where(out_of_bounds)[0] @@ -756,7 +807,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[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False) + out_of_bounds = (fowards.t[1:,0] > self.x_limits_workspace[1] if self.x_limits_workspace[1] != None else False) | (fowards.t[1:,0] < self.x_limits_workspace[0] if self.x_limits_workspace[0] != None else False) | (fowards.t[1:,1] > self.y_limits_workspace[1] if self.y_limits_workspace[1] != None else False) | (fowards.t[1:,1] < self.y_limits_workspace[0] if self.y_limits_workspace[0] != None else False) | (fowards.t[1:,2] > self.z_limits_workspace[1] if self.z_limits_workspace[1] != None else False) | (fowards.t[1:,2] < self.z_limits_workspace[0] if self.z_limits_workspace[0] != None else False) if np.any(out_of_bounds): #print(fowards.t) #indices = np.where(out_of_bounds)[0]