From 4da5338ca8563f793dda33aeddfaa7ae5a5033b6 Mon Sep 17 00:00:00 2001 From: Alexander Schaefer Date: Thu, 8 May 2025 13:52:59 +0200 Subject: [PATCH] AS: changed interpolation for angles using unit quaterions instead of rpy --- osc_recieve.py | 4 +- .../colcon_command_prefix_setup_py.sh.env | 6 +- .../osc_ros2/osc_ros2.egg-info/SOURCES.txt | 1 + .../osc_ros2.egg-info/entry_points.txt | 1 + .../__pycache__/sitecustomize.cpython-310.pyc | Bin 283 -> 283 bytes .../install/osc_ros2/lib/osc_ros2/interface1 | 33 + .../log/build_2025-05-08_11-12-16/events.log | 38 + .../build_2025-05-08_11-12-16/logger_all.log | 174 ++++ .../osc_ros2/command.log | 2 + .../osc_ros2/stderr.log | 0 .../osc_ros2/stdout.log | 16 + .../osc_ros2/stdout_stderr.log | 16 + .../osc_ros2/streams.log | 18 + workspace/log/latest_build | 2 +- workspace/src/joint_angles.py | 2 +- .../__pycache__/osc_ros2.cpython-310.pyc | Bin 23280 -> 24934 bytes .../osc_ros2_unit_quater.cpython-310.pyc | Bin 0 -> 24940 bytes workspace/src/osc_ros2/osc_ros2/osc_ros2.py | 4 +- .../osc_ros2/osc_ros2/osc_ros2_unit_quater.py | 773 ++++++++++++++++++ workspace/src/osc_ros2/setup.py | 1 + 20 files changed, 1082 insertions(+), 9 deletions(-) create mode 100755 workspace/install/osc_ros2/lib/osc_ros2/interface1 create mode 100644 workspace/log/build_2025-05-08_11-12-16/events.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/logger_all.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/osc_ros2/command.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/osc_ros2/stderr.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout_stderr.log create mode 100644 workspace/log/build_2025-05-08_11-12-16/osc_ros2/streams.log create mode 100644 workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2_unit_quater.cpython-310.pyc create mode 100644 workspace/src/osc_ros2/osc_ros2/osc_ros2_unit_quater.py diff --git a/osc_recieve.py b/osc_recieve.py index 2decd4f..8db6630 100644 --- a/osc_recieve.py +++ b/osc_recieve.py @@ -5,8 +5,8 @@ def handler(address,*args): print(args) osc_startup() -osc_udp_server("0.0.0.0", 7000, "osc_server") -osc_method("/time", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK) +osc_udp_server("0.0.0.0", 8000, "osc_server") +osc_method("/*", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK) while True: 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 index 9454844..aafd015 100644 --- a/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env +++ b/workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env @@ -1,4 +1,4 @@ -AMENT_PREFIX_PATH=/BA/workspace/install/osc_ros2:/BA/workspace/install/painting_robot_control:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble +AMENT_PREFIX_PATH=/BA/workspace/install/painting_robot_control:/BA/workspace/install/osc_ros2:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble COLCON=1 COLCON_PREFIX_PATH=/BA/workspace/install HOME=/root @@ -7,10 +7,10 @@ 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/workspace/src +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=/BA/workspace/build/osc_ros2:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages +PYTHONPATH=/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/osc_ros2:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages ROS_DISTRO=humble ROS_LOCALHOST_ONLY=0 ROS_PYTHON_VERSION=3 diff --git a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt b/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt index 29aa2b0..d1bd27f 100644 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt @@ -3,6 +3,7 @@ setup.cfg setup.py osc_ros2/__init__.py osc_ros2/osc_ros2.py +osc_ros2/osc_ros2_unit_quater.py osc_ros2.egg-info/PKG-INFO osc_ros2.egg-info/SOURCES.txt osc_ros2.egg-info/dependency_links.txt 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 index 008581c..e07b640 100644 --- a/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt +++ b/workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt @@ -1,3 +1,4 @@ [console_scripts] interface = osc_ros2.osc_ros2:main +interface1 = osc_ros2.osc_ros2_unit_quater:main 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 index 61a20a467b61bcb4270dca4688432a06590af810..09a7b7f206b548d123328e720b29c257b01432f1 100644 GIT binary patch delta 20 acmbQuG@FS#pO=@50SG2el-bBFzz6^@dQo) diff --git a/workspace/install/osc_ros2/lib/osc_ros2/interface1 b/workspace/install/osc_ros2/lib/osc_ros2/interface1 new file mode 100755 index 0000000..1a76e3e --- /dev/null +++ b/workspace/install/osc_ros2/lib/osc_ros2/interface1 @@ -0,0 +1,33 @@ +#!/usr/bin/python3 +# EASY-INSTALL-ENTRY-SCRIPT: 'osc-ros2','console_scripts','interface1' +import re +import sys + +# for compatibility with easy_install; see #2198 +__requires__ = 'osc-ros2' + +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', 'console_scripts', 'interface1')()) diff --git a/workspace/log/build_2025-05-08_11-12-16/events.log b/workspace/log/build_2025-05-08_11-12-16/events.log new file mode 100644 index 0000000..1f67c10 --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/events.log @@ -0,0 +1,38 @@ +[0.000000] (-) TimerEvent: {} +[0.001144] (-) JobUnselected: {'identifier': 'joint_control'} +[0.001287] (-) JobUnselected: {'identifier': 'joint_info'} +[0.001722] (-) JobUnselected: {'identifier': 'mock_robot'} +[0.001807] (-) JobUnselected: {'identifier': 'painting_robot_control'} +[0.001845] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()} +[0.001927] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'} +[0.098870] (-) TimerEvent: {} +[0.202777] (-) TimerEvent: {} +[0.307583] (-) TimerEvent: {} +[0.408819] (-) TimerEvent: {} +[0.511739] (-) TimerEvent: {} +[0.614804] (-) TimerEvent: {} +[0.715769] (-) TimerEvent: {} +[0.742076] (osc_ros2) Command: {'cmd': ['/usr/bin/python3', '-W', 'ignore:setup.py install is deprecated', '-W', 'ignore:easy_install command is deprecated', 'setup.py', 'develop', '--editable', '--build-directory', '/BA/workspace/build/osc_ros2/build', '--no-deps', 'symlink_data'], 'cwd': '/BA/workspace/build/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', 'COLCON_PREFIX_PATH': '/BA/workspace/install', 'ROS_DISTRO': 'humble', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'TERM': 'xterm', 'ROS_LOCALHOST_ONLY': '0', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin', 'LANG': 'C.UTF-8', 'LS_COLORS': 'rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:', 'AMENT_PREFIX_PATH': '/BA/workspace/install/painting_robot_control:/BA/workspace/install/osc_ros2:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/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:/BA/workspace/build/painting_robot_control:/BA/workspace/install/painting_robot_control/lib/python3.10/site-packages:/BA/workspace/build/osc_ros2:/BA/workspace/install/osc_ros2/lib/python3.10/site-packages:/BA/workspace/build/mock_robot:/BA/workspace/install/mock_robot/lib/python3.10/site-packages:/BA/workspace/build/joint_info:/BA/workspace/install/joint_info/lib/python3.10/site-packages:/BA/workspace/build/joint_control:/BA/workspace/install/joint_control/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.816847] (-) TimerEvent: {} +[0.918814] (-) TimerEvent: {} +[1.010150] (osc_ros2) StdoutLine: {'line': b'running develop\n'} +[1.019763] (-) TimerEvent: {} +[1.102223] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'} +[1.103083] (osc_ros2) StdoutLine: {'line': b'writing osc_ros2.egg-info/PKG-INFO\n'} +[1.103439] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to osc_ros2.egg-info/dependency_links.txt\n'} +[1.103902] (osc_ros2) StdoutLine: {'line': b'writing entry points to osc_ros2.egg-info/entry_points.txt\n'} +[1.104486] (osc_ros2) StdoutLine: {'line': b'writing requirements to osc_ros2.egg-info/requires.txt\n'} +[1.104964] (osc_ros2) StdoutLine: {'line': b'writing top-level names to osc_ros2.egg-info/top_level.txt\n'} +[1.107418] (osc_ros2) StdoutLine: {'line': b"reading manifest file 'osc_ros2.egg-info/SOURCES.txt'\n"} +[1.108798] (osc_ros2) StdoutLine: {'line': b"writing manifest file 'osc_ros2.egg-info/SOURCES.txt'\n"} +[1.110206] (osc_ros2) StdoutLine: {'line': b'running build_ext\n'} +[1.110843] (osc_ros2) StdoutLine: {'line': b'Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .)\n'} +[1.111883] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'} +[1.113374] (osc_ros2) StdoutLine: {'line': b'Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'} +[1.114410] (osc_ros2) StdoutLine: {'line': b'\n'} +[1.114812] (osc_ros2) StdoutLine: {'line': b'Installed /BA/workspace/build/osc_ros2\n'} +[1.115029] (osc_ros2) StdoutLine: {'line': b'running symlink_data\n'} +[1.120758] (-) TimerEvent: {} +[1.133626] (osc_ros2) CommandEnded: {'returncode': 0} +[1.159663] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0} +[1.161402] (-) EventReactorShutdown: {} diff --git a/workspace/log/build_2025-05-08_11-12-16/logger_all.log b/workspace/log/build_2025-05-08_11-12-16/logger_all.log new file mode 100644 index 0000000..b459fc9 --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/logger_all.log @@ -0,0 +1,174 @@ +[0.138s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'osc_ros2', '--symlink-install'] +[0.139s] 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=True, 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=['osc_ros2'], 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.309s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.309s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta' +[0.310s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta' +[0.310s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.310s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.310s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.310s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.310s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.310s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/workspace' +[0.310s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.311s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.319s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.319s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.319s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.319s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.319s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.320s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg' +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta'] +[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg' +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta'] +[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta' +[0.324s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros'] +[0.324s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros' +[0.329s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_control' with type 'ros.ament_python' and name 'joint_control' +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install'] +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore' +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install' +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg'] +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg' +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta'] +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta' +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros'] +[0.329s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros' +[0.330s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_info' with type 'ros.ament_python' and name 'joint_info' +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install'] +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore' +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install' +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg'] +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg' +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta'] +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta' +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros'] +[0.331s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros' +[0.332s] DEBUG:colcon.colcon_core.package_identification:Package 'src/mock_robot' with type 'ros.ament_python' and name 'mock_robot' +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install'] +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore' +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install' +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg'] +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg' +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta'] +[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta' +[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros'] +[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros' +[0.333s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2' +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install'] +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore' +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install' +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg'] +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg' +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta'] +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta' +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros'] +[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros' +[0.335s] DEBUG:colcon.colcon_core.package_identification:Package 'src/painting_robot_control' with type 'ros.ament_python' and name 'painting_robot_control' +[0.335s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.335s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.335s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.335s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.335s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.354s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_control' in 'src/joint_control' +[0.354s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_info' in 'src/joint_info' +[0.354s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'mock_robot' in 'src/mock_robot' +[0.354s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'painting_robot_control' in 'src/painting_robot_control' +[0.354s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None' +[0.354s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None' +[0.355s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False' +[0.355s] 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': True, 'test_result_base': None} +[0.355s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.358s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.359s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/BA/workspace/src/osc_ros2' with build type 'ament_python' +[0.359s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path') +[0.363s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[0.363s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1' +[0.365s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv' +[0.366s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh' +[0.367s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[0.367s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment +[0.658s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/BA/workspace/src/osc_ros2' +[0.659s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell +[0.659s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment +[1.111s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data +[1.492s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath_develop') +[1.492s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.ps1' +[1.492s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data +[1.494s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.dsv' +[1.494s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.sh' +[1.505s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake module files +[1.506s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake config files +[1.509s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib' +[1.509s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' +[1.509s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc' +[1.509s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/python3.10/site-packages' +[1.510s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath') +[1.510s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1' +[1.511s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv' +[1.511s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh' +[1.512s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin' +[1.512s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2) +[1.512s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.ps1' +[1.514s] INFO:colcon.colcon_core.shell:Creating package descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/package.dsv' +[1.514s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.sh' +[1.515s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.bash' +[1.516s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.zsh' +[1.517s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/BA/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2) +[1.518s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.518s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.518s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.518s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.527s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[1.527s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.528s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.528s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.529s] 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.529s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.529s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.ps1' +[1.530s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_ps1.py' +[1.532s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.ps1' +[1.534s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.sh' +[1.535s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_sh.py' +[1.535s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.sh' +[1.537s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.bash' +[1.538s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.bash' +[1.539s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.zsh' +[1.540s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.zsh' diff --git a/workspace/log/build_2025-05-08_11-12-16/osc_ros2/command.log b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/command.log new file mode 100644 index 0000000..a3b47cf --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/command.log @@ -0,0 +1,2 @@ +Invoking command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data +Invoked command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data diff --git a/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stderr.log b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stderr.log new file mode 100644 index 0000000..e69de29 diff --git a/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout.log b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout.log new file mode 100644 index 0000000..f0eac70 --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout.log @@ -0,0 +1,16 @@ +running develop +running egg_info +writing osc_ros2.egg-info/PKG-INFO +writing dependency_links to osc_ros2.egg-info/dependency_links.txt +writing entry points to osc_ros2.egg-info/entry_points.txt +writing requirements to osc_ros2.egg-info/requires.txt +writing top-level names to osc_ros2.egg-info/top_level.txt +reading manifest file 'osc_ros2.egg-info/SOURCES.txt' +writing manifest file 'osc_ros2.egg-info/SOURCES.txt' +running build_ext +Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .) +Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 +Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2 + +Installed /BA/workspace/build/osc_ros2 +running symlink_data diff --git a/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout_stderr.log b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout_stderr.log new file mode 100644 index 0000000..f0eac70 --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout_stderr.log @@ -0,0 +1,16 @@ +running develop +running egg_info +writing osc_ros2.egg-info/PKG-INFO +writing dependency_links to osc_ros2.egg-info/dependency_links.txt +writing entry points to osc_ros2.egg-info/entry_points.txt +writing requirements to osc_ros2.egg-info/requires.txt +writing top-level names to osc_ros2.egg-info/top_level.txt +reading manifest file 'osc_ros2.egg-info/SOURCES.txt' +writing manifest file 'osc_ros2.egg-info/SOURCES.txt' +running build_ext +Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .) +Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 +Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2 + +Installed /BA/workspace/build/osc_ros2 +running symlink_data diff --git a/workspace/log/build_2025-05-08_11-12-16/osc_ros2/streams.log b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/streams.log new file mode 100644 index 0000000..5be999d --- /dev/null +++ b/workspace/log/build_2025-05-08_11-12-16/osc_ros2/streams.log @@ -0,0 +1,18 @@ +[0.750s] Invoking command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data +[1.010s] running develop +[1.101s] running egg_info +[1.101s] writing osc_ros2.egg-info/PKG-INFO +[1.102s] writing dependency_links to osc_ros2.egg-info/dependency_links.txt +[1.102s] writing entry points to osc_ros2.egg-info/entry_points.txt +[1.103s] writing requirements to osc_ros2.egg-info/requires.txt +[1.103s] writing top-level names to osc_ros2.egg-info/top_level.txt +[1.106s] reading manifest file 'osc_ros2.egg-info/SOURCES.txt' +[1.107s] writing manifest file 'osc_ros2.egg-info/SOURCES.txt' +[1.109s] running build_ext +[1.109s] Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .) +[1.110s] Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2 +[1.112s] Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2 +[1.113s] +[1.113s] Installed /BA/workspace/build/osc_ros2 +[1.113s] running symlink_data +[1.132s] Invoked command in '/BA/workspace/build/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 develop --editable --build-directory /BA/workspace/build/osc_ros2/build --no-deps symlink_data diff --git a/workspace/log/latest_build b/workspace/log/latest_build index 6dc464b..7e144df 120000 --- a/workspace/log/latest_build +++ b/workspace/log/latest_build @@ -1 +1 @@ -build_2025-05-03_10-02-17 \ No newline at end of file +build_2025-05-08_11-12-16 \ No newline at end of file diff --git a/workspace/src/joint_angles.py b/workspace/src/joint_angles.py index 2e66b6f..bfed20b 100644 --- a/workspace/src/joint_angles.py +++ b/workspace/src/joint_angles.py @@ -12,7 +12,7 @@ def main(): #osc_udp_client("192.168.1.24", 8000, "osc_client") osc_udp_client("127.0.0.1", 8000, "osc_client") # Example joint positions to send - joint_positions1 = [0.4,0.4, 0, 0.0, 0.0, 0.0] + joint_positions1 = [0.4,0.3, 0.5, 0.0, 0.0, 0.0]#, 6.0] joint_positions2 = [0.4,-0.4, 0.6, 0.0,0.0, 0.0] joint_positions3 = [-0.4,-0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0] joint_positions4 = [-0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0] diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc index 85c71a9bab19a133946b3daa35b4017c53625ba4..a3475762f38e335a630aafd4fdb201ef4db02603 100644 GIT binary patch delta 8141 zcmb_hd2pM@b^rGJ01yBP5IjYSA_bC?D3BBn9h60hl4Xh1eNwj%7~(<{C=#R=A4L)V zV7Q?=_DpM=$y#n3TP|tGZ8Gs$`b_IIZBr*bB4;{@ox+ZrOk7XXq)Eop$vDnL?(gjf zf}|xU`J>`szukB3+qdt%eY^U_i|qZsXZ~TI&#U3zU6+@f-SO!2{s$fGFXUs6t~B>u z(oT7}pBLim<$-Aa8J!pL;!BQG1-yip0_x+-csXBmiJkKE3SJ3FAz#d^@D1=Kyc*vk zUc;B-Tg-#J7T*%cDTSOmUJp5C_i7I9RC%u!TAnFB(7P>s_(1RKa4c>{#TmKVxhGu? z#Yj5?zr>_R(@bqbn_`?zGp2d8f?32lCnk9bquP0FkErn+n?1IapXE~PAe6?*VZ7GTw71IwpY!07@Iw;^k-PQZ(;!bGR zXfv{qO<4%GBMa%elBd=W6*kXf-Et+H?@~p;+Q@SYL<4B z77qYt8~z9Ar*ZvYN3$dL}=lkr9N|jor z6l_`Ny%Il~9oy6V)T8xlDK<~yep^nOAE1t!Y*ysKtjIE}Oo=?CG<`kpwaTFJRf|on zw94pso;1$CkQ1r%!xqEV_2%4np-S(og&v{V!1pBhj-sEY!QBWn1Q}vBWHwIF8P#OD zamXB-FjU`#lkK?bH}exXG)}7iDt-#~Xfay{+W4*a6Xhy?8zAiqAh!dudI97PK-SDc z(uyq%(Ars$g4_wnx_OYz%36lqhkSPAzjpR zJ(4EEyx&21##}#j6Mh?}wP`Xjx4gfN1BFQ^Qtw9Iujq8zsvK;Aa?@_ytW78Ho0o!(u#`I6FK#$b+Y&!9*08mGaw#m8?O2)wkBw5b6wO zH2F8aj^iJ()@0YN_=U(|jL(zZ65Ja6By3hFXVx}T-tCi!vk z`A?_$+$>Gw%r{DQvY9~H<1F2_RYZg1!=u5(XtpyJAu|kP(fGh#gTrI6++b`drohR@ zpos9S-C~L0U^I~cT?Caah) ze`{hS8s*_cA9gq1KU1`LH`}{e_{orkXfn=Nd}P$jI1{FbjbzH^oF}ZjEKFXO@v8k! z(0+?5;_`G~?2C@jJn~G{n^hSj9v#aVkdZOqf*J3z$lz$Sn;b$$mvpoB^2y=r6Zs-hkUALgYIJTKWh$#{LBp! z{hG_rSpZBr{wma8o9>2)4!whQfW{^7Sh_sz_@Q3w1r`12#QCP)OkAZH(PVDD6H_f_ zHHz1z7X!Wm_yE3M&?}w~^?L#y7yERan{_I!{E(=B?0CzuQa99mPUg6(uaR#p-Qxs% zF_ZPd6KT5M(KT-eeu?MMBs)!wl0Ij+05n;Q>nKz8bDAhOnLV0d`N%q9J)SoQp)pb! zRhABpJJow2TzM8_NB3|rxlKlv2h$6= zg54KKkOM*8-W)tVY6kleuo>2pT(rQ$W?ir|xIg3;eKga0YEDu!PE8U`#xWKVaj}oO z&NC4+G8i(HS7sa$(Vq}0AmrCWfoc<-o1kifF5S9bfSoI5etiiG!ue!jN0o2E>@w0& zChrO^)5RoZL8ky$kNiq-r%VUkMM5P+mGLTo5RzyLN6_7cDa1qQBtw$bmB@ct=2xPw z1-}_k(V;YdAjjlPpu1Q8y!KYt_rRLGI&-jYk}c!do-l5e+~~y{g6SRqN%k5)yKVWr zSYE5IM{-$B8zXy`?R3p0Z6_t|6Y{BL1A)sll~QsJT}bFtr0K)xBzxuC%PZv{H3lIi zAV0hOW~aCxaQU6(e_$L*RuXn!sKDL=73)h#W^N%FPc%L{6crI1foh3SLLZ~%6g9Lr z;z2Yamz^&r<=0kJ2RZ@CxMx$MNU6hS2*4w>OKYXOArvTZ6d+NQ;)mpLE!mhg=^`0x zSe#z0(}M{|9ML&GKsQWB^82Q)B1=bP)pCP7E@`Gg5V`{2=1S=)BjrdrQ?67VUOn9L zm05b{Ij7jb@xU;j{tkq~9DT}-8$ashDTj)(be_$MFmqFwsVGB%099PG_fB!b#Dj`G zw)6=-g#gCLYu`#mzv-SG=dhj}?*+xH#+CtI3VBbC_Yc#GSpjlrZ;tsP#jM7b1I+Yz zzwu7tt@bZCrHs#IcC^EJmuR~PzWs@Cp z6!3B=R8nbgMX8cb#aY>YaUL%8W0!oat)fhwNjC}rRn%Nm0U+NEoxWj^D4M%$nxV#L z0~8V}*V$RGOEx#%tHPtbsZw5T>J1mFVFp$yYXfmHm?(>+E;G>C$ldOYV{HcfPUobwrl`K8nyho>!?&BU(-0eYicTmzSXY z&c;LRo|zS!b}@EXj&!XpK=k6V(8?dE3NMl0?ke+Cg{v(#P&2hu{;X?9Z;<+^F5K#^ z=~+lGM-VOIB@`w!3jz+0#k@e(40XWO&J!)i-sW zF-vmBwDQSdiqpS!P5N8re8U=|wJD#2k;A+Wsmb*@)o)8bo>YY30WA5F#I6tZ|m@?U%3-7P*tW5)~3j1yN zCVoKp!|+y=B%@k_o(@=o&NZfL0u=Z-^$nCI)tvrY1dk|F`ZV5qK0)CGMN5tX1^smt zE>LVCinJ1fBk($sJdhmB331L**ptECT;s!VHn_LC2;{-p-1zSyJh03=WLb*cjXMf1 z0;;Dj0-ogNZP;A8lSk#pyE=S!&i@lEzDGPJpWeM?y=UHm_OE~oxU?pUslg-mf_w{X zNViWqb+WCu067b@Cfd-^^#Ja7eq03Up6AE!Mqk%&{EnFi_iSUHW#Gv$uee#hy?5`1 zTWO?=XeO_3cxWUx7{wiU^t7lU6?PNO6^-`|^JvD!qx8+*8^p8n&VB9dGxBr$ev?i* zn`4PYqW|)nEB-6LrD(I*4#xcUkJMk=%yAISGGSL-+ z8oG7}x<-g0YR*&Rq=t?vp>}}6^po2_rWQ&aoJE>AB%e93V%bV`-5TA|=r!ES+@tb?1DD%O-++G#RUgv7gS)xs68>juD!{$ds?bh3@n4AQ`7fp+ z3j-rxJ~&kA7$`+GSVoVeRF0?e%`z(=+1nwz4prp?%gb>IeiGxpL!-1z3X#{x&QNjM*m*jBDRna^2tzr#wq#+N5(UbSUhHmU4$SP#BmrozTuc5%+ncH z_u=8w!)C^D^zincOx~GToKq5|bz~egvTwhe!A6ceWM|9UNeaC!WITzHSUfx&?~B@b z{CW9{<7TVM=IYL-YT;J$zCR|OHj?XyzYh1s%<-^#e-lTfJkh`&lYf0;g;Bst<$EXY zWP4@z$=c%mMD7lUha#9H9I6%V^5V(+SX{DGtxMMv(uq6Z$hf#ly5uKDB4#WyI26H~ z)c56qQ)Qt}nps87^ECY$8ZM>g0u5;eqK=yV^8QmztV6zfs?`ez)ZMpwnd9UC10`7h A(EtDd delta 6540 zcmb6-TX0*)b$9Q*xOfr(Nq`a`Ai;-3LVSRvD2aMn5=BXrMCvW-4TD^W0v`amxHL`V zBCtaxPU<>lx_0W=k%x4rZ7jzg%dDG8Q@d`aoo2>KowTt!!PL#8(>k5TjXiDr;Usp? z*$aZC;$$Y|;N0Ew+TF8f&pEsQ`Azcf%fy{5Dso8hXZxu9`lc`3c0a0;FY>3=-B)Aw zYcegO?i=bw2lY_z4e6qj25AW`y+JOzsE_&qQ$)+?I{3QjdRh)&53Qh;@b%Iv8h~#x zb;U|RPc^LpJ*5vxs&vtJS_;+jKFj5-55~2_Pk~?RU8^MN(wwwFC|M*#GNc7rSEx*t zMa7UJD)cS7M&UbZ1sr@MHi5HK3*a17GjS9oBTj<=bQv5JKra>aGU@}G zW@v&&sXwD7RDGSHA$Cwo6seNFe)c{h;Xg=~q0|Lws%It(i@$52bz&f^VHL_g2LoAG z4YVFq+6?{mVH;yUuBxUwt;5Gf~A(`G%I~3++wVhHo>7S3M=&l=J--9Bfs;71b~IsXOB|oS^O%FnAl-Z+TN( zhAY1*w-Tk$4vP)?OVHnrdTB>Cc@!+*mYCse^D^=2p+boLMu>cRVVPYh*75gS@=$um}b^=o+6`2%u{AB=s~gL zE<6PXq$|&6+~7`+!D>CDwF`CNXdY0sC2y6jfOi@0a_K3FZZmms3-XxmJO(x)kJ*0H zDkRJ34pTBKM0eg)OrvGIV8C6gl&(;U4KMipRihZ8UT;7=09ZUnU>_0Vqq|Y{pfoQ} zSp;v~fEtKI_R_1UAeoHcX!VN@2t9S=lg{ zng%~5;R1W(w#ixc7YS{7XXeS&&l9sag?S~_1-s;j~>8=7;IT(Z(e1oQ7Tj(6e?^9hfxZuyaril zFiJ5$z*^ofs4Sz03<7>|xUiY4r=+kZvCuvo4dmVixg&EVD>|=%%YnOr?{3gH&S8vh zG^`M0kIpHgZGp&sOthP5zhS2Xq8*~g!5*9R=Ft{<;t}Kw(UX8_U4uCVn6@>T(|`%* zFPEHSv$5Yp1EZxPw?djs&dxf0g?|J`K&Phw(DgHnv5AtvMp7nM@ z>A*H4*n(gif6!m**a6K?VRt8g+CTN%dFGaRHZ5H%J50#Ir5DycLzem~>J^?1>|XkI zppQ7anH$+W0C=?KPBE*5R!&LjEN6))CZ~12FIZ}UZhVrT3igt%{E6TLyuJ1$f1>t6 zg^&4Bx)eb_g5wAV0C0C*lx*jdb@lv9b*;{?5=n9qRh5a%->KWqo9a&x7oV*UI90M$ z-b%I-8&UaU{f0C0kL6kiG5=-2%6H@zFZ4?Y$PyQ^$va2|{8!61q7hB@0`vmk4%9&0 zz#)I6{PJTGl*sLZKRHj-lNajpWCV{06MkDqKj%-v0Y zS1cC70F_f`qAbBagQmAk0Ad~gpy`tKPq3i$>r0nH^Q3xEqRMHg{dD{6Pl=SV5Gh>@ z&ZK}dIcPEWG}kL7=}x}0sg4geAJ$g3(t=xA&tGXC_5K00%*m{fJxJaF1F?spkv`4; zr=^^Kts@9py!@G#y-JR>@wZzZl+)!%IuUZRtEfh7M-{ek0qk=Ca`sqadLqW6dd%FT zdy#TNPvNl;l-a}B2%-dv%POpRp?H!JgN4&&EhA@?jGD1zw2YNXQ1vzC z`PDHC>!Oym{8z23`7<^s?!0+ht5h>3;izY=Hx)dKur+Y-U?LW3Gm9g3TGy8QhCC-{ zAg=?}o~I7!*5!VIYAeW230cuM6p)3YdlYKyyzB!)R`jz4vb`Yus1O!?Lj_^1_3Z`W zr-iWS=c!;LlpRx2FG6)skW;i(XgjdYzFSzi(~x1+E<*tbH-e`wNz`qM3$zdxBq0hd z&&?vV5B0(&LV+s;CnRS*E&7B1l&1gB6>f7c!FI<4ObALfR(4I%Y2PME9l61yr>X zWfrmfOaUi?4xaD{A%XaV{H^YFWEcNM_rDPl>qUUb@Sknp>HV{UpwPII3O(vHm^%OkA+fZ%~q(q%lm-OpQhVorK^XGr@a9QqjltDQ~cIRE#Z;lMB; za$4-#wItKo=aDLGDWc6h5AXVFJw^={K)?}*DT}3xRUZKy{d z{m#$h96k%UbR&P@$a?&Q9&zk6RLU*Iq88?n8wNz%NMzQk+y zzd`Qs_x1-!JNNXQB=bwro<2f`_;UxkTA=!>G>Eum*~ATD5>8!Up?VRn1d$q`+6)I& zCC<@cq?VJzetR%dhx`lm+}?Z0vsU(Rc%;`KZh+(A!l1IS0S>`N_%(tyGTIagf$mKU z%|`x)KMJ8F(!l?ux1RhTFYoD96jsKgJyj}H)r~xSu$AoL*&aWCt!Evg&hbYMhDn0I z0`N2Z9e_{sz@b)hn;$(?+xrdBttN<}Ws39`xIsf?FyK0Gu4PVv`$X|&CF9O`pwMqM z9D1AKHazR`YU-_&Alf*fGW-_5eaLTl8;aq#`0Izt1L1(U+#0TJ;+t{ZZH>}`H4G;n zbs-t(ZTDegjbRmVUNBDo?`a&NMUM50 z3Mgm4HWp9BBGK`25r|g8Pfm#@W*OU$Qka~X2wN)3+m{nMN+KWZpgS{OSX1J6>iF%b*eu zEI+Z^&vzXUuP|)WIt7uxaC{H%K2wza6|kl61(%g5nU)y5#ZV%0Gv;S34mRrYf_M_6 zs;=sm1K#U~V)Gj{)Uw>!WFzm;huy zi5=xt=QdW?LDMFw#3f5v+mubQ*=zxKlWmhju{GGz{Mxz4+wmp%2E1>AqdFEv;hj-+ z_o!z9o*Wk8?N_%O7QAxDC3x!vF5WyiQEnOaL8a)&M+Yp`Gfv%aI3b}}`0c?;Cs1kG zn8f}R`u{RG4cE8+^Py(da4iJz$k?F*&1mqm;1eFak=gORlt@z+y&CFG4Wt&&e@Hr( z7A|Zdwt6sJNM_yqt&0_+=rQ=OE^esX1n9J_EgTk3FU${n6rYIe1MCGfz}}^vOO>Rg zaC-_7bMscpqsK4^JO+8x5~^h_{AZERk~p8fH+-cNi59pI zPR^Q1&6%2v>hb9KL{z`d?%?b^4A&GNPU*?y_)zkGwiRb00)+?Vwb=~x5W_o64XJ`M5e*^q123*VnAPJI7@BPO6-W%c+V~PAX-)IAv zl8P!R29k)JdtY}?&tR4WLq%mQ=5_b`-gn=9_uY5zeXr5qpN`?*sbAiH@t3|5i~Tiy zME~jdcpN|bdkCzUrPN|`^i*rgk~*g<^sd#kC4Ej`iqFOQTdyUSl5;Mo?+kzMwEC^=>#@0A71J8Hq>AOr zuB&L@Qcm+PpL-8Ijm_p2PoHgAmAvW=JkzMwoin1eSSdRV(ey^TB2H2|3Zyyyv{P~_ z_|l$hEa0K+Ly&6N<)w;qv0>q(A0HQ1t2Jx6BJ*FKa9LE4p=hsOu*;&l>{J_d5l3ckBvQUuF4bxm zO65ylCYsUfYqxB_i*d0lwRt>K#bUWuvh8B=-Pnysrk^-EeYqhn*~_JJWt#Cj4MZ13 z!#;HDOL3I|TUiBAqOjZ)1At|i@RCE2QFKMcABBk@rxWhhXQ=B@#1_J?X-oD(Sw*Sf zAHCD>N&M^y1P%!6Dp2ewEoD)?ssO1PBUPPs_jAoAA=NfTrCh1LQnAc=(O5D$g^9^T zqFq|3%;a@1zAONWr_%^{I+YUycU~2vc!;eOP@^)~LT`fzcHn2zWHywX;@*90e-?Rg zc^JcCz(`^UdBBTax>#IlSgW-PhBH$n)YW7P%oqW~5E0KA zB8Z3_4d8n$EhujDx$xUa4d#NS%xOFyR7=Br(0L}r=HgbuN@8LsEW=9SnY7YYAD)Jl zvHI~$Sy^iU&$N}R^kM!CT0@vW8Ee=YL2SQur!{7czpl(>tqE%jVg{_O);2tI)^=+L zo`cp-YZsnFmTB$Aa~SoEpq@R}9jIrN=iwNp%3a>j3#Xqfo_gW*A&@`lYaRsYK19sK zbQIID5MUxS3IssRyV*D?ZYz%&*ZlZiJ=60b>^Cb93I3!H*9KN0+;%d*~RLtGw z04Z0Mb!~&@E@m-ivkoq|#fsDSi?RA%idzS=HWaZN-=+h!5qaW$iym{5&Bv738aTPz zj})U7e>?ULh?de{0{32vzZ$QDGtl$0ayhn=1m`fE6t|Sx(9lX6F?~+P!gI0TN-;0V za@>H`hq#=Tk#Wpl23s^ChFah}#o-q5oskyR0^W~sjKgu_+17xSqe1%$^<=^tM!3Zq zLAVwDz6-F95|qw1U|{SjCfr)0mEgX=WsR>TkUqibs52G{k+Xvc`Ovja7zS}9KZGi&Es zirYz1Y}gJsW>-7k>C zwCkE^reeU!9!VAJJ2zr$eXTz0j!WuwRg9xg-vjC2^*ZIb8+mqZ#E!+7NtxE9AHTOF zez%qPz+&6b;bT1e*>D1Z@?kHMFoAFB3NIHT~}62Dr@cE60>5f z%5_D&#^0%~Z=?tDm*QySoA}xtDfKpgw@b-yS!%b1_^{NTNU1+SsXdhHw=KVADUPR7 z;tv3Cp$#v-rs)gM0|ZkrPw;(^pon}QqWXhxOI^#fGN|t+Fn%vEh`Gw6(%ce=y4mchs;ti$w0HQ0ZNP!%+*heAqFo5LX`JU}^Liq$bsf#Kct zZ5Vf|RTWbf`no`K&J5pnhi`l8G+($KlpALHc`yQLwv}Z_HBoc!Ou{-!FoF8im1j8b_gjOlTma1_ z#EZY?zCY0#1hh}K)9`idop=Nz!>wU|M*SPvQjqsZte!)f#QJy9=5cB>f?svzcj`uW z8NjTGL#?5p7SL@BG5Z*rr&>d3|7oPPvD6zHO>B`p4)R8z(Zn5q=2N#o^L`I#ax^MM z;;|nW0EJow$dIl6mIMamaz}3C=CP8prv;QY> zk+7bnvUolxpOoguNP1JP=jAA^jkHE+micu1PW`O^EnE3PDz*??QzA9a`t`g(Pi`xy z@uXklXls;f{4~S)NA+}T6!3nhrL6C5jgm}#U0eA^Sfgs4YAG1E(;?+{LiC@xpJ%8w zl=~{mo%P>e#SFvj!d%Te`)IG?+|x3!u6*XI#_tKNWiRskl=WGx6EE@merpccC^*xL z2dtOBNICag=MZzSBj!9}4t2zQ4l(z3#1s*8Uq{U65p#beCRkf1F&9h32>|P1C&92L ztP2v8{#UtW01H+t1%9Y^q;%%1v=W>z`4Rc%od?z*#P6Yv*aqR(YNbD{1mutz@Y{#2 z1P*sXON0Wr2NxDJz<9 z2-B*}msV@ej5#^I83g4<-4Tsits?x5TN}E`{EX?v-247t%HB|>-A7K;Un$k9R-``# z^QhS!7xV&K4dIxVtIkD$J6UjtkJinK5a>puTwWETV$GPMAEK_vAaIk@*vK_jo%bw! zdGTY%pE&!pr=OU8>IF}K@#v{pFMj;gsTWRp@u!ZSIr>~a{vJum?~PEflY+ez976D4 ze(lTj?_WOllxy9y|NsA=>qJJnBo-=;dEz7q`EsC}jaBn<38RUj$NsNcc?ARkqNp_% z%#zJJf#>D!&o=i0Ih&99!9x!e4&d(~4Wye2KV578&)}c?!Y$ogZeW0kE`$E~+t`oj z$o&Tn+>ahqzleRUI~@>JwZ6RS6wH&g3N}e5$RCrZ^lm7{!R%50iw-*g`{_co{N#7>lEhX#t&BBX>kjo%MV z1PkDu=QC?ij99Ttn#edYTIuT0N&TX_qMnYut9r_nS?GG=^R-6F!KYd{;3?IJR)G`> zUit{>Lx6zgN3T=e)Q}14xpgBRDsBo#Y7MYy`%%0EW!+P4MAsUZiNw&K)nzENWJhhL z=PTx3)-vt4D^TGGCy#zDFVd308-pX!C?v>q@YW6^DyFmk>Zaa zxJ~GqKU3&_>0?6YejMoDqj7X!_%I5sTq%{ETGMncH;5aoLXASS4>kctdZAXTU&`}{ zR7zM?nOF;gw3P1CU9~>P<1NW~Y1MXUnXS?K>Rc?eStBKDQH<{ixrU7Gx`h z8SBC9UZ+71D4}=*ug*BkOA7ss=sIHC0_1n?ja_+r@p7)rOA|3u2q3iFH7%` z&;4&VNm4idWG1^JUa1H-=kKpV)Z}5Xg7|NjFIJW+?qFB)^u8OpX{WqgEH^Mv)jCLqfg?+frp^wNfub zO!$;bobXp3+5gDj!2j;VX-Kv-QRImE!Z}FjhBDJmq2r}b%srENnAEjM%lzUy2y$&sMf~;nOtY%(@DKIdYj{8fSuX6~g zj*Ww!U)d`ah`d#&3G*#6XD?N5*iLb&WMA^)ER%RipIW>e_L;9#8>@CPh%u-=t6Fxv z{tjC6G%UuR2BdfzStiq;At~%Q?c}{UuhGERa;@rk#+9I9BLu;4gE*~LsRImJyF7gb z5PEvK0n6+I)Y(J{i%s3~jJoWg?e)+OGn18%$x`qi8vOt7Bf+n{2n!I7T z(GI+Z(dtFlOW2L_r3z3F^#N8M=R%Vf9Iwy+ZHRxhBN+2is0mjL$TL zn{Y^>ZSs-`=Br}K>-XE2#3yE`9gW7hkEui!^7>dN@jrADou~!~*dV;LL@f1IurZl; zOKef533?}f_6HE|Gm5IJN>)iL3E1$-nx9d#%2vgAO}(l9Z$0N*_J5)$+E#u&*?vzL z36u=}s4n?u{3wx3D5|C^25L1>ufcx_#Zajoicvqfy>u!m%Vqs?3Dlbd%+%Hd{~b^% z&tdhja@fzELyjC;N%%R_ethEnY$^)~)SLQWsk_sth5iUm{ZG{?s&xck45K;&Ol(zl za@`4a2=R9#J%=aN#%11DU(2Nu%I0Z?GDCF($K2+h>3dbpZ|^(my+%?+?FQGEK^__l zYF%PWVvk}H$f4gbk|UfB9R7D>NdHgi9SJqVe?!_3V32jdVSC2Hz_6YeI3i zeaNoAV>eFRm(+w$6XARvfjc7=5S9Xy`8`lds4xktmtta!d^hOps?rCQMNzFM+cESF zwKAFU;5q|kuu7&rQWC!iXQ$K1&uMW>m$gGl9e+n_r|U3aG+%_#9V%pZ(9fN;60Jx* zFQKM2^;MPXlW$~v&l~Q6Pci#~AXmpkq~$W|JivPyW-F#I8Wf~XmAAm7r%7lCQh1w3 zE9jGfoDP}%f|MB{xL~7L_XYaS3eDqJmN7d~(ZMP6!m48~U=!jg1$VqBKbtXU^M)t` z+sxXWqkROubirjYIRc0B2cYT)|QaE*kBoOx$O0ptK zSKNDgG{4y@u_*+CH9@R!VGR6okA{~KcLEKIxA5ZTH*Gdp1cFuOmPToA@VwZL7tl1s z5(^4BP^SVqSEbV!#pE^jfsdxaaN%ovyg<<~7e+^f*#IOE3S>A|mB8_+QiJNd@k-Ug z&enXgRHq%MTvpAnl4#wtpc`J#n6&;jIw5-D%SOU;L^}pv65!%l{CesQ!&Z8L+JQZi z&d6@T*!y$T?S4njdxWt?s5ik|#@Uw-E*GTkBzFr-TTsTq0qT!+=C$38~kUZkClIq?=8h=OuI#~kli?0o5*x~8q^ zs^u{+5mqjzG~GxcwxxzfuZo*Ztac z@rQnHKc=hpflX`o^G9p9atlgJ9S3Yh!vdEr!U@MhkHLM@&F}jzPXYJsvxcPOlf(?T zuSO0}lwZCv_Z@Z*eKh6sG&h6!26rw{MEI{mAwGg=R){BHh-O8&RDw(9@}WB7&!ST1 zH6tA+`sf!GvE@g+cc8xYL_BJnH8>zB2c>r6N5+AEDa zV(xu;<-nA=f;DI5(3Dvy6wDPjaXwEwc6V&bv{s>*hw|fmzP%`kFB0s8e^+iQLs>Y) z$c%KmG$-A@$#Bq2cMGXXV0|S0$%V!$j4TE7tWB$}yKA%VJ;O?)NpGMl!x?u=S4(2 z`d&Z3ekm^Q0}JiQ|2YU$#MET|H3R;6@N@CSxP&YT29gX#HLU%BBW@YePfVP462W&% zy{fK32t=yyMJ6sd@Qjq-VX6_9UE#9)-O^BJNS`Zd4CW%)ZyO@(EDfy?rDOGjB^ z%BisIuX9=c_EWLQP}xhG{dIU&DwLDob-btPUHnPd?>-Vb!L~Jo0mJ6f9Lr@ng-haX z;GKSAkyegaGZ=Wh1#gs=v0#9~rkTz5fz1MQV6&O#Przo=5h1r9Y?iz=d|?@E7U{31 zDZhLpHXFh=%q<};KQj9st+E|76J@2h%cL%cOsW^t++=^>i)p@>{AqyWhTy!rN!uUI zGtWGE5{BSVhgfLy%)?-rWTTxj-6_abwc3=qTy@G95mmY@8Rr8ZmgS>(s@zyS0z|l{ zHUJ_|xL2qkF>YEI5>Q`c6xt+blslEJVBT3Z z2L^rX?@nymSqX9zn|1y{eR5O&5;ygmV{u~cazmBXASU->@0(zj4<>XRlOSn__&jYi z><=(|;WJuv_&kcGIyj3$ocOA;ploPss-u%wdKDXF;;Hsl9Hap6EykUsWAMAOn7XQ7 zRo;y8!2miM0}iwp_dS`@k$ioTe2K1nB(C<0e@fvx399K0WpRLcbNIcUgFh=a@;En> zgm=6~+!P`BC_)m8b&kOI7G z#N~(|Q{5Z~@f~r492b;LbKDTe1!emXH|UVZt|*Y-$@ zJN#S;KUWU9B=osfe=FM>SkSxR9;MNzE!6o(u|*kN8{&F*l7}pM&`;2}27R1K_;*Fp z`oc5`wHZkp3DXEx35A3~>x2Ow(z~$6?cp_U44(L}VwHmrHmh>Rv00w5cDIH*R=GWE z!>bWA4 zF!{si=HePgbxSO|xgdQ4^cfG~A?TZ=qb|nerRqdUa$|K~Dl2 z8Zde-o|^(K{vNf|*7kgjKHm-FLO6aafJ2bL=q9J;`5#OAkyUA^+rTz`1&Et>Z;fyE zh;w0k>tb$jNvze}v0I{fS#?Kli3jsWB~tijyi5n!y`fBb%KR;eAl`TyzaQoh@YYZ# zU#aeq+thXF8$b+i3v;oqh67X3rcH`}fF>ijf3Wj2#61qm#Kb>DqC4Etv&uYtGMrCb z{Wrtv1BOwfWVvRH2L}QI7h~! z2(+h{;E3QUs6i|@y%gGg0vp#_#T%f>dgA%#j~_d6^vv<+UJ@6n;)Lvi_*H6(HH)K-zI*POMZxMIYJwD$ox8}6@ajA znb>x>W|x?CWbP64-U#_zbZ_JT1(EMtNOmWCaNu4g#6jdPlP){>NAA_Mn<7B$zmEXh zLfT*(I1H>Vkj__YsUr}2Ac83^eIz!4!+)foYH1@74EUIxhWvLqileUMbTYPr5EriE zKsD9WN^ps!qb^dKPKWZW_!o7EHHg7CHjU8A;W!@0ru@K|DL8(&rp$=M4_04OUu#+sYJTr<%-*=TBVv9kP)er=c~0! zAstq55)PfP6jy{j6UI^9bx=CY+wd+#2)Btd4cI2kwuPi^?ZvK!G8Q_DJCwZ`2=3-y zc4tL3$9y$a;FvSNTC15$4PN|T#^LY=R88kQt16XovbH!c;P*(5b756a1u?#l4>!qp zFk$I{{}LA@?2z7y99u1%H-_b#BRbm%(AoVwCQlfUHtay*5O7hb6sF8W2kw%UPnn1D zS{AESyCS2xiF_Nqrz*~>sCVI({BH>Xj1-`@bLiM}lW}VHz=)s?=K_BGkC*(X;A&%F zvT&I*`M5ZTG}fkhYMl())jGTHdVN4|8N@=~Hqg9{A{%)a$no>Conz)@I}z`xP6@xV zm#h~#g^J3qdr2pVaw1WT3oqTK%uKrfU0VkeqArKWiN6Ofa9zXZ9Y=~GW{~DyDPjh*Vhb}HF*-Ix@pIan+MDq= zlTKotR9x*8V*?v7#H1{2*2s2nF7c-RX5vjls@WV^4%X9BO|+rCsW>?G62n_#25gl5ZIEDJb|7 zuNzM88a9Hofxw~P!4@_LylG5X$t~Doif==mG<1zrbsPgxJ{gq2;YD+e4)Yq|W@Oh; z7l)zFg6a(Kq+}bRizTeSFUITr9jSB-C`{v{y3%e;$2Z@goa>`FmW6tVbK!f8?aCP_ z-u|f72Nj`eWqwYzjCqs_QZMOnf~Qu-SnWr7zuPk}DfV!{)9QbM%8rLM|1gqU^=sx- zS+k!Tr7hGAIB_I6gruFnv-XLgcI3Y!k{`EGFyx#bsedAX9r+*bnLkoLwxgN#Ev?K3 z9sZrgC~d{oX`7WLrC$mwW&IQ>7PrLK=$I@<2)RCoW7~{ZjCE?|Rc8n2HrtMCWs5tV zzE(euqh%L&6-|1V-xE+=5>0XZZfp|wtl!b%mfEGM#t@2}q=Q8qhKEv$MSb2$Dpq1j?*e+(+mO6<(?!_;^fwgjRU-2H~m|EYD z_0y-?c5cg{gS8P_{d7R9z{K}a?vqoiCYY`_qgEV;-5D9hkDyoNzQ1Sg$e07_kYbVl z`AGgY4T82JG&LU5lqxDc^LN(Yrdd_2Me=_%&8p%{J@ZHEkI<|tz7~#kn`Xa&QR|l+ z1^xS7^cVBGmBNAOf~4FQ9j%>SKLAyi1|`PeIgOMUYs1b#(CQ%^M;@9WIvw5?gQCA5 zM`Qn4ltTTPpo+hQ65`+XqJOAOaGY}o&zjc21N>YY__(uzY7cvNn{$C7ip~a2JQCLN ziDv`M3HM#snk!JXjQFY+;urVhLWq$LRZC`VnIr?XdNY0i%mWlyjj1G?6~X7H4UqqHW9=61=$(L zMU|1*cm?wCR>U$Z2i{G2>dFE94wCz7W!YxsvZr1-C>6|V^Pt!O*q-Vhlu}8!!T&KyAG9G! z#MFfTBa%4A6#XY-y87e9P50r=wjEunl3seB_io}JOKfN0>j^zqUKf8wK;0wn3m(z4Tpp#~-o|BMfSm@i>p0_goC!of8$O%lDG2F~PMgnFgmj`eOX zf0$j1r840xa4LSE0@5Iee@ww&An>whaS76=`3|Hywi2Flifs~}cIxCyQfU4TWlI2^ zrP=~+8*0FfGhPy+)D#xgLJrR2yObwhy;OYedFecwV$DMl%1pL>v@IT?NE=5qbhIaS<-z@PGyBV?!ujpu9=n=~{e)ij$JfOUOe|;_uRjR=QwI-!#^R zuo`4Eq6X>XX;j3?z#?!$ij0GRjqGU$Jsm2HgYd3~r#?FiiGN2mDvO>$Ue!gA6FP+; zIp2V{n`PrhuvMqnm@i7#0`XPK9_NmF{ahs2IC?sLdg}5){0>21Qo2&aW;UYk_zx({ zmn``TgRXh<1DegW$=6{BGT@U*vw16YALMJQlX)nSxM8F+u=Zz_C-BQDA1K@5QJd9a z6f(%C6CR&1CXwgo1b$g%A4-xjN&03|j2?%_r*RB#RC-TXT8HMugcm^uwU7@hdFWEB zR8tQ1kQZvSo?PI)x2bZYt9Knd89n`23YJlxN|NccCuj|A70~e@~f+%WZk)G-81%J{GP1A*0 zX-4=vK-N>@*AaTTZW{4MdLiB&>~grDG4rX2(f41WE&)4HOhAH(iBSr=^sc^#=^Ev1 zn?ZPKZc!YD(>{O~6%&~3y3#>9aFp(TCVrCDeU;^&9u(9CZHKW|LnFkfnQs0BFVa5^ z@{J;WL7^ic-5+WGR5DSJC$t4)9iH%e<|p)i{Dh{DpSU!OpSU24pSa+vuZ3d)K)u2cOvc*v!k1@1Hw&Zh!H7>)gvP zAK3rc`F$^&`==2;zW>$pd2uJY?gkr{Qsi)^@rbBC3OAA z*S_LE-|?Tn>Oa4XHPkLwt6ox6mTR~i4*t+=3c!wr4m&I@W9i2Vv3fz6L=>d1hI$_U z-4>2zK#PQ@?-ecn5vBE&>UOnQfrmGiqfR=(nIjy9uJdsUCm;4ghhfd&9(UNZXkL*pDLqrXM>TPAPQK#FrK;fXLZkm7sVOP-y5 zcJ_rAXWjTz)wb=0Fa7D=|Fy3$_^3Drz-QsULB<*JbqYRFl1xpmHP(b$xijx#D>mj+V zoBuFZ;U1AifB_Y|>8(vwWAw=^Xb;{fk?#^UVsy$T_J!CNj4|jbK~;uf?9AqkeQC|O z6S2@-;&1ENh@f(1{K9`sjrd|pdJBjlN}@FY`VQ5CHcvX`VfJEO;uYzia|$R594Nj> zdPoJ=8BT)l7#8#~_T5g(V$I*mU_FlaBxwOxhMY7}80I{2s|@PXoc_h^`T)o2^4`dn zww|-#_`w=Pl!X%=%2s2rb5II=rLj-nMHdLJ58@(21zMQoI-D->Mwk6%R~6A{rI1Fv zg|6-67-zUe6b{YNk=V*fT)Ou#pvv*}M=1*zli_-%l*Lz9;XX>-UWtB>wDd*nWvx-V zFp8n+b4JPPLGhOq@wPKYU)C7b&bM)~mWdk1DGf0(@l|?*UT_RGOi&H9D)Z%r22b}e2X#Mds;G&tMVw_7G@vf5=RzxZT4PIkW20{yGTdKy`ifQO*mZa4*LVm8~116GI|@-k21Td9cK?+L1kr6 ze`s3oQaEdKhx^3yCHSO81qX16&I91sFJhB{dv8eGz!jk8A@goNkz+TpVOT1h2`^E( zNEcL;%(JJCJ%xQ{Or9lE=_eWG`{ggcXb{X|$C2YKU#RbkD$M`*9f zHDaqsyVyu0n_Qq1)>3f83lFH3O|tg{;8w?W;y1LhvL8pl3X$!+{KhOO!pX#FZ^49> zUuwq}37rrnkfv|=-VXUVEaf=$?itts(SE$*rD49oc6t@3RIz~+ReK4RtTJQ38t`owN!J zg6;bF6r{aO7C{>Q63)HIgWwT=PqXdec?!Osr{L^)$~|9jOP!#-46k|lA1?It|2$Sq z2oa##k|bSdATC#7ilhrcP(S1gx;+4UoVr~DmSJ}!10t@u5io4x^VZR)wT(u+ zIY$u>QqW#d!m;?*7zcS|;BcGR+x_2r7-!s^3XZqJ2z!t;BM0Ey53`}f?hzqwz&<4oxagr!RSUk2*?{>XOvLoTrvaCiGJFa4e33L*!R;ZilJi>I8dhH zD6R%!TQzVrQGJb$6U&3J+o1~4l{@*sA{|W5W+;XBa8f-&r74Dv@+NL1l2pH*(*0QV zo7v}eEenMS9l=#^8am}ve{4V<16V$wHpEHlDDLnkxQoU7ZgDR~q?u$2jDB<`<_))% z7o;$=3kakFk!Gibkt7NIp>w7S^;ENsT7V&ws(d`FV0b3UnEXEM1aTPZ-FZTSS#5F8@^L5)xh*G zN_FM{lR>p?LqA%((6}Owpo|pkiTuR<^a)uZCSIXWqEn_m#*!4Q+vCk|#F{+gh`jgw pQIw3?hY`??= 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 + + if lower_limit: + if lower_limit 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]}." + ) + + 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 = [joint_position_dict[name] for name in self.joint_names] + joint_position_dict = dict(zip(msg.name, msg.velocity)) + self.current_joint_velocities = [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") + + 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): + pass + + def trapezoidal_timestamps(self, n_points, total_duration, accel_fraction=0.2): + """ + Generate timestamps for a trapezoidal velocity profile. + + Parameters: + n_points (int): Total number of trajectory points. + total_duration (float): Duration of the full motion [seconds]. + accel_fraction (float): Fraction of the total time spent accelerating and decelerating. + Default is 0.2 (i.e., 20% accel, 60% cruise, 20% decel) + + Returns: + list of float: Timestamps for each point [seconds]. + """ + + if n_points == 2: + return [0.0, total_duration] + + # Time fractions + ta = accel_fraction * total_duration # acceleration time + tc = total_duration - 2 * ta # constant velocity time + + # Number of points per segment + n_accel = int(n_points * accel_fraction) + if n_accel == 0: + return list(np.linspace(0, total_duration, n_points)) + n_cruise = n_points - 2*n_accel + # Time vectors for each segment + t_accel = np.linspace(0, ta, n_accel, endpoint=False) + t_cruise = np.linspace(ta, ta + tc, n_cruise, endpoint=False) + t_decel = np.linspace(ta + tc, total_duration, n_accel, endpoint=True) + + timestamps = np.concatenate([t_accel, t_cruise, t_decel]) + return list(timestamps) + + + + def send_tcp_coordinates(self): + """Send the desired TCP coordinates to the robot.""" + try: + msg = JointTrajectory() + msg.joint_names = self.joint_names + steps_per_m = 100 + [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 = [ + sm.SE3([ + x + (x1 - x) / (steps - 1) * i, + y + (y1 - y) / (steps - 1) * i, + z + (z1 - z) / (steps - 1) * i + ]) * q0.interp(q1, i / (steps - 1)).SE3() + for i in range(steps) + ] + + print(cart_traj) + + if self.desired[-1]: + timestamps = self.trapezoidal_timestamps(steps, self.desired[-1]) + 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.prev_pose == 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.prev_pose[:3] + [roll, pitch, yaw] = self.prev_pose[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 /= 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): + pass + + def send_cartesian_trajectory(self): + pass + + 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 + 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. Each element of the cost mask corresponds to a catesian coordinate [x, y, z, Rx, Ry, Rz].") + 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"Cost mask: {cost_mask}") + 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: + while True: + try: + joint_velocity_limits = {name: float(input(f"Enter the velocity limit for joint '{name}' (or press Enter to skip): ").strip())} for name in joint_names} + break + except ValueError: + print("Invalid input. Please enter numeric values or leave blank to skip.") + ''' + joint_velocity_limits = None + 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() + rclpy.shutdown() + osc_terminate() + +if __name__ == '__main__': + main() diff --git a/workspace/src/osc_ros2/setup.py b/workspace/src/osc_ros2/setup.py index b941410..023d058 100644 --- a/workspace/src/osc_ros2/setup.py +++ b/workspace/src/osc_ros2/setup.py @@ -28,6 +28,7 @@ setup( entry_points={ 'console_scripts': [ 'interface = osc_ros2.osc_ros2:main', + 'interface1 = osc_ros2.osc_ros2_unit_quater:main', ], }, )