From f1780b673cc9d5cf41ddc44149fa7296bbf6f2b9 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Fri, 23 May 2025 13:34:13 +0200 Subject: [PATCH] AS: nothing --- com_node.py | 99 ++ 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 0 -> 283 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 0 -> 173 bytes .../__pycache__/osc_ros2.cpython-310.pyc | Bin 0 -> 33127 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-23_11-18-54/events.log | 50 + .../build_2025-05-23_11-18-54/logger_all.log | 378 ++++++ .../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 + 66 files changed, 4602 insertions(+) create mode 100644 com_node.py create mode 100644 workspace/build/.built_by create mode 100644 workspace/build/COLCON_IGNORE create mode 100644 workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py create mode 100644 workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py create mode 100644 workspace/build/osc_ros2/colcon_build.rc create mode 100644 workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh create mode 100644 workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env create mode 100644 workspace/build/osc_ros2/install.log create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt create mode 100644 workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe create mode 100644 workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc create mode 100644 workspace/build/osc_ros2/prefix_override/sitecustomize.py create mode 100644 workspace/install/.colcon_install_layout create mode 100644 workspace/install/COLCON_IGNORE create mode 100644 workspace/install/_local_setup_util_ps1.py create mode 100644 workspace/install/_local_setup_util_sh.py create mode 100644 workspace/install/local_setup.bash create mode 100644 workspace/install/local_setup.ps1 create mode 100644 workspace/install/local_setup.sh create mode 100644 workspace/install/local_setup.zsh create mode 100755 workspace/install/osc_ros2/lib/osc_ros2/interface create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/__init__.cpython-310.pyc create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc create mode 100644 workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py create mode 100644 workspace/install/osc_ros2/share/ament_index/resource_index/packages/osc_ros2 create mode 100644 workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 create mode 100644 workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.bash create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.dsv create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.ps1 create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.sh create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.xml create mode 100644 workspace/install/osc_ros2/share/osc_ros2/package.zsh create mode 100644 workspace/install/setup.bash create mode 100644 workspace/install/setup.ps1 create mode 100644 workspace/install/setup.sh create mode 100644 workspace/install/setup.zsh create mode 100644 workspace/log/COLCON_IGNORE create mode 100644 workspace/log/build_2025-05-23_11-18-54/events.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/logger_all.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/osc_ros2/command.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/osc_ros2/stderr.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout_stderr.log create mode 100644 workspace/log/build_2025-05-23_11-18-54/osc_ros2/streams.log create mode 120000 workspace/log/latest create mode 120000 workspace/log/latest_build diff --git a/com_node.py b/com_node.py new file mode 100644 index 0000000..7fa4de1 --- /dev/null +++ b/com_node.py @@ -0,0 +1,99 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32 +from math import sin, cos, pi +from trajectory_msgs.msg import JointTrajectory +from sensor_msgs.msg import JointState +import time + +# Adjusted joint limits based on nearest multiple of step angle + + + +class ArmControllerNode(Node): + + def __init__(self): + super().__init__('arm_controller_node') + self.subscription = self.create_subscription( + JointTrajectory, + '/scaled_joint_trajectory_controller/joint_trajectory', + self.joint_trajectory_callback, + 100 + ) + self.dq1_publisher = self.create_publisher(Int32, 'dq1_steps', 10) + self.dq2_publisher = self.create_publisher(Int32, 'dq2_steps', 10) + self.joint_state_publisher = self.create_publisher(JointState, 'joint_states', 10) + + self.q1_min = -1.57 + self.q1_max = 1 + self.q2_min = 0.02 + self.q2_max = 2.8 + + self.current_q1 = 0.00 + self.current_q2 = 1.57 # Initial joint angles + steps_per_revolution = 200 + gear_ratio = 19.2 + self.steps_per_radian = (steps_per_revolution * gear_ratio) / (2 * pi) + + self.ceate_timer(0.2, self.publish_joint_state) + + def publish_joint_state(self): + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.get_clock().now().to_msg() + joint_state_msg.name = ['joint_1', 'joint_2'] + joint_state_msg.position = [self.current_q1, self.current_q2] + joint_state_msg.velocity = [0.0, 0.0] + joint_state_msg.effort = [0.0, 0.0] + self.joint_state_publisher.publish(joint_state_msg) + + def joint_trajectory_callback(self, msg): + prev_timetag = 0 + for point in msg.points: + timetag = point.time_from_start.sec + point.time_from_start.nanosec / 1e9 - prev_timetag + prev_timetag = point.time_from_start.sec + point.time_from_start.nanosec / 1e9 + + new_q1=max(min(point.positions[0],self.q1_max),self.q1_min) + new_q2=max(min(point.positions[1],self.q2_max),self.q2_min) + dq1 = new_q1 - self.current_q1 + dq2 = new_q2 - self.current_q2 + dq1_steps = int(round(dq1 * self.steps_per_radian)) + dq2_steps = int(round(dq2 * self.steps_per_radian)) + + self.current_q1 += dq1_steps/self.steps_per_radian + self.current_q2 += dq2_steps/self.steps_per_radian + + dq1_steps_msg = Int32() + dq1_steps_msg.data = dq1_steps + self.dq1_publisher.publish(dq1_steps_msg) + + dq2_steps_msg = Int32() + dq2_steps_msg.data = dq2_steps + self.dq2_publisher.publish(dq2_steps_msg) + x= 0.4 * cos(self.current_q1) + 0.25025 * cos(self.current_q1+self.current_q2) + y= 0.4 * sin(self.current_q1) + 0.25025 * sin(self.current_q1+self.current_q2) + + self.get_logger().info(f"Steps taken: steps_q1 = {dq1_steps}, steps_q2 = {dq2_steps}") + self.get_logger().info(f"New joint positions (in radians): q1 = {self.current_q1}, q2 = {self.current_q2}") + self.get_logger().info(f"New x_y positions (in meters): x = {x}, y = {y}") + self.get_logger().info(f"Duration: {timetag}") + time.sleep(timetag) + + + + +def main(args=None): + try: + rclpy.init(args=args) + node = ArmControllerNode() + rclpy.spin(node) + except KeyboardInterrupt: + print('Communication to painting robot closed') + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + + + diff --git a/workspace/build/.built_by b/workspace/build/.built_by new file mode 100644 index 0000000..06e74ac --- /dev/null +++ b/workspace/build/.built_by @@ -0,0 +1 @@ +colcon diff --git a/workspace/build/COLCON_IGNORE b/workspace/build/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py b/workspace/build/osc_ros2/build/lib/osc_ros2/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py b/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py new file mode 100644 index 0000000..6f13d2c --- /dev/null +++ b/workspace/build/osc_ros2/build/lib/osc_ros2/osc_ros2.py @@ -0,0 +1,1048 @@ +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 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 [1, 1, 1, 0, 0, 0] 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 new file mode 100644 index 0000000..573541a --- /dev/null +++ b/workspace/build/osc_ros2/colcon_build.rc @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..f9867d5 --- /dev/null +++ b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh @@ -0,0 +1 @@ +# 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 new file mode 100644 index 0000000..c73f6a0 --- /dev/null +++ b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env @@ -0,0 +1,19 @@ +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 new file mode 100644 index 0000000..8ab07e3 --- /dev/null +++ b/workspace/build/osc_ros2/install.log @@ -0,0 +1,14 @@ +/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 new file mode 100644 index 0000000..275623c --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/PKG-INFO @@ -0,0 +1,12 @@ +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 new file mode 100644 index 0000000..650a016 --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt @@ -0,0 +1,16 @@ +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 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/dependency_links.txt @@ -0,0 +1 @@ + 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 new file mode 100644 index 0000000..008581c --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt @@ -0,0 +1,3 @@ +[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 new file mode 100644 index 0000000..c78337e --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/requires.txt @@ -0,0 +1,6 @@ +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 new file mode 100644 index 0000000..99ee4eb --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/top_level.txt @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/zip-safe @@ -0,0 +1 @@ + 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 new file mode 100644 index 0000000000000000000000000000000000000000..109482d94db6c4f6b4013edefa18ae8697e8f949 GIT binary patch literal 283 zcmY*Vu};H44D}_aDk`eP!ph#IS9LB3_yCqpu$)59Rj<8rk)1%BKj=?%=a;fF@e2$H zr!5F9`T0G|?+uQFK_8^{597s7=6fgpRx08~nnwZ-oC@#=2utOXRYDgXL$HZUwQOz2 zQ>SKfH-n-65D(lp+SE+VrMbH`uaQ<<<^>x6%vBzOiCkEUJQ?aX 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 new file mode 100644 index 0000000..f67eaa9 --- /dev/null +++ b/workspace/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# 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 new file mode 100644 index 0000000..03f0025 --- /dev/null +++ b/workspace/install/local_setup.bash @@ -0,0 +1,121 @@ +# 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 new file mode 100644 index 0000000..6f68c8d --- /dev/null +++ b/workspace/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# 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 new file mode 100644 index 0000000..5c15896 --- /dev/null +++ b/workspace/install/local_setup.sh @@ -0,0 +1,137 @@ +# 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 new file mode 100644 index 0000000..b648710 --- /dev/null +++ b/workspace/install/local_setup.zsh @@ -0,0 +1,134 @@ +# 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 new file mode 100755 index 0000000..61d20a1 --- /dev/null +++ b/workspace/install/osc_ros2/lib/osc_ros2/interface @@ -0,0 +1,33 @@ +#!/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 new file mode 100644 index 0000000..275623c --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/PKG-INFO @@ -0,0 +1,12 @@ +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 new file mode 100644 index 0000000..650a016 --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/SOURCES.txt @@ -0,0 +1,16 @@ +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 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/dependency_links.txt @@ -0,0 +1 @@ + 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 new file mode 100644 index 0000000..008581c --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/entry_points.txt @@ -0,0 +1,3 @@ +[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 new file mode 100644 index 0000000..c78337e --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/requires.txt @@ -0,0 +1,6 @@ +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 new file mode 100644 index 0000000..99ee4eb --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/top_level.txt @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2-1.0.0-py3.10.egg-info/zip-safe @@ -0,0 +1 @@ + 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 new file mode 100644 index 0000000..e69de29 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 new file mode 100644 index 0000000000000000000000000000000000000000..030e934c8974a61184b337d29722d9f727eb138b GIT binary patch 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 literal 0 HcmV?d00001 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 new file mode 100644 index 0000000000000000000000000000000000000000..fcb655d072fdc342fbdcb9904ded16ae3d481a76 GIT binary patch literal 33127 zcmd6Q36vbidERtS&$+V)7Q2hZ-8hKF;utJIiU$Z11WAzMwS)jr5SN5TgYDVH>@l<3 z-AiCwGc;|`4?rA6v}M_rWI7AScErT7_+Y(*{`#-_yV~8Ijp5&~Jur6u${)sJf5M08KLZaB;phA* z0xM=|bFm3t^*L=`pU^el$LHen#)L7Sm`KR8F_)ZAO{C;LF_)gtOl0IfIhQ@n{HeLF z`P@Xe#A$Oq^SydZi_P~nGMr+J=b@a@{zw zQLdXNHa#A*GFJB0n3XN*m*Nwf@!VzQ@SKz9EmpVH^J;8jYss{FXLWm_wDl6&x3p98 z8|`}wZyFmP)qG>TYL!NHzxT;%x#FI-i!-H3w`wo@18or}n2rKji9h8Q-4dSSPgkdL z(e@z7RGrEBl6$^t;h`H3=a$NI)ro6;=J(qBza z#I1PAn9?o7O1x@JBreB{*hKPFY&7Ywk@m-n^QEWDj$5jf>`5(BGw+l5u>q;vja`bZ zpxSGi7OTZpbT@A4wb+bd#jhE)SRr8{wve=9sN5L$*Us=Y3l&sfm<#H^-hDvDI(%v8 zEjBTl@)OR|Ldo{ig+jSfb_)f6?WA2ohJv$n&Y85!3vRhuu@lJb4@AlrCW~`(=ZcfF zelD8P?`pQ}yn%UfN^?`VW(tMLxuW9~3U9=&pE&UFu>%*X_N=o|oGcwESJ0!mxdRyL zf?ahE9hfVhJFu{f8K~TuKR9;4DZ8cp$T?e_E;-FyjqLe_WxE&MTg##9Ly*%9O~)U@ zpl zoJ)4Buo49OB(hgC4*_dk2;k;2JmPaOheyCr)Hu;$%IJYl+3b7AQmX7Icz18=$w zgi$Uz=DDI%vdn75M0s<*8Y-ar>&00SjcRnFc8(N+=swewX=oeD7*qPYkhginixISjZ zmNfgv@=6@6UH!?+9Q%@?t#Lgz{ zH_^9EjlSW2vy9(>wS|57Ozd?%c;3pEBhMylZaiB(^<*uH@g2peZo?=wMm|+b1tULH zS&RGaG7H<$&JNaVb=Q*>15Z2o#2TEm)RR9GtE|DV$B4O`UelHz(6IesUI(l@nDhBq zMY|ANe7>>)Gpp)+&cZuR6Pmc`sylAG>TY8t(P(Ayalpx-wJXxgbS*8t)a@T}H_JR` zE&;M0jlHIwJyOqPaP@ZKeRDHgR<;73vZ!k$7?<6M&(t#OW1opxqbd(aKZw~A#9#)3 zn7yy3q~xHrFH&;<>v7eedKdbZ@0gOSbzy{mt(IfxnzgP=8p7NqZ8jk>1J(i7dn`!2Wwq;yF<-1 zJl*+vA~J&AweDa9`&E05yhmb{KBRGubbAxV?JoAQl5ETd`uf+EbbA@}Q}xKLF zdb0-Vn3Ep8ua-j}?rx^tq(?8PcWvZt=}`&&xTmd0`r={f5k|IIrk3SQ7PcV&y^+~@ zH!~i<44}GCpju!4YzWo&u@4HWF(M9r)k+RfsmARmTgK~!%I8`z-QmbQG$1){KMlwS zhMieE5NqDa`q<`f=Nk~d&(6N6*E97VV8zpTGX>Lx%pQzqJ50955gOz6IzS^rHGgWH z0ga3qu@=c~^(_{z}^8$Ti0OX_OkM z4b;|Rybb#Sq&ySAlCd~a8wgrCZjILmq!sMv^?^t$&xfr%8@v_DU5L4R$xONXYW?Wx ziA!<0@53m4K<@WjCsFq)xzAgtG5!bKv6+L`b01^Qy!AoE974>Uj9nKC)+QB^E_hiZi$&d%sr8qVD2#T8zUp}f>0>LJog4~{hut-L-zu%xf9>3(j)KwyWzXP z7{2=t73nkFR% zZdL-2egw4I1J*gb%MPxLfZbEEtJKz#r{xV=Qd;^Pq&#S0Jd^Wr{p*(_Ep(>rSJUOrG6J#6~g zatqr|29IKxQ}}(=L@u3B{Q zsOQIgtsLRo$g=s_Bjh(HtMdyFKFhkvv$OySfO=6!y{WldSo8f(333kz1FU^!NeYf!9FZKAs>+WsSaksGgXpBq znY8a>a5sZC*7!ceonZW71h?6{<$u)Pec)a8&U-h#dyDhvJ^MuErQ%%KGRu{PB{y%L zm@9z=4Mx0lu{i0@Et~Fzs)<}nU{H4h7?I_rl6h{fSeYG_87UPf&#O7f+Z641kGIwO zw9K~x=lLbaHO~PCz}*5Eij^n~G^;CKEIWCx_mL&r#`Fgea@dPsJZyULWd9;YKeYc{|MPY<#%!Sq z@f9R!(dDIN4kMm1a@c&~x@LNbvltx9yQ8@{*&DS$oA{?uZ0CqTz=F-m3yM_D*-~l2 zoK!P~RoPxyWR1)GHY&TXZSZazO7ecp8#`u~1YtT$!A%r%Hj#{a2Mn~ghc9YgI9~>^ z0wkL7$IMn~s<<@QK)GHpjAy@JEU6g8c_cZ1-vt+ z{M)u|o4RRxhZR88q>pBwGaRWnXE!nxcy{vskqeztZxcKIf4qbIF)LM zn#79foUbm;Swwpdu#LGpx`4R92jF+u967KWy)#*@xOR1Ju4D(Y^k(%VTy%H+T}ocp z4tNhV2C?}zdGi>t7Pf_ET;m-eFt=UvLfJi!&H@FlJytP6gyAKslY#>*bjFR0QPG|T zE%U?)iZ#^`Md=*7G+8bIQ>zs_Z5ADoeLfS&K5sxbt)q4D&^`Gv{2i1b$b^sHvo~~L zp^9#^bfcw)c=hH@yl-smeHh;I#~^9<4m5_U({5P|h7507v@BxCyjQ=W8@DqK3Vf8V z=FZ4-p!JSaWv_0^l# zeXpw9yZ1C!P*_T8ZmeaiJT+AkdSJ?~&dW^7D4EQaFF&*uy&AopIgvy++lphDN=f~$t6+c~3%{zW# z!G_YxPY8~Kj@E@D)=vmUh^K|Qvg@ZWHoB1xJCybsaoG=&S2)f+0evH}p5teZUz{w7V%YBsw8c$H0O!Qd zgwK8)o%iE>n=USIsb*oT(T`K*??TJlN{@2^uG4JV}l>d(R^?8mFlyuSu; zbL`~fg-0Pzcy9c}u}7Zrhgu7S`Fnz_#~yw3=JARrPbh__}5AF@EpT zf<@hYVC^PtbkawM8E- z;s8-fQ-!|+KW8U+VyN)rO%=Y8YThT)Nv-*(>rCNa`fJHlQp;*d#H6)8+%a}o{`Tl; z7z2#yW7-(f3@xqqAx9tXl0nWa;**F^zTK1Q(Q=ypj{aqRTNbtOk2UJQqwi;}19&o! zBds6SHsEgy>P~7y_{(bjNZW;LAFiyC<&3xWSNbwHOG|5qnNL&g8ecL-^|)%&_@aK7 zo>FhKL274>1NdXBY}dqJSn{hzjwO?--8apRI>s<9^(G}3`i(c=prv2Y-!cBi=nYIH zzOE;kFNZ#3Z1BfA(mLy98~Qg?J-13{Ip%LKolN9({`JR)@eb;bW5YJH97*)5*;X>n zItJK-$aPEbJf!cGQ6E4(-o~25|q3fnhRy8g1x`qUlOGox8=?EQj^9f*0b zzPVFlgqk}&iG9I|WVs5&kMW!{;<0Fb){Y1Xrqz<5t%? zng%rDL=djpMr`-%U91Dp2+6tC1BI==j_bg%dJ!g6yg;c7D2)eD%GOg>UkzAsdhAyx?G^&EDQ*JtXG)>~R*D3qjrRA0R$5r>pa)-|=n zR2enH6n?*<+xOt&YcuV$P6_x~%({s52V&Qs>@d5l5hI;M{1IutYHBsHd)VlPn0NF& zHu_cyZ*jx;aJWSivK==vKHbfcwO~H>tw4DMwqS&f8)|6Gjs4xIp*foN_k@p40)his zz(CrcX5wcMjN6|K^Q|@tEFnX`J?4F6wQ)XdKDq=o2*tQSKa>N)Xp%$>kOdK(lxrH1 zEt|_Fi1DA8u2ezAPn%XyIPX2TdJDHgR_$%%oA}BbGiw9m}v^SHU5 zJUdDuxggx@L;J#hoE>c|wKk_lWK6?_lsj2BOC~%{!t! z`x6M>FceMtow)at3pB&*jQd_fh6&=nO}NoPLGiB#zlZfC=z|>Y1MIi=U4UwfVP^(4 zhuiT^Ko+c-{&!7V!Aid-n{Pc%%exuvC8XGY?&>wzd|7cI#;>^X8f6c+;D+Dg?gmmb;e(2T& zuENa?rrgQf4+Gb{^$z|;h~2FUp*C_g(dm;!i{{Ant`Ue(18w2;_1tM-MM0|dTy+Vu z+r0Ul!`(zM_PF2MTAg#+z8{TxFSbTETe-dcrO|=N?h>Jfy{#j%`%SS!O0QcAl)W%c zFU{q>KyK2grrG>xul)l0C*rgw)l31GRl2Ao6-A~u_moLKKFy%R#xj~w)O0G4^4qe* zWYx=yC?(zKgZ-~ihrccm!?nxz{B&b??nsk6C_BS_=?ZQP(BRyQ_(vYK4?OC2E&0ws zT-_P(=&$TF$TgdY-5FPR#yjL`S%w@w|I&~aG|CP|%I*%!?rtgDk20pd3A}r-2ac|~ z2R2^1G4L%E6{)Fk2mAv(Usj5h9;8rC{gYO5|rr+B3f;cU-Aqt36~Tm^Rw$ z-E-@;8;bKH^qaDv5bCjI0Hf@J{aqNqn=l2pOS1knpi4;B>*F0}HSAqPC()SO_eA|m zv)_wX@E-YlXx4rmHQg{&4f@~yJ`UA;r|y5YF;w>FP~q)R_fq1Y^DWQcY$R_*--l%* zFtRF*{}W;E4s&+cycHe}OGny@VB@<%>eCQypXu!b>Ybb0XD*c8$@7RRUQqP@JvYm8 z6E0xf9|#+dbp5oqrBieR3ZZD8xOw_GG(2JuBzuIpU{_x%Td>_QA1PMIOF@bQVZIqw z5`A&78Y0COIZJbI=&&Q${{qw8&RkzW+)ogFlLMt_xXD;-X=5yU;)d4Io-XWHWK0?_ z-vaUkcUnM-gyp{hSAfBIT8RS?VjIB`1)LI-qk%DyF>G8efEI zd>n+GVscVe7Z?(da$pud$#OGc4303xVY=SvACUTt#++!aTiBhR*8I{7VRshKg|(AN zWbKCig&=njbKA8C`J=U0>Z`olV(@OUnRMg5tgMjJUDBg47KcDI8*D~1-);3LEFbC3 zZ%QpeFN4;O1+4|KIN+h#$D;Xev$Y%F{Wq=NaHGvYR)#}n;YN#NG|RKtYH<=cl=|kZ zHMK+wQJk$OUQPsCE$)OU?zevsaNYv^En8!ov}QHg%da)?x%Xg)J*btCbs9^q)2=5} z_IvKmuZP_^wMyH!#@Q+i6E6l(dxcRy@C zsdxqZU$Vq!8N7kuw6}iM9*=e_a(gW^Q@Ro%u?4kSj4Lt_in{}99Zfc+I5v-hiki^= zH@xd4$X-CgFJUkp)@A7ZAeD#k8wbWs#<$>Z03`5kA)M7631|K7ExFwgc&DL4OTM!^ zuI`wUGjhP&15`)7qJP8NbNjH;0uo*543&(6o_heVSYR>x8{Y4eN`<6{$axc`l2MfV zVzMsiub!wU>nWT|Nmq6U`3+FQcUg(n{2xw5^Vie$49q9glzm0Y%5#{S z3Cn(5%F1)iKv|;uOj!2&q^vv#so1_)7AHPG2dSCHoN{mAKFfO>h%VTwx~NFPiB(Fz z8e4&smRKn(DN+)5y{~r*4c|w!2h^B=%qLY#PeZX#$O1v(j4Y5CMh@IUZu#p#Vc7@@ z>$du9*%lOr^p~@NN&t5Ox1dyFYo#UBH9DXO$`+XUyI{4qLp0I}tF2;i&=IS3B&x~8 zQK;U|U#x~?zZQ1qjaAydHC8+Q$O#yOh2~Qcto9E`j*7}g$xlSGadOq}KN=WfwIN?v z0M5Y99Wr`o1E+9^gSIO$Mpvc|hs}!*cm~L33B3or9$IXbr(q#B$F%F(sNdVxB=Bro zOk-C96?|gVq}wEb+edk{CqnzSVeS_J0e*bGco9LlVxJ{Ie3(He^xYLf-*+<87)xuO zui3t~;K#u^*gq;!g^u`q329#?KA*r1spyf3)iVWdDcKX7>P zgBij*>X&@ScxNcl6zdE1-3ha=GN|Hbp+&sU14u;i)Ve!=fZlq)<%-7)OLyTdgQ)0| zHmzNY*LBwbr3(bf<4!XPXFdEo0O=MIZpy`ReB5g|77F42)tLA>pz{vuG!wxA`D`R# zS0rDuEgvc1ZrfqFLfSWbO`GWzq#Qo4^j(Y94e2NK?Qucu5NwbN7K+r67(-$V#!M&i z1_`_YHXgo#l!b={i!H1f;L%Op>P1iJwIahFBR}{9_zr=)fR^Bhjupsb39=fb| z1qfUY%v|1o$L}9zyW_5RCboyd2 z$W=70YPex~xoVU47tmyc|6i?%d-&QvkG$U6mIg{w(B!)Bx%zv<>Klg0s{bOB)|~fT z|9xTofl*`|s2n!OM~7|dI{hrbD=^`=XBc4+_WiV2VZvlr$>IsRz9Dw0aj(3fsCtUs z{d8l@{oz*Q*dQLhQe=aIfy->UQt~y|kK3><{U8e`)p+_DbmC!X3+76GFTwqZXP!C! z=o80IAAkCJr6#T;x9EyTb84HS8HS&RTb_b+H^&{I0d{If#x@Nb9?{49Dakmw;OBS~ z=98ze|5>I59g>=5Hsa@&Dv|w8mWZs5C((mV7-uJs8(klUnOYAxlOg#fvD@`)hxBY_ zm%a3Jk=Q3epIhw&0ekE7aEkg zee{Gg1L}UJe`PH;>p1)}^u379j+~b}fF2H#6BuUiQX6CBjTSEP{~=1CTDZV?#1%!gE9i*Nr5o|HzF=HY%)9VfQ` zEg#~=bB_J*n1TCuL`F2t$4?*#RIW+@M0P=FaGX@2NXpk24&rwR__%nn0K^+Xtf7gP zKhS9$9>Bx8S*7%#C{@c}{>AiO7jYZWDCh zyi@-D_R&cWcjF&1atc4e$kboaR1(-E4YzyxG*0eQlnx`@_;3uKWi;R;*ao>89P??= zgdzmSHLd|$TCipS!5*YAz6(abku*q!RuZheW(>rJ;U@yvPFsPM3uGS2DcTvqn+>p9 znifPGWA|abBOP%nWhHYBSO8A{Wa&4lx6!Nu7rg=w59d^1o)1TRbhw9kD*HyYQgZw* z90Qnyfj&<~0gjgD{p94*yj{-~*Vt1wemfz!6Vb5A<=*|7?)LhXmh<%)W z1S7@cDmW_Q#{q%9!9+hxm__XTf)gRpxY_zcIAgI;@~UMEbvCnA2^9m#kOmhy0Hg0s znDIXBy)ZJ@y-o`7&h@ll93KARHp|83?85ZE7WW(se5??2HEfOCfFRg{pFbsdgZoBI zIo;YAn4;KB5*%FW#rW04)znpL1H#yq^tBk`^{cU~naTiCpg^}0-1;K9!%e!86Zv3z zW2Gct@@n#G`fBcK{Hk`97PGLFEyUfd+r=%|?ON%%lDv|>lA8uVL|dLo5oBch)2_4YnS!Q@yo{L#O37W)aCT$%;oImuFJWpZt=RAc6+aYm>|bik7u}H zXGk%{&6i0Jriu+zg4&3Gf-NR)LDbB zG$b#|VYcG-+a2>Fbr#OjEbU?Talr}9x>^QKmNa`dBt!a0m}TJ5r*e`aQxJby@Y>1{ z?xXGoHL{rLvIigh1)d3)q986IL~3M4zFnE z{>Uue0%DQ-WXIf*Id6ek-JXf$zctM2_P2D*-&%hYW_A11k^HxYS>68ej`<_?M_^XB ze<7UfCd^*KtYsn7A~fjsZ{U5C6&QHdyWylua6*l^WbewpfH7DD#-OL(3nyRu0jv31 zPpwxNgFaylvT%O(dr=4tR)TJSjgIy@!9Q{gVQU5Cqn!g_A9$C$N3QkUyImb@C49@< zTocl>=&Fd@9}eqy&#MBA#TcBgeFqo|7($A^myE?h$lB<{N*qmz3$}QD&C56m0>(n_ zMAkO!V zp9qegrQB^ryOO}Zml%#=X9IBrM^)NDY+dY13VR`TzP8w|E4eFO?Xlc3A&CS5c16Du zzoNINUx{7OuAuI=x-wU?S2FFftZN{q#941>@_O(Etm6=B{M7L~{rJff&x@O*H@ZO= z(<}!@@4OuJ4chAPl_j#pycBfn)Y1w@{rbDBEaJU)g`g z0^L#|WKIk|+JDaa-em9>48Dzlu%)1xlvC`0H*K57Z{lkE9C919U4U(Z0sdnM+{l1V zo+OO}#W^hxt~Ixh_6ZM*4(uD+793#iF`)HIt0S)PLyi>1%l2qIj!bQtI^aqjn32%6 zp7f(Z>Fr@@1DubEcGS^Z)}apB@&NKM>ez$Bfvq)#Cc>+7zin^W!XJ_0&EhBg(cRRc zytImtu962fxvWS`#jKi<#aUI0aCli9){%suEhI z;CW`SFw~4`-d)l_xn_iNO(4&@nhq{%xF!_n;*l#524e5qfZs;Yllsi2!e;afPK|4_ z4_bK3f*G0^Y+L>`cFiPq%^r6N0?s_8oWXCC6x`7AV9&kf`Qvm;g^{YpRWLFV6cI~Mh2T0G*O#Q zg4z7cOXXs4)J=35hV<}W)=3N%!9Fs6A_W=jIH5Z@n7dI%2l`QG;KzVU`tj>SxDyCr z0H}Z{WYbN~1~=ukO;~9G0Ln$U?jOO=c@oeH%T=Qx7ip|~(8ZK$bXfUFk(G~Ya)ky; z5KYZwK%qlwAcy3_*p)2hBoyd_!;hNwB*`JnLXTZJj>Thet2=RnWK3pxT;TZ70YY^KI#D`65gm;f4yrF$G%of;H+&>2DH5q z_-hzF1#cO=4LNYdb4$tXC>e0kXyJ~+F1CXafwSpvrQN7r;9HNZ>~7#d$e%{xNSAJ# z_dq4FHQ*e&-M!X;;u#`15IlppZN`-tP}EnWR8Nc!cEE+yvU9OI+=B(#K+6shCd#l?gb?rV@3zjWr??W)U@8(!hSqyce zFZZ*GJOe?Kf*gfL6D&v!X&+$fM|k%Q2JbL{Kq+PqGN6dQ9fNklo1)N_Q^D^Zg9fzW z7~3Q66c$O?BjJbapJv?8Fd(O-2LB*413F_Od{!Q8w9~f9m^OSXB%k2h^{dX#y~MeX zqIQye3g-?9v4t~T_@mhBX!|0GBg&Zfs9EoU-l_YQs?U>k4^avQJdFTuM047N<*MUbsJBb8>`SDSc8eC&X zBoLq|UxuU>5(v3d6qWZNt$paMwq!!NWmmy^Kxxrp`lDPKHN_AkPH2gpSC-XcE&Z7N zlLVgcXYgYj*pW7`N{7woDzo5uDxt8$U0x`eBc2Xdi)(Jk3{8uWCZ}29TMX_X>>Xlo z4}<245L)3A$kO;+P?Hl0VbgvE=`Rur58+0K%7Z3$O8G@~7ukmP>n5c4!~+wA?HypQ zt_YcIa=!r1-$>PK8ovgt^2x`xvq^| z*S25RKJqtQk^lK8mpEL1{L1$R*VlvV_XO8Zfme4X%Vj@hmlo!rZNmws8CI&pXBNC} z%`ZT@Kvmm0`ys-{^Gr*^Yaq_g`bn$AYv3xz{spFW6)R4;P=fayaLEEBA*8_h8r(ov z#A6~o#8sw0O0elcv9z9z=d|yPr{n!`L-%%c0EzapH^4-QeLjo|#p+M=V=FNC#&^f` zYvLWqin~x=(kKN63v6U{QaIcqZ>Au30eYL{2*HD zHm&*KYAp0EL~A}gAMjJpjXyR1?3r;dv9Ihn&h#h0wCgXr@@pQniN?oaMw`La{x1wp zBJkr7ZNQlxJf6;pj#MT{(e_Po$Yy4LoKqwl5lMi?ya-heu`U{xpa5wBiGeKUiL3|?a3Fu1^A zfx!|3(pC183A~f*5Bni3*(i8V7x$k!& zUSLi^S~=7gce}-MiE&Vm^5bN+_)4Dzb6in)LC3a`3+*!yb(P}yTj8lYt#3O$mNo3 z!p!>`zE6NT*er@!#praQ+!m%`);1^wptZG*Z{N&pbiR(S)tET2_uujLt3+iDp0+`4 zl5;n$Y__&jnb$1C{Pv$Q1Y6j(vW4?sOF)ge7h|!*+6lG?GrKNk-9gk5LPR%|)7^v# zsXuPp|>OKZ3fH*L`cn9*e&mnu|`!YQuauUZtYd= z(bj@cXzi2MPFJ$EG+KSJmRK=qalz49d^oI8xAyb6oRydFRsn+R_yUxaSG{%dU6PFY z9u>;n379x6PzF=-vvur9*dozjm_%h#;F1rUbs6#>F}!fybr z18er9;atmCrNZwgT|%3$03!IECFc7=nD2dTG0gW-jH6-oT#d;(!dPNWCERUt?#2N8 z8TBr2k4N+H?P~5(C%;Ix19kq7u+BS!I=e0a3LCTQ?vxL9_MGY%*P09N!SyB7`2k?`Iw|cZt(EXBpz;`TXhdJ09oxjfA54+{s5DgGK>o zjHBL>mz?Iu2!zG(wl#hGE)?NfJ5Xtb#c{|ZkzcZ{Denli3Q}X>4=U0~+xHt99;w4x z@?Pk3pRKHty<-RRH*g3cC;brcs&LYqgbIpTkaEyqiKiRj;)qZ1@fLX^w*ZO`X~wmN z1Fi(*bf9$oG-T$~yp;SbJ)V~}~ z_*2G5L@=8U&fY_{1((1MEVJm+49D-GbA#sAE#mP;7$YGgGK25NjZ877ewOzl1nw~B zpU0dl`_a3bOR6~xyYGhB*Db-xExu)PkeaqJcqGGUsX;#S09^gc@E(MlbMeCA?Q7U+ z22S6@W3`d0!V0`z)+yhX;3OT)dwbf7w|tc$?2Gz1$ekSFDd=gd7pO6ePPq&uz8}yw z?QP${A&J8=TZ^A#B9L{N!&!3>CF&#zJ_f|qrw4-duOpKh*tl9u_cG#B2;R_?+(iaC zTvNZu@jZ-WoXowGS;rXs6axcG(gsx>NJx$#&O3Gn%TCmv2gmU3gGupO_au$^PSHDW zNKg~ul`aA_CR;EryvZrK)=oCijUZ4S|IATuSLZ*phYFTM8YH9ya+xBHe2&UugjDP4R!8KL7A-DERh+3&8Mm z66~kL?@wUoy7A)*lr+goR6Px?dGq#zeC6HhRhq9Mj^jsEe=#Admskimq=9$v#?=_Y z);CgVuroURh6T`-_+}Ev|4(w3Pphxn+SO&cTE~|j2-o}&EkDpJSj9WcO{oLg9?Z|$;ce+Zou2Hc*&ov*`4*5JPDC(8~ucR!7( z6OZTi8Od<`G|W2qA#gths>H5VU7KQ6KY9El9h~?EH+kr&VZ1j?JXB76cZSn5~ZpqpM{TX zqolv)snYVfDwN|-@Jn6x(gHkg^Qe#7!m!Rqd;K&pSAi4^9EY>V>K+QJ5^9zA5&HyU zE?X37vaA0vA3wmC>1oV&!xgyn2V#vSwn-1)*x)ABhsPb+#T7|#5`~4Oq-{yL^4&rc zATBK~S(K-2N=^g6(7OJ1Hbb9TbPoXE&~bR-jzgrOr{R4It^!bseyY>ZdTl`ZPVvbF zpWXjwXO0Sb%E;);S}E+AmtMS+*OTz2O_?sebMwa!grk0mr!Q<(C*!D5qtYIEuBTH} z?ITt<{z~sx`IPtS0oEFLucps%SHUf!4o{x6#t? zq&0hisF{T=k+XA0?h4xZ58@ z74|U(9v`_bM3f+-q{JglawFUm{qzy_ZS)6GGUjk&4uq5xH QD5LS0u0wpm>3-+`0g1aHod5s; literal 0 HcmV?d00001 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 new file mode 100644 index 0000000..6f13d2c --- /dev/null +++ b/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py @@ -0,0 +1,1048 @@ +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 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 [1, 1, 1, 0, 0, 0] 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 new file mode 100644 index 0000000..e69de29 diff --git a/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 b/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 new file mode 100644 index 0000000..aff3120 --- /dev/null +++ b/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2 @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..79d4c95 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..26b9997 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1 @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..f3041f6 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..257067d --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv @@ -0,0 +1 @@ +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 new file mode 100644 index 0000000..caffe83 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1 @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..660c348 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh @@ -0,0 +1,3 @@ +# 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 new file mode 100644 index 0000000..92f5b25 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.bash @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 0000000..ece85b1 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.dsv @@ -0,0 +1,6 @@ +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 new file mode 100644 index 0000000..ed891bc --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.ps1 @@ -0,0 +1,116 @@ +# 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 new file mode 100644 index 0000000..2631417 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.sh @@ -0,0 +1,87 @@ +# 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 new file mode 100644 index 0000000..086e035 --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.xml @@ -0,0 +1,20 @@ + + + + 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 new file mode 100644 index 0000000..b8ae59f --- /dev/null +++ b/workspace/install/osc_ros2/share/osc_ros2/package.zsh @@ -0,0 +1,42 @@ +# 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 new file mode 100644 index 0000000..10ea0f7 --- /dev/null +++ b/workspace/install/setup.bash @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 0000000..558e9b9 --- /dev/null +++ b/workspace/install/setup.ps1 @@ -0,0 +1,29 @@ +# 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 new file mode 100644 index 0000000..fa9641d --- /dev/null +++ b/workspace/install/setup.sh @@ -0,0 +1,45 @@ +# 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 new file mode 100644 index 0000000..54799fd --- /dev/null +++ b/workspace/install/setup.zsh @@ -0,0 +1,31 @@ +# 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 new file mode 100644 index 0000000..e69de29 diff --git a/workspace/log/build_2025-05-23_11-18-54/events.log b/workspace/log/build_2025-05-23_11-18-54/events.log new file mode 100644 index 0000000..1715eb4 --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/events.log @@ -0,0 +1,50 @@ +[0.000000] (-) TimerEvent: {} +[0.000713] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()} +[0.000892] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'} +[0.099350] (-) TimerEvent: {} +[0.204846] (-) TimerEvent: {} +[0.306030] (-) TimerEvent: {} +[0.411047] (-) TimerEvent: {} +[0.513096] (-) TimerEvent: {} +[0.619096] (-) TimerEvent: {} +[0.627853] (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.719851] (-) TimerEvent: {} +[0.823949] (-) TimerEvent: {} +[0.834145] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'} +[0.834903] (osc_ros2) StdoutLine: {'line': b'creating ../../build/osc_ros2/osc_ros2.egg-info\n'} +[0.835327] (osc_ros2) StdoutLine: {'line': b'writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO\n'} +[0.836046] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt\n'} +[0.836452] (osc_ros2) StdoutLine: {'line': b'writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt\n'} +[0.836989] (osc_ros2) StdoutLine: {'line': b'writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt\n'} +[0.837272] (osc_ros2) StdoutLine: {'line': b'writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt\n'} +[0.837616] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} +[0.839458] (osc_ros2) StdoutLine: {'line': b"reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} +[0.840438] (osc_ros2) StdoutLine: {'line': b"writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt'\n"} +[0.840670] (osc_ros2) StdoutLine: {'line': b'running build\n'} +[0.840842] (osc_ros2) StdoutLine: {'line': b'running build_py\n'} +[0.841140] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build\n'} +[0.841463] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build/lib\n'} +[0.841699] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} +[0.841954] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} +[0.842490] (osc_ros2) StdoutLine: {'line': b'copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2\n'} +[0.843272] (osc_ros2) StdoutLine: {'line': b'running install\n'} +[0.843860] (osc_ros2) StdoutLine: {'line': b'running install_lib\n'} +[0.844143] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2\n'} +[0.844392] (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.845074] (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.845941] (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.857320] (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.857970] (osc_ros2) StdoutLine: {'line': b'running install_data\n'} +[0.858218] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index\n'} +[0.858539] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index\n'} +[0.858748] (osc_ros2) StdoutLine: {'line': b'creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'} +[0.858940] (osc_ros2) StdoutLine: {'line': b'copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages\n'} +[0.859184] (osc_ros2) StdoutLine: {'line': b'copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2\n'} +[0.859981] (osc_ros2) StdoutLine: {'line': b'running install_egg_info\n'} +[0.861241] (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.865248] (osc_ros2) StdoutLine: {'line': b'running install_scripts\n'} +[0.886317] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'} +[0.887220] (osc_ros2) StdoutLine: {'line': b"writing list of installed files to '/BA/workspace/build/osc_ros2/install.log'\n"} +[0.901360] (osc_ros2) CommandEnded: {'returncode': 0} +[0.914716] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0} +[0.915734] (-) EventReactorShutdown: {} diff --git a/workspace/log/build_2025-05-23_11-18-54/logger_all.log b/workspace/log/build_2025-05-23_11-18-54/logger_all.log new file mode 100644 index 0000000..afe3134 --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/logger_all.log @@ -0,0 +1,378 @@ +[0.201s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] +[0.201s] 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.348s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.348s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta' +[0.349s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta' +[0.349s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.349s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.349s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.349s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.349s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.349s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/workspace' +[0.349s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.350s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.360s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.361s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install'] +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg'] +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta'] +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros'] +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python'] +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python' +[0.362s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['cmake', 'python'] +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'cmake' +[0.363s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'python' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['python_setup_py'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'python_setup_py' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['ignore', 'ignore_ament_install'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'ignore' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'ignore_ament_install' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['colcon_pkg'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'colcon_pkg' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['colcon_meta'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'colcon_meta' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['ros'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'ros' +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['cmake', 'python'] +[0.364s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'cmake' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'python' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extensions ['python_setup_py'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control) by extension 'python_setup_py' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['ignore', 'ignore_ament_install'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'ignore' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'ignore_ament_install' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['colcon_pkg'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'colcon_pkg' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['colcon_meta'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'colcon_meta' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['ros'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'ros' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['cmake', 'python'] +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'cmake' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'python' +[0.365s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extensions ['python_setup_py'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control/joint_control/__pycache__) by extension 'python_setup_py' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['cmake', 'python'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'cmake' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'python' +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['python_setup_py'] +[0.366s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'python_setup_py' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['ignore', 'ignore_ament_install'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'ignore' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'ignore_ament_install' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['colcon_pkg'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'colcon_pkg' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['colcon_meta'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'colcon_meta' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['ros'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'ros' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['cmake', 'python'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'cmake' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'python' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extensions ['python_setup_py'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info) by extension 'python_setup_py' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['ignore', 'ignore_ament_install'] +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'ignore' +[0.367s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'ignore_ament_install' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['colcon_pkg'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'colcon_pkg' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['colcon_meta'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'colcon_meta' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['ros'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'ros' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['cmake', 'python'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'cmake' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'python' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extensions ['python_setup_py'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/joint_info/__pycache__) by extension 'python_setup_py' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['ignore', 'ignore_ament_install'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'ignore' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'ignore_ament_install' +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['colcon_pkg'] +[0.368s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'colcon_pkg' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['colcon_meta'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'colcon_meta' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['ros'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'ros' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['cmake', 'python'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'cmake' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'python' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extensions ['python_setup_py'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/resource) by extension 'python_setup_py' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['ignore', 'ignore_ament_install'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'ignore' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'ignore_ament_install' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['colcon_pkg'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'colcon_pkg' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['colcon_meta'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'colcon_meta' +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['ros'] +[0.369s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'ros' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['cmake', 'python'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'cmake' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'python' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extensions ['python_setup_py'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info/test) by extension 'python_setup_py' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros' +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['cmake', 'python'] +[0.370s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'cmake' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'python' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['python_setup_py'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'python_setup_py' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['ignore', 'ignore_ament_install'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'ignore' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'ignore_ament_install' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['colcon_pkg'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'colcon_pkg' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['colcon_meta'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'colcon_meta' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['ros'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'ros' +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['cmake', 'python'] +[0.371s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'cmake' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'python' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extensions ['python_setup_py'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot) by extension 'python_setup_py' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['ignore', 'ignore_ament_install'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'ignore' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'ignore_ament_install' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['colcon_pkg'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'colcon_pkg' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['colcon_meta'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'colcon_meta' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['ros'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'ros' +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['cmake', 'python'] +[0.372s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'cmake' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'python' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extensions ['python_setup_py'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/mock_robot/__pycache__) by extension 'python_setup_py' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['ignore', 'ignore_ament_install'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'ignore' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'ignore_ament_install' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['colcon_pkg'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'colcon_pkg' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['colcon_meta'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'colcon_meta' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['ros'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'ros' +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['cmake', 'python'] +[0.373s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'cmake' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'python' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extensions ['python_setup_py'] +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/resource) by extension 'python_setup_py' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['ignore', 'ignore_ament_install'] +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'ignore' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'ignore_ament_install' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['colcon_pkg'] +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'colcon_pkg' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['colcon_meta'] +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'colcon_meta' +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['ros'] +[0.374s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'ros' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['cmake', 'python'] +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'cmake' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'python' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extensions ['python_setup_py'] +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot/test) by extension 'python_setup_py' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install'] +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg'] +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta'] +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta' +[0.375s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros'] +[0.376s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros' +[0.380s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['cmake', 'python'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'cmake' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'python' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['python_setup_py'] +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'python_setup_py' +[0.381s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['ignore', 'ignore_ament_install'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'ignore' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'ignore_ament_install' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['colcon_pkg'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'colcon_pkg' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['colcon_meta'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'colcon_meta' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['ros'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'ros' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['cmake', 'python'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'cmake' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'python' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extensions ['python_setup_py'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control) by extension 'python_setup_py' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['ignore', 'ignore_ament_install'] +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'ignore' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'ignore_ament_install' +[0.382s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['colcon_pkg'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'colcon_pkg' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['colcon_meta'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'colcon_meta' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['ros'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'ros' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['cmake', 'python'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'cmake' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'python' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extensions ['python_setup_py'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/painting_robot_control/__pycache__) by extension 'python_setup_py' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['ignore', 'ignore_ament_install'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'ignore' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'ignore_ament_install' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['colcon_pkg'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'colcon_pkg' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['colcon_meta'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'colcon_meta' +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['ros'] +[0.383s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'ros' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['cmake', 'python'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'cmake' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'python' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extensions ['python_setup_py'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/resource) by extension 'python_setup_py' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['ignore', 'ignore_ament_install'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'ignore' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'ignore_ament_install' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['colcon_pkg'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'colcon_pkg' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['colcon_meta'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'colcon_meta' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['ros'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'ros' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['cmake', 'python'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'cmake' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'python' +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extensions ['python_setup_py'] +[0.384s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control/test) by extension 'python_setup_py' +[0.385s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.385s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.385s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.385s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.385s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None' +[0.405s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False' +[0.405s] 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.405s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.408s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.409s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/BA/workspace/src/osc_ros2' with build type 'ament_python' +[0.409s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path') +[0.412s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[0.412s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1' +[0.414s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv' +[0.415s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh' +[0.415s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[0.415s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment +[0.666s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/BA/workspace/src/osc_ros2' +[0.667s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[0.667s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment +[1.043s] 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.310s] 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.313s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake module files +[1.314s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake config files +[1.316s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib' +[1.316s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' +[1.316s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc' +[1.317s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/python3.10/site-packages' +[1.317s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath') +[1.317s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1' +[1.317s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv' +[1.318s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh' +[1.318s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' +[1.318s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2) +[1.319s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.ps1' +[1.319s] INFO:colcon.colcon_core.shell:Creating package descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/package.dsv' +[1.320s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.sh' +[1.321s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.bash' +[1.322s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.zsh' +[1.323s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/BA/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2) +[1.323s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.324s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.324s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.324s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.342s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[1.342s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.342s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.342s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.344s] 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.344s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.344s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.ps1' +[1.345s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_ps1.py' +[1.347s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.ps1' +[1.347s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.sh' +[1.348s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_sh.py' +[1.349s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.sh' +[1.349s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.bash' +[1.350s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.bash' +[1.351s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.zsh' +[1.351s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.zsh' diff --git a/workspace/log/build_2025-05-23_11-18-54/osc_ros2/command.log b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/command.log new file mode 100644 index 0000000..2b1b253 --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/command.log @@ -0,0 +1,2 @@ +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-23_11-18-54/osc_ros2/stderr.log b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stderr.log new file mode 100644 index 0000000..e69de29 diff --git a/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout.log b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout.log new file mode 100644 index 0000000..4bb3204 --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout.log @@ -0,0 +1,35 @@ +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-23_11-18-54/osc_ros2/stdout_stderr.log b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout_stderr.log new file mode 100644 index 0000000..4bb3204 --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/stdout_stderr.log @@ -0,0 +1,35 @@ +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-23_11-18-54/osc_ros2/streams.log b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/streams.log new file mode 100644 index 0000000..799c7ec --- /dev/null +++ b/workspace/log/build_2025-05-23_11-18-54/osc_ros2/streams.log @@ -0,0 +1,37 @@ +[0.633s] 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.834s] running egg_info +[0.834s] creating ../../build/osc_ros2/osc_ros2.egg-info +[0.835s] writing ../../build/osc_ros2/osc_ros2.egg-info/PKG-INFO +[0.835s] writing dependency_links to ../../build/osc_ros2/osc_ros2.egg-info/dependency_links.txt +[0.836s] writing entry points to ../../build/osc_ros2/osc_ros2.egg-info/entry_points.txt +[0.836s] writing requirements to ../../build/osc_ros2/osc_ros2.egg-info/requires.txt +[0.836s] writing top-level names to ../../build/osc_ros2/osc_ros2.egg-info/top_level.txt +[0.837s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' +[0.839s] reading manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' +[0.840s] writing manifest file '../../build/osc_ros2/osc_ros2.egg-info/SOURCES.txt' +[0.840s] running build +[0.840s] running build_py +[0.840s] creating /BA/workspace/build/osc_ros2/build +[0.841s] creating /BA/workspace/build/osc_ros2/build/lib +[0.841s] creating /BA/workspace/build/osc_ros2/build/lib/osc_ros2 +[0.841s] copying osc_ros2/osc_ros2.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 +[0.842s] copying osc_ros2/__init__.py -> /BA/workspace/build/osc_ros2/build/lib/osc_ros2 +[0.843s] running install +[0.843s] running install_lib +[0.843s] creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2 +[0.844s] 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.844s] 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.845s] byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/osc_ros2.py to osc_ros2.cpython-310.pyc +[0.857s] byte-compiling /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc_ros2/__init__.py to __init__.cpython-310.pyc +[0.857s] running install_data +[0.857s] creating /BA/workspace/install/osc_ros2/share/ament_index +[0.858s] creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index +[0.858s] creating /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages +[0.858s] copying resource/osc_ros2 -> /BA/workspace/install/osc_ros2/share/ament_index/resource_index/packages +[0.858s] copying package.xml -> /BA/workspace/install/osc_ros2/share/osc_ros2 +[0.860s] running install_egg_info +[0.860s] 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.866s] running install_scripts +[0.886s] Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 +[0.886s] writing list of installed files to '/BA/workspace/build/osc_ros2/install.log' +[0.901s] 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 new file mode 120000 index 0000000..b57d247 --- /dev/null +++ b/workspace/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/workspace/log/latest_build b/workspace/log/latest_build new file mode 120000 index 0000000..98e1656 --- /dev/null +++ b/workspace/log/latest_build @@ -0,0 +1 @@ +build_2025-05-23_11-18-54 \ No newline at end of file