nwegy/OSC_ROS2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AS: changed interpolation for angles using unit quaterions instead of rpy

Browse Source
This commit is contained in:
Alexander Schaefer 2025-05-08 13:52:59 +02:00
parent b67b2a5174
commit 4da5338ca8
20 changed files with 1082 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
osc_recieve.py
View File

@ -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:

6
workspace/build/osc_ros2/colcon_command_prefix_setup_py.sh.env
View File

@ -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

1
workspace/build/osc_ros2/osc_ros2.egg-info/SOURCES.txt
View File

@ -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

1
workspace/build/osc_ros2/osc_ros2.egg-info/entry_points.txt
View File

@ -1,3 +1,4 @@
[console_scripts]
interface = osc_ros2.osc_ros2:main
interface1 = osc_ros2.osc_ros2_unit_quater:main

BIN
workspace/build/osc_ros2/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
View File

Binary file not shown.

33
workspace/install/osc_ros2/lib/osc_ros2/interface1 Executable file
View File

@ -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')())

38
workspace/log/build_2025-05-08_11-12-16/events.log Normal file
View File

@ -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: {}

174
workspace/log/build_2025-05-08_11-12-16/logger_all.log Normal file
View File

@ -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=<colcon_mixin.mixin.mixin_argument.MixinArgumentDecorator object at 0x7ffffe202b60>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7ffffe2ebaf0>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7ffffe2ebaf0>>, 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'

2
workspace/log/build_2025-05-08_11-12-16/osc_ros2/command.log Normal file
View File

@ -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

0
workspace/log/build_2025-05-08_11-12-16/osc_ros2/stderr.log Normal file
View File

16
workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout.log Normal file
View File

@ -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

16
workspace/log/build_2025-05-08_11-12-16/osc_ros2/stdout_stderr.log Normal file
View File

@ -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

18
workspace/log/build_2025-05-08_11-12-16/osc_ros2/streams.log Normal file
View File

@ -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

2
workspace/log/latest_build
View File

@ -1 +1 @@
build_2025-05-03_10-02-17
build_2025-05-08_11-12-16

2
workspace/src/joint_angles.py
View File

@ -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]

BIN
workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc
View File

Binary file not shown.

BIN
workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2_unit_quater.cpython-310.pyc Normal file
View File

Binary file not shown.

4
workspace/src/osc_ros2/osc_ros2/osc_ros2.py
View File

@ -369,6 +369,7 @@ class OSC_ROS2_interface(Node):
def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits
print("tcp_coordinates_handler")
if self.robot:
try:
if len(args) == 6:
@ -683,8 +684,8 @@ def main():
print("Invalid path. Please enter a valid path to the URDF file.")
tree = ET.parse(robot_urdf)
root = tree.getroot()
joint_names = [link.name for link in robot.links if link.isjoint]
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 = {}
@ -745,7 +746,6 @@ def main():
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

773
workspace/src/osc_ros2/osc_ros2/osc_ros2_unit_quater.py Normal file
View File

@ -0,0 +1,773 @@
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):
print("Joint names received from JointState message:")
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')
while True:
try:
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
)
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
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 log 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 == "":
commands_port = 8000
else:
commands_port = int(commands_port)
break
except ValueError:
print("Invalid input. Please enter a valid port.")
continue
if robot:
while True:
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
if lower_limit:
if lower_limit<lim[0][i]:
while True:
sure = input(f"Are you sure you want to set the lower limit to {lower_limit} rad which is less than the default limit {lim[0][i]}(y/n): ").strip().lower()
if sure == 'y':
lim[0][i] = float(lower_limit)
break
elif sure == 'n':
print("Lower limit not changed.")
break
print("Invalid input. Please enter 'y' or 'n'.")
else: lim[0][i] = float(lower_limit)
if upper_limit:
if upper_limit<lim[0][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[0][i] = float(lower_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:
if not(self.joint_names):
while True:
print('-+'*50)
print("Joint names:")
print(self.joint_names)
print('-'*50)
correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: (y/n)?: ").strip()
if correct.lower() == 'y':
break
elif correct.lower() == 'n':
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 = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
if correct.lower() == 'y':
break
else:
print("Please re-enter the joint names.")
print('invalid input. Please enter "y" or "n".')
self.n_joints = len(joint_names)
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_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)
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
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'Ready to receive OSC messages on 0.0.0.0:{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
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])
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):
pass
def joint_position_handler(self, *args):
pass
def cartesian_trajectory_handler(self, *args):
"""Handles incoming OSC messages for cartesian trajectory."""
if self.robot:
pass
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)] + [None]
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
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]}."
)
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 <limit> 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()

1
workspace/src/osc_ros2/setup.py
View File

@ -28,6 +28,7 @@ setup(
entry_points={
'console_scripts': [
'interface = osc_ros2.osc_ros2:main',
'interface1 = osc_ros2.osc_ros2_unit_quater:main',
],
},
)