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

AS

Browse Source
This commit is contained in:
Alexander Schaefer 2025-05-12 20:20:00 +02:00
parent 4da5338ca8
commit 472cbc6b08
53 changed files with 2629 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

BIN
test/.DS_Store vendored
View File

Binary file not shown.

2
test/joint_states_recording.py
View File

@ -16,7 +16,7 @@ class JointStateLogger(Node):
self.joint_states_callback, self.joint_states_callback,
1000 1000
) )
self.robot = rtb.ERobot.URDF('/ws/src/ba-alexanderschaefer/ur10e.urdf') self.robot = rtb.ERobot.URDF('/BA/ur10e.urdf')
# Directory to store logs # Directory to store logs
self.log_dir = './joint_states_logs' self.log_dir = './joint_states_logs'
os.makedirs(self.log_dir, exist_ok=True) os.makedirs(self.log_dir, exist_ok=True)

2
test/joint_trajectory_recording.py
View File

@ -25,7 +25,7 @@ class JointTrajectoryLogger(Node):
) )
self.counter = 0 self.counter = 0
self.robot = rtb.ERobot.URDF('/ws/src/ba-alexanderschaefer/ur10e.urdf') self.robot = rtb.ERobot.URDF('/BA/ur10e.urdf')
def trajectory_callback(self, msg: JointTrajectory): def trajectory_callback(self, msg: JointTrajectory):
self.counter += 1 self.counter += 1

35
test/osc_command_recording.py Normal file
View File

@ -0,0 +1,35 @@
from osc4py3.as_eventloop import *
from osc4py3 import oscmethod as osm
import csv
import time
# CSV setup
csv_file = './pose_log.csv'
with open(csv_file, 'w', newline='') as f:
writer = csv.writer(f)
writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
# OSC handler
def pose_handler(timestamp, x, y, z, roll, pitch, yaw):
print(f"[{timestamp:.6f}] Received: x={x}, y={y}, z={z}, roll={roll}, pitch={pitch}, yaw={yaw}")
with open(csv_file, 'a', newline='') as f:
writer = csv.writer(f)
writer.writerow([timestamp, x, y, z, roll, pitch, yaw])
# Setup
osc_startup()
osc_udp_server("0.0.0.0", 8000, "pose_server")
# Register OSC method (with timestamp argument)
osc_method("/tcp_coordinates", pose_handler, argscheme=osm.OSCARG_READTIME+ osm.OSCARG_DATAUNPACK)
print("Listening for OSC messages on port 8000...")
try:
while True:
osc_process()
time.sleep(0.01)
except KeyboardInterrupt:
print("Shutting down...")
osc_terminate()

31
test/rampfunction.py
View File

@ -0,0 +1,31 @@
import numpy as np
def symmetric_timestamps(total_duration: float, num_points: int, flat_ratio: float = 0.3) -> list:
"""
Generate symmetric timestamps with increasing → decreasing → flat → increasing spacing pattern.
Args:
total_duration (float): The total duration (last timestamp).
num_points (int): Total number of timestamps (must be >= 3).
flat_ratio (float): Fraction of timestamps in the constant-spacing center (0.0–0.9).
Returns:
List[float]: List of timestamps from 0 to total_duration.
"""
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
a = symmetric_timestamps(7.5, 30, 0.7)
for i in range(len(a)-1):
print(f"{a[i]:.2f} - {a[i+1]:.2f} = {a[i+1]-a[i]}")

2
test/ros2_ws/install/local_setup.sh
View File

@ -6,7 +6,7 @@
# since a plain shell script can't determine its own path when being sourced # since a plain shell script can't determine its own path when being sourced
# either use the provided COLCON_CURRENT_PREFIX # either use the provided COLCON_CURRENT_PREFIX
# or fall back to the build time prefix (if it exists) # or fall back to the build time prefix (if it exists)
_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/BA/ros2_ws/install" _colcon_prefix_sh_COLCON_CURRENT_PREFIX="/BA/test/ros2_ws/install"
if [ -z "$COLCON_CURRENT_PREFIX" ]; then if [ -z "$COLCON_CURRENT_PREFIX" ]; then
if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2

8
test/ros2_ws/install/setup.bash
View File

@ -17,6 +17,14 @@ _colcon_prefix_chain_bash_source_script() {
fi fi
} }
# source chained prefixes
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="/opt/ros/humble"
_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="/BA/workspace/install"
_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
# source this prefix # source this prefix
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"

4
test/ros2_ws/install/setup.ps1
View File

@ -21,6 +21,10 @@ function _colcon_prefix_chain_powershell_source_script {
} }
} }
# source chained prefixes
_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
_colcon_prefix_chain_powershell_source_script "/BA/workspace/install\local_setup.ps1"
# source this prefix # source this prefix
$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) $env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" _colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"

12
test/ros2_ws/install/setup.sh
View File

@ -7,7 +7,7 @@
# since a plain shell script can't determine its own path when being sourced # since a plain shell script can't determine its own path when being sourced
# either use the provided COLCON_CURRENT_PREFIX # either use the provided COLCON_CURRENT_PREFIX
# or fall back to the build time prefix (if it exists) # or fall back to the build time prefix (if it exists)
_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/BA/ros2_ws/install _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/BA/test/ros2_ws/install
if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
@ -29,6 +29,16 @@ _colcon_prefix_chain_sh_source_script() {
fi fi
} }
# source chained prefixes
# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
COLCON_CURRENT_PREFIX="/opt/ros/humble"
_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
COLCON_CURRENT_PREFIX="/BA/workspace/install"
_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
# source this prefix # source this prefix
# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script # setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"

8
test/ros2_ws/install/setup.zsh
View File

@ -17,6 +17,14 @@ _colcon_prefix_chain_zsh_source_script() {
fi fi
} }
# source chained prefixes
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="/opt/ros/humble"
_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="/BA/workspace/install"
_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
# source this prefix # source this prefix
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script # setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"

7
test/ros2_ws/log/build_2025-05-08_20-38-18/events.log Normal file
View File

@ -0,0 +1,7 @@
[0.000000] (-) TimerEvent: {}
[0.000622] (-) JobUnselected: {'identifier': 'more_interfaces'}
[0.000830] (-) JobUnselected: {'identifier': 'py_pubsub'}
[0.000965] (-) JobUnselected: {'identifier': 'py_srvcli'}
[0.001070] (-) JobUnselected: {'identifier': 'python_parameters'}
[0.001108] (-) JobUnselected: {'identifier': 'tutorial_interfaces'}
[0.001146] (-) EventReactorShutdown: {}

160
test/ros2_ws/log/build_2025-05-08_20-38-18/logger_all.log Normal file
View File

@ -0,0 +1,160 @@
[0.113s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'joint_control', '--symlink-install']
[0.114s] 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=['joint_control'], 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.255s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
[0.256s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
[0.256s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
[0.256s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
[0.256s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
[0.256s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
[0.256s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
[0.256s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
[0.257s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/test/ros2_ws'
[0.257s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
[0.257s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
[0.257s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
[0.257s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
[0.257s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
[0.267s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
[0.268s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
[0.268s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
[0.270s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
[0.270s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
[0.270s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
[0.271s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
[0.272s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
[0.274s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['ignore', 'ignore_ament_install']
[0.274s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'ignore'
[0.274s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'ignore_ament_install'
[0.274s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['colcon_pkg']
[0.274s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'colcon_pkg'
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['colcon_meta']
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'colcon_meta'
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['ros']
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'ros'
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['cmake', 'python']
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'cmake'
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'python'
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extensions ['python_setup_py']
[0.275s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control) by extension 'python_setup_py'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['ignore', 'ignore_ament_install']
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'ignore'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'ignore_ament_install'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['colcon_pkg']
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'colcon_pkg'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['colcon_meta']
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'colcon_meta'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['ros']
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'ros'
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['cmake', 'python']
[0.276s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'cmake'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'python'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extensions ['python_setup_py']
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_states_control/__pycache__) by extension 'python_setup_py'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extensions ['ignore', 'ignore_ament_install']
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extension 'ignore'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extension 'ignore_ament_install'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extensions ['colcon_pkg']
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extension 'colcon_pkg'
[0.277s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extensions ['colcon_meta']
[0.278s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extension 'colcon_meta'
[0.278s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extensions ['ros']
[0.278s] Level 1:colcon.colcon_core.package_identification:_identify(src/more_interfaces) by extension 'ros'
[0.283s] DEBUG:colcon.colcon_core.package_identification:Package 'src/more_interfaces' with type 'ros.ament_cmake' and name 'more_interfaces'
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extensions ['ignore', 'ignore_ament_install']
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extension 'ignore'
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extension 'ignore_ament_install'
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extensions ['colcon_pkg']
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extension 'colcon_pkg'
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extensions ['colcon_meta']
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extension 'colcon_meta'
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extensions ['ros']
[0.284s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_pubsub) by extension 'ros'
[0.285s] DEBUG:colcon.colcon_core.package_identification:Package 'src/py_pubsub' with type 'ros.ament_python' and name 'py_pubsub'
[0.285s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extensions ['ignore', 'ignore_ament_install']
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extension 'ignore'
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extension 'ignore_ament_install'
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extensions ['colcon_pkg']
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extension 'colcon_pkg'
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extensions ['colcon_meta']
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extension 'colcon_meta'
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extensions ['ros']
[0.286s] Level 1:colcon.colcon_core.package_identification:_identify(src/py_srvcli) by extension 'ros'
[0.287s] DEBUG:colcon.colcon_core.package_identification:Package 'src/py_srvcli' with type 'ros.ament_python' and name 'py_srvcli'
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extensions ['ignore', 'ignore_ament_install']
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extension 'ignore'
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extension 'ignore_ament_install'
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extensions ['colcon_pkg']
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extension 'colcon_pkg'
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extensions ['colcon_meta']
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extension 'colcon_meta'
[0.288s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extensions ['ros']
[0.289s] Level 1:colcon.colcon_core.package_identification:_identify(src/python_parameters) by extension 'ros'
[0.289s] DEBUG:colcon.colcon_core.package_identification:Package 'src/python_parameters' with type 'ros.ament_python' and name 'python_parameters'
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extensions ['ignore', 'ignore_ament_install']
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extension 'ignore'
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extension 'ignore_ament_install'
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extensions ['colcon_pkg']
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extension 'colcon_pkg'
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extensions ['colcon_meta']
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extension 'colcon_meta'
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extensions ['ros']
[0.290s] Level 1:colcon.colcon_core.package_identification:_identify(src/tutorial_interfaces) by extension 'ros'
[0.291s] DEBUG:colcon.colcon_core.package_identification:Package 'src/tutorial_interfaces' with type 'ros.ament_cmake' and name 'tutorial_interfaces'
[0.291s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
[0.291s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
[0.291s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
[0.291s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
[0.291s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
[0.292s] WARNING:colcon.colcon_core.package_selection:ignoring unknown package 'joint_control' in --packages-select
[0.311s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'more_interfaces' in 'src/more_interfaces'
[0.311s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'python_parameters' in 'src/python_parameters'
[0.311s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'tutorial_interfaces' in 'src/tutorial_interfaces'
[0.311s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'py_pubsub' in 'src/py_pubsub'
[0.311s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'py_srvcli' in 'src/py_srvcli'
[0.311s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
[0.314s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
[0.315s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
[0.315s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
[0.315s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
[0.316s] DEBUG:colcon.colcon_core.event_reactor:joining thread
[0.323s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
[0.323s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
[0.323s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
[0.323s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
[0.325s] 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
[0.325s] DEBUG:colcon.colcon_core.event_reactor:joined thread
[0.327s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
[0.327s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/test/ros2_ws/install/local_setup.ps1'
[0.329s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/test/ros2_ws/install/_local_setup_util_ps1.py'
[0.331s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/test/ros2_ws/install/setup.ps1'
[0.340s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/test/ros2_ws/install/local_setup.sh'
[0.341s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/test/ros2_ws/install/_local_setup_util_sh.py'
[0.342s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/test/ros2_ws/install/setup.sh'
[0.344s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/test/ros2_ws/install/local_setup.bash'
[0.345s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/test/ros2_ws/install/setup.bash'
[0.347s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/test/ros2_ws/install/local_setup.zsh'
[0.347s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/test/ros2_ws/install/setup.zsh'

2
test/ros2_ws/log/latest_build
View File

@ -1 +1 @@
build_2025-03-27_14-22-13 build_2025-05-08_20-38-18

2
workspace/build/joint_control/colcon_build.rc
View File

@ -1 +1 @@
1 0

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

@ -1,4 +1,4 @@
AMENT_PREFIX_PATH=/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=1
COLCON_PREFIX_PATH=/BA/workspace/install COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root HOME=/root
@ -7,10 +7,10 @@ LANG=C.UTF-8
LC_ALL=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 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: 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 OLDPWD=/BA/workspace/src
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/BA/workspace/build/joint_control PWD=/BA/workspace/build/joint_control
PYTHONPATH=/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_DISTRO=humble
ROS_LOCALHOST_ONLY=0 ROS_LOCALHOST_ONLY=0
ROS_PYTHON_VERSION=3 ROS_PYTHON_VERSION=3

2
workspace/build/joint_control/joint_control
View File

@ -1 +1 @@
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/joint_control /BA/workspace/src/joint_control/joint_control

10
workspace/build/joint_control/joint_control.egg-info/SOURCES.txt
View File

@ -25,7 +25,11 @@ joint_control/trajectory_server_cart_fast_smooth.py
joint_control/trajectory_server_new.py joint_control/trajectory_server_new.py
joint_control/trajectory_server_new_cart.py joint_control/trajectory_server_new_cart.py
joint_control/trajectory_server_trapezoidal.py joint_control/trajectory_server_trapezoidal.py
joint_control.egg-info/PKG-INFO
joint_control.egg-info/SOURCES.txt
joint_control.egg-info/dependency_links.txt
joint_control.egg-info/entry_points.txt
joint_control.egg-info/requires.txt
joint_control.egg-info/top_level.txt
joint_control.egg-info/zip-safe
resource/joint_control resource/joint_control
test/test_copyright.py
test/test_flake8.py
test/test_pep257.py

2
workspace/build/joint_control/package.xml
View File

@ -1 +1 @@
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/package.xml /BA/workspace/src/joint_control/package.xml

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

Binary file not shown.

2
workspace/build/joint_control/resource/joint_control
View File

@ -1 +1 @@
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/resource/joint_control /BA/workspace/src/joint_control/resource/joint_control

2
workspace/build/joint_control/setup.cfg
View File

@ -1 +1 @@
/ws/src/ba-alexanderschaefer/workspace/src/joint_control/setup.cfg /BA/workspace/src/joint_control/setup.cfg

1
workspace/build/joint_control/setup.py Symbolic link
View File

@ -0,0 +1 @@
/BA/workspace/src/joint_control/setup.py

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

@ -3,7 +3,7 @@ setup.cfg
setup.py setup.py
osc_ros2/__init__.py osc_ros2/__init__.py
osc_ros2/osc_ros2.py osc_ros2/osc_ros2.py
osc_ros2/osc_ros2_unit_quater.py osc_ros2/osc_ros2_rpy.py
osc_ros2.egg-info/PKG-INFO osc_ros2.egg-info/PKG-INFO
osc_ros2.egg-info/SOURCES.txt osc_ros2.egg-info/SOURCES.txt
osc_ros2.egg-info/dependency_links.txt osc_ros2.egg-info/dependency_links.txt

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

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

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

Binary file not shown.

33
workspace/install/joint_control/lib/joint_control/sandbox Executable file
View File

@ -0,0 +1,33 @@
#!/usr/bin/python3
# EASY-INSTALL-ENTRY-SCRIPT: 'joint-control','console_scripts','sandbox'
import re
import sys
# for compatibility with easy_install; see #2198
__requires__ = 'joint-control'
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('joint-control', 'console_scripts', 'sandbox')())

2
workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link Normal file
View File

@ -0,0 +1,2 @@
/BA/workspace/build/joint_control
.

38
workspace/log/build_2025-05-08_15-47-35/events.log Normal file
View File

@ -0,0 +1,38 @@
[0.000000] (-) TimerEvent: {}
[0.002486] (-) JobUnselected: {'identifier': 'joint_control'}
[0.002732] (-) JobUnselected: {'identifier': 'joint_info'}
[0.003448] (-) JobUnselected: {'identifier': 'mock_robot'}
[0.003512] (-) JobUnselected: {'identifier': 'painting_robot_control'}
[0.003556] (osc_ros2) JobQueued: {'identifier': 'osc_ros2', 'dependencies': OrderedDict()}
[0.003627] (osc_ros2) JobStarted: {'identifier': 'osc_ros2'}
[0.098230] (-) TimerEvent: {}
[0.200156] (-) TimerEvent: {}
[0.301233] (-) TimerEvent: {}
[0.403399] (-) TimerEvent: {}
[0.505383] (-) TimerEvent: {}
[0.610326] (-) TimerEvent: {}
[0.700841] (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.712129] (-) TimerEvent: {}
[0.813149] (-) TimerEvent: {}
[0.918149] (-) TimerEvent: {}
[0.974475] (osc_ros2) StdoutLine: {'line': b'running develop\n'}
[1.018345] (-) TimerEvent: {}
[1.070145] (osc_ros2) StdoutLine: {'line': b'running egg_info\n'}
[1.071046] (osc_ros2) StdoutLine: {'line': b'writing osc_ros2.egg-info/PKG-INFO\n'}
[1.071438] (osc_ros2) StdoutLine: {'line': b'writing dependency_links to osc_ros2.egg-info/dependency_links.txt\n'}
[1.072067] (osc_ros2) StdoutLine: {'line': b'writing entry points to osc_ros2.egg-info/entry_points.txt\n'}
[1.072579] (osc_ros2) StdoutLine: {'line': b'writing requirements to osc_ros2.egg-info/requires.txt\n'}
[1.072982] (osc_ros2) StdoutLine: {'line': b'writing top-level names to osc_ros2.egg-info/top_level.txt\n'}
[1.075522] (osc_ros2) StdoutLine: {'line': b"reading manifest file 'osc_ros2.egg-info/SOURCES.txt'\n"}
[1.076853] (osc_ros2) StdoutLine: {'line': b"writing manifest file 'osc_ros2.egg-info/SOURCES.txt'\n"}
[1.078227] (osc_ros2) StdoutLine: {'line': b'running build_ext\n'}
[1.078700] (osc_ros2) StdoutLine: {'line': b'Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .)\n'}
[1.079630] (osc_ros2) StdoutLine: {'line': b'Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'}
[1.080643] (osc_ros2) StdoutLine: {'line': b'Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2\n'}
[1.081929] (osc_ros2) StdoutLine: {'line': b'\n'}
[1.082296] (osc_ros2) StdoutLine: {'line': b'Installed /BA/workspace/build/osc_ros2\n'}
[1.082546] (osc_ros2) StdoutLine: {'line': b'running symlink_data\n'}
[1.101143] (osc_ros2) CommandEnded: {'returncode': 0}
[1.119108] (-) TimerEvent: {}
[1.125807] (osc_ros2) JobEnded: {'identifier': 'osc_ros2', 'rc': 0}
[1.127119] (-) EventReactorShutdown: {}

174
workspace/log/build_2025-05-08_15-47-35/logger_all.log Normal file
View File

@ -0,0 +1,174 @@
[0.111s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'osc_ros2', '--symlink-install']
[0.112s] 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 0x7ffffe202b90>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7ffffe2ebb20>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7ffffe2ebb20>>, mixin_verb=('build',))
[0.233s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
[0.234s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
[0.234s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
[0.235s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
[0.235s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
[0.235s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
[0.235s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
[0.235s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
[0.235s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/workspace'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
[0.244s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
[0.244s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
[0.244s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
[0.244s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
[0.244s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
[0.245s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
[0.246s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
[0.246s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
[0.246s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
[0.247s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
[0.248s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
[0.248s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
[0.248s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
[0.248s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
[0.248s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install']
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore'
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install'
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg']
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg'
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta']
[0.249s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta'
[0.250s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros']
[0.250s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros'
[0.255s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_control' with type 'ros.ament_python' and name 'joint_control'
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install']
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore'
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install'
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg']
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg'
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta']
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta'
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros']
[0.256s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros'
[0.257s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_info' with type 'ros.ament_python' and name 'joint_info'
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install']
[0.258s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore'
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install'
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg']
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg'
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta']
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta'
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros']
[0.259s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros'
[0.260s] DEBUG:colcon.colcon_core.package_identification:Package 'src/mock_robot' with type 'ros.ament_python' and name 'mock_robot'
[0.260s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install']
[0.260s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore'
[0.260s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install'
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg']
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg'
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta']
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta'
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros']
[0.261s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros'
[0.261s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2'
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install']
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore'
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install'
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg']
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg'
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta']
[0.262s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta'
[0.263s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros']
[0.263s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros'
[0.263s] DEBUG:colcon.colcon_core.package_identification:Package 'src/painting_robot_control' with type 'ros.ament_python' and name 'painting_robot_control'
[0.263s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
[0.263s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
[0.263s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
[0.263s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
[0.264s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
[0.280s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_control' in 'src/joint_control'
[0.280s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_info' in 'src/joint_info'
[0.280s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'mock_robot' in 'src/mock_robot'
[0.280s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'painting_robot_control' in 'src/painting_robot_control'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_args' from command line to 'None'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target' from command line to 'None'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_target_skip_unavailable' from command line to 'False'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_cache' from command line to 'False'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_clean_first' from command line to 'False'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'cmake_force_configure' from command line to 'False'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'ament_cmake_args' from command line to 'None'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_cmake_args' from command line to 'None'
[0.281s] Level 5:colcon.colcon_core.verb:set package 'osc_ros2' build argument 'catkin_skip_building_tests' from command line to 'False'
[0.281s] 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.281s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
[0.284s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
[0.285s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/BA/workspace/src/osc_ros2' with build type 'ament_python'
[0.285s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'ament_prefix_path')
[0.287s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
[0.288s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.ps1'
[0.289s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.dsv'
[0.290s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/ament_prefix_path.sh'
[0.291s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.291s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.591s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/BA/workspace/src/osc_ros2'
[0.592s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.592s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.996s] 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.386s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath_develop')
[1.387s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.ps1'
[1.387s] 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.388s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.dsv'
[1.389s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/osc_ros2/share/osc_ros2/hook/pythonpath_develop.sh'
[1.399s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake module files
[1.401s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2' for CMake config files
[1.403s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib'
[1.403s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin'
[1.403s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/pkgconfig/osc_ros2.pc'
[1.404s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/lib/python3.10/site-packages'
[1.404s] Level 1:colcon.colcon_core.shell:create_environment_hook('osc_ros2', 'pythonpath')
[1.404s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.ps1'
[1.405s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.dsv'
[1.405s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/osc_ros2/share/osc_ros2/hook/pythonpath.sh'
[1.406s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/osc_ros2/bin'
[1.406s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(osc_ros2)
[1.406s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.ps1'
[1.407s] INFO:colcon.colcon_core.shell:Creating package descriptor '/BA/workspace/install/osc_ros2/share/osc_ros2/package.dsv'
[1.408s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.sh'
[1.409s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.bash'
[1.410s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/osc_ros2/share/osc_ros2/package.zsh'
[1.410s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/BA/workspace/install/osc_ros2/share/colcon-core/packages/osc_ros2)
[1.411s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
[1.411s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
[1.411s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
[1.411s] DEBUG:colcon.colcon_core.event_reactor:joining thread
[1.419s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
[1.419s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
[1.419s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
[1.419s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
[1.421s] 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.421s] DEBUG:colcon.colcon_core.event_reactor:joined thread
[1.421s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.ps1'
[1.422s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_ps1.py'
[1.424s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.ps1'
[1.426s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.sh'
[1.427s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_sh.py'
[1.428s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.sh'
[1.430s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.bash'
[1.430s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.bash'
[1.433s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.zsh'
[1.434s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.zsh'

2
workspace/log/build_2025-05-08_15-47-35/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_15-47-35/osc_ros2/stderr.log Normal file
View File

16
workspace/log/build_2025-05-08_15-47-35/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_15-47-35/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_15-47-35/osc_ros2/streams.log Normal file
View File

@ -0,0 +1,18 @@
[0.707s] 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
[0.973s] running develop
[1.067s] running egg_info
[1.067s] writing osc_ros2.egg-info/PKG-INFO
[1.068s] writing dependency_links to osc_ros2.egg-info/dependency_links.txt
[1.069s] writing entry points to osc_ros2.egg-info/entry_points.txt
[1.069s] writing requirements to osc_ros2.egg-info/requires.txt
[1.069s] writing top-level names to osc_ros2.egg-info/top_level.txt
[1.072s] reading manifest file 'osc_ros2.egg-info/SOURCES.txt'
[1.074s] writing manifest file 'osc_ros2.egg-info/SOURCES.txt'
[1.075s] running build_ext
[1.075s] Creating /BA/workspace/install/osc_ros2/lib/python3.10/site-packages/osc-ros2.egg-link (link to .)
[1.076s] Installing interface script to /BA/workspace/install/osc_ros2/lib/osc_ros2
[1.077s] Installing interface1 script to /BA/workspace/install/osc_ros2/lib/osc_ros2
[1.078s]
[1.079s] Installed /BA/workspace/build/osc_ros2
[1.079s] running symlink_data
[1.098s] 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

52
workspace/log/build_2025-05-08_20-39-10/events.log Normal file
View File

@ -0,0 +1,52 @@
[0.000000] (-) TimerEvent: {}
[0.001849] (-) JobUnselected: {'identifier': 'joint_info'}
[0.002118] (-) JobUnselected: {'identifier': 'mock_robot'}
[0.002365] (-) JobUnselected: {'identifier': 'osc_ros2'}
[0.003113] (-) JobUnselected: {'identifier': 'painting_robot_control'}
[0.003260] (joint_control) JobQueued: {'identifier': 'joint_control', 'dependencies': OrderedDict()}
[0.003349] (joint_control) JobStarted: {'identifier': 'joint_control'}
[0.098879] (-) TimerEvent: {}
[0.203989] (-) TimerEvent: {}
[0.304907] (-) TimerEvent: {}
[0.405933] (-) TimerEvent: {}
[0.508068] (-) TimerEvent: {}
[0.608947] (-) TimerEvent: {}
[0.711913] (-) TimerEvent: {}
[0.742618] (joint_control) 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/joint_control/build', '--no-deps', 'symlink_data'], 'cwd': '/BA/workspace/build/joint_control', '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/workspace/src', '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/joint_control', 'LC_ALL': 'C.UTF-8', 'PYTHONPATH': '/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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.812809] (-) TimerEvent: {}
[0.915822] (-) TimerEvent: {}
[1.019811] (-) TimerEvent: {}
[1.032272] (joint_control) StdoutLine: {'line': b'running develop\n'}
[1.120804] (-) TimerEvent: {}
[1.124824] (joint_control) StdoutLine: {'line': b'running egg_info\n'}
[1.126047] (joint_control) StdoutLine: {'line': b'writing joint_control.egg-info/PKG-INFO\n'}
[1.126518] (joint_control) StdoutLine: {'line': b'writing dependency_links to joint_control.egg-info/dependency_links.txt\n'}
[1.127460] (joint_control) StdoutLine: {'line': b'writing entry points to joint_control.egg-info/entry_points.txt\n'}
[1.128217] (joint_control) StdoutLine: {'line': b'writing requirements to joint_control.egg-info/requires.txt\n'}
[1.128815] (joint_control) StdoutLine: {'line': b'writing top-level names to joint_control.egg-info/top_level.txt\n'}
[1.134816] (joint_control) StdoutLine: {'line': b"reading manifest file 'joint_control.egg-info/SOURCES.txt'\n"}
[1.137334] (joint_control) StdoutLine: {'line': b"writing manifest file 'joint_control.egg-info/SOURCES.txt'\n"}
[1.138935] (joint_control) StdoutLine: {'line': b'running build_ext\n'}
[1.139472] (joint_control) StdoutLine: {'line': b'Creating /BA/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)\n'}
[1.140854] (joint_control) StdoutLine: {'line': b'Installing cart_coords script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.141974] (joint_control) StdoutLine: {'line': b'Installing joint_control script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.142528] (joint_control) StdoutLine: {'line': b'Installing plugdata script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.143457] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.144480] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart_fix script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.145277] (joint_control) StdoutLine: {'line': b'Installing plugdata_cart_smooth script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.145925] (joint_control) StdoutLine: {'line': b'Installing sandbox script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.146641] (joint_control) StdoutLine: {'line': b'Installing test script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.147533] (joint_control) StdoutLine: {'line': b'Installing trajectory_server script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.148350] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.149175] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.149737] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast_max_acc script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.150493] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_cart_fast_smooth script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.151177] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_new script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.152702] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_new_cart script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.153815] (joint_control) StdoutLine: {'line': b'Installing trajectory_server_trapezoidal script to /BA/workspace/install/joint_control/lib/joint_control\n'}
[1.154683] (joint_control) StdoutLine: {'line': b'\n'}
[1.155151] (joint_control) StdoutLine: {'line': b'Installed /BA/workspace/build/joint_control\n'}
[1.155455] (joint_control) StdoutLine: {'line': b'running symlink_data\n'}
[1.175447] (joint_control) CommandEnded: {'returncode': 0}
[1.205699] (joint_control) JobEnded: {'identifier': 'joint_control', 'rc': 0}
[1.207432] (-) EventReactorShutdown: {}

2
workspace/log/build_2025-05-08_20-39-10/joint_control/command.log Normal file
View File

@ -0,0 +1,2 @@
Invoking command in '/BA/workspace/build/joint_control': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data
Invoked command in '/BA/workspace/build/joint_control' returned '0': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data

0
workspace/log/build_2025-05-08_20-39-10/joint_control/stderr.log Normal file
View File

30
workspace/log/build_2025-05-08_20-39-10/joint_control/stdout.log Normal file
View File

@ -0,0 +1,30 @@
running develop
running egg_info
writing joint_control.egg-info/PKG-INFO
writing dependency_links to joint_control.egg-info/dependency_links.txt
writing entry points to joint_control.egg-info/entry_points.txt
writing requirements to joint_control.egg-info/requires.txt
writing top-level names to joint_control.egg-info/top_level.txt
reading manifest file 'joint_control.egg-info/SOURCES.txt'
writing manifest file 'joint_control.egg-info/SOURCES.txt'
running build_ext
Creating /BA/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
Installing cart_coords script to /BA/workspace/install/joint_control/lib/joint_control
Installing joint_control script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_fix script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_smooth script to /BA/workspace/install/joint_control/lib/joint_control
Installing sandbox script to /BA/workspace/install/joint_control/lib/joint_control
Installing test script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_max_acc script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_smooth script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_trapezoidal script to /BA/workspace/install/joint_control/lib/joint_control
Installed /BA/workspace/build/joint_control
running symlink_data

30
workspace/log/build_2025-05-08_20-39-10/joint_control/stdout_stderr.log Normal file
View File

@ -0,0 +1,30 @@
running develop
running egg_info
writing joint_control.egg-info/PKG-INFO
writing dependency_links to joint_control.egg-info/dependency_links.txt
writing entry points to joint_control.egg-info/entry_points.txt
writing requirements to joint_control.egg-info/requires.txt
writing top-level names to joint_control.egg-info/top_level.txt
reading manifest file 'joint_control.egg-info/SOURCES.txt'
writing manifest file 'joint_control.egg-info/SOURCES.txt'
running build_ext
Creating /BA/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
Installing cart_coords script to /BA/workspace/install/joint_control/lib/joint_control
Installing joint_control script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_fix script to /BA/workspace/install/joint_control/lib/joint_control
Installing plugdata_cart_smooth script to /BA/workspace/install/joint_control/lib/joint_control
Installing sandbox script to /BA/workspace/install/joint_control/lib/joint_control
Installing test script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_max_acc script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_cart_fast_smooth script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_new_cart script to /BA/workspace/install/joint_control/lib/joint_control
Installing trajectory_server_trapezoidal script to /BA/workspace/install/joint_control/lib/joint_control
Installed /BA/workspace/build/joint_control
running symlink_data

32
workspace/log/build_2025-05-08_20-39-10/joint_control/streams.log Normal file
View File

@ -0,0 +1,32 @@
[0.752s] Invoking command in '/BA/workspace/build/joint_control': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data
[1.029s] running develop
[1.122s] running egg_info
[1.123s] writing joint_control.egg-info/PKG-INFO
[1.124s] writing dependency_links to joint_control.egg-info/dependency_links.txt
[1.125s] writing entry points to joint_control.egg-info/entry_points.txt
[1.125s] writing requirements to joint_control.egg-info/requires.txt
[1.126s] writing top-level names to joint_control.egg-info/top_level.txt
[1.132s] reading manifest file 'joint_control.egg-info/SOURCES.txt'
[1.134s] writing manifest file 'joint_control.egg-info/SOURCES.txt'
[1.136s] running build_ext
[1.136s] Creating /BA/workspace/install/joint_control/lib/python3.10/site-packages/joint-control.egg-link (link to .)
[1.138s] Installing cart_coords script to /BA/workspace/install/joint_control/lib/joint_control
[1.139s] Installing joint_control script to /BA/workspace/install/joint_control/lib/joint_control
[1.139s] Installing plugdata script to /BA/workspace/install/joint_control/lib/joint_control
[1.141s] Installing plugdata_cart script to /BA/workspace/install/joint_control/lib/joint_control
[1.142s] Installing plugdata_cart_fix script to /BA/workspace/install/joint_control/lib/joint_control
[1.142s] Installing plugdata_cart_smooth script to /BA/workspace/install/joint_control/lib/joint_control
[1.143s] Installing sandbox script to /BA/workspace/install/joint_control/lib/joint_control
[1.144s] Installing test script to /BA/workspace/install/joint_control/lib/joint_control
[1.144s] Installing trajectory_server script to /BA/workspace/install/joint_control/lib/joint_control
[1.145s] Installing trajectory_server_cart script to /BA/workspace/install/joint_control/lib/joint_control
[1.146s] Installing trajectory_server_cart_fast script to /BA/workspace/install/joint_control/lib/joint_control
[1.147s] Installing trajectory_server_cart_fast_max_acc script to /BA/workspace/install/joint_control/lib/joint_control
[1.147s] Installing trajectory_server_cart_fast_smooth script to /BA/workspace/install/joint_control/lib/joint_control
[1.148s] Installing trajectory_server_new script to /BA/workspace/install/joint_control/lib/joint_control
[1.150s] Installing trajectory_server_new_cart script to /BA/workspace/install/joint_control/lib/joint_control
[1.151s] Installing trajectory_server_trapezoidal script to /BA/workspace/install/joint_control/lib/joint_control
[1.152s]
[1.152s] Installed /BA/workspace/build/joint_control
[1.152s] running symlink_data
[1.173s] Invoked command in '/BA/workspace/build/joint_control' returned '0': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data

174
workspace/log/build_2025-05-08_20-39-10/logger_all.log Normal file
View File

@ -0,0 +1,174 @@
[0.101s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--packages-select', 'joint_control', '--symlink-install']
[0.102s] 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=['joint_control'], 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.209s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
[0.210s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/Gazebo.meta'
[0.210s] INFO:colcon.colcon_metadata.package_discovery.colcon_meta:Using configuration from '/root/.colcon/metadata/default/fastrtps.meta'
[0.210s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
[0.210s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
[0.210s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
[0.210s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
[0.211s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
[0.211s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/BA/workspace'
[0.211s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
[0.211s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
[0.211s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
[0.211s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
[0.212s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
[0.212s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
[0.212s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
[0.212s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
[0.212s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
[0.220s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
[0.221s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
[0.221s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
[0.221s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
[0.221s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
[0.221s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
[0.222s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
[0.222s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
[0.222s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
[0.223s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
[0.224s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
[0.224s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
[0.224s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
[0.224s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
[0.224s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ignore', 'ignore_ament_install']
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore'
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ignore_ament_install'
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_pkg']
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_pkg'
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['colcon_meta']
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'colcon_meta'
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extensions ['ros']
[0.225s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_control) by extension 'ros'
[0.230s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_control' with type 'ros.ament_python' and name 'joint_control'
[0.231s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ignore', 'ignore_ament_install']
[0.231s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore'
[0.231s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ignore_ament_install'
[0.231s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_pkg']
[0.232s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_pkg'
[0.232s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['colcon_meta']
[0.232s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'colcon_meta'
[0.232s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extensions ['ros']
[0.232s] Level 1:colcon.colcon_core.package_identification:_identify(src/joint_info) by extension 'ros'
[0.233s] DEBUG:colcon.colcon_core.package_identification:Package 'src/joint_info' with type 'ros.ament_python' and name 'joint_info'
[0.233s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ignore', 'ignore_ament_install']
[0.233s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore'
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ignore_ament_install'
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_pkg']
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_pkg'
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['colcon_meta']
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'colcon_meta'
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extensions ['ros']
[0.234s] Level 1:colcon.colcon_core.package_identification:_identify(src/mock_robot) by extension 'ros'
[0.235s] DEBUG:colcon.colcon_core.package_identification:Package 'src/mock_robot' with type 'ros.ament_python' and name 'mock_robot'
[0.235s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ignore', 'ignore_ament_install']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ignore_ament_install'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_pkg']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_pkg'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['colcon_meta']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'colcon_meta'
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extensions ['ros']
[0.236s] Level 1:colcon.colcon_core.package_identification:_identify(src/osc_ros2) by extension 'ros'
[0.237s] DEBUG:colcon.colcon_core.package_identification:Package 'src/osc_ros2' with type 'ros.ament_python' and name 'osc_ros2'
[0.237s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ignore', 'ignore_ament_install']
[0.237s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore'
[0.237s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ignore_ament_install'
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_pkg']
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_pkg'
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['colcon_meta']
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'colcon_meta'
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extensions ['ros']
[0.238s] Level 1:colcon.colcon_core.package_identification:_identify(src/painting_robot_control) by extension 'ros'
[0.239s] DEBUG:colcon.colcon_core.package_identification:Package 'src/painting_robot_control' with type 'ros.ament_python' and name 'painting_robot_control'
[0.239s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
[0.239s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
[0.239s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
[0.239s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
[0.239s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
[0.256s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'joint_info' in 'src/joint_info'
[0.256s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'mock_robot' in 'src/mock_robot'
[0.256s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'osc_ros2' in 'src/osc_ros2'
[0.256s] INFO:colcon.colcon_core.package_selection:Skipping not selected package 'painting_robot_control' in 'src/painting_robot_control'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_args' from command line to 'None'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target' from command line to 'None'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_target_skip_unavailable' from command line to 'False'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_cache' from command line to 'False'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_clean_first' from command line to 'False'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'cmake_force_configure' from command line to 'False'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'ament_cmake_args' from command line to 'None'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_cmake_args' from command line to 'None'
[0.257s] Level 5:colcon.colcon_core.verb:set package 'joint_control' build argument 'catkin_skip_building_tests' from command line to 'False'
[0.257s] DEBUG:colcon.colcon_core.verb:Building package 'joint_control' with the following arguments: {'ament_cmake_args': None, 'build_base': '/BA/workspace/build/joint_control', '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/joint_control', 'merge_install': False, 'path': '/BA/workspace/src/joint_control', 'symlink_install': True, 'test_result_base': None}
[0.257s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
[0.260s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
[0.261s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/BA/workspace/src/joint_control' with build type 'ament_python'
[0.261s] Level 1:colcon.colcon_core.shell:create_environment_hook('joint_control', 'ament_prefix_path')
[0.263s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
[0.263s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.ps1'
[0.266s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.dsv'
[0.268s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/joint_control/share/joint_control/hook/ament_prefix_path.sh'
[0.269s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.269s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[0.582s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/BA/workspace/src/joint_control'
[0.583s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
[0.583s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
[1.017s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/BA/workspace/build/joint_control': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data
[1.437s] Level 1:colcon.colcon_core.shell:create_environment_hook('joint_control', 'pythonpath_develop')
[1.437s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/joint_control/share/joint_control/hook/pythonpath_develop.ps1'
[1.438s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/BA/workspace/build/joint_control' returned '0': PYTHONPATH=/BA/workspace/build/joint_control/prefix_override:/usr/lib/python3/dist-packages/colcon_core/task/python/colcon_distutils_commands:/BA/workspace/install/joint_control/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/joint_control/build --no-deps symlink_data
[1.440s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/build/joint_control/share/joint_control/hook/pythonpath_develop.dsv'
[1.440s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/build/joint_control/share/joint_control/hook/pythonpath_develop.sh'
[1.451s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control' for CMake module files
[1.455s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control' for CMake config files
[1.459s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control/lib'
[1.459s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control/bin'
[1.459s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control/lib/pkgconfig/joint_control.pc'
[1.460s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control/lib/python3.10/site-packages'
[1.460s] Level 1:colcon.colcon_core.shell:create_environment_hook('joint_control', 'pythonpath')
[1.460s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/joint_control/share/joint_control/hook/pythonpath.ps1'
[1.461s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/BA/workspace/install/joint_control/share/joint_control/hook/pythonpath.dsv'
[1.461s] INFO:colcon.colcon_core.shell:Creating environment hook '/BA/workspace/install/joint_control/share/joint_control/hook/pythonpath.sh'
[1.462s] Level 1:colcon.colcon_core.environment:checking '/BA/workspace/install/joint_control/bin'
[1.462s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(joint_control)
[1.462s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/joint_control/share/joint_control/package.ps1'
[1.463s] INFO:colcon.colcon_core.shell:Creating package descriptor '/BA/workspace/install/joint_control/share/joint_control/package.dsv'
[1.464s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/joint_control/share/joint_control/package.sh'
[1.465s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/joint_control/share/joint_control/package.bash'
[1.466s] INFO:colcon.colcon_core.shell:Creating package script '/BA/workspace/install/joint_control/share/joint_control/package.zsh'
[1.467s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/BA/workspace/install/joint_control/share/colcon-core/packages/joint_control)
[1.467s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
[1.467s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
[1.468s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
[1.468s] DEBUG:colcon.colcon_core.event_reactor:joining thread
[1.473s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send'
[1.473s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
[1.473s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
[1.473s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
[1.475s] 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.475s] DEBUG:colcon.colcon_core.event_reactor:joined thread
[1.475s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.ps1'
[1.476s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_ps1.py'
[1.478s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.ps1'
[1.480s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.sh'
[1.481s] INFO:colcon.colcon_core.shell:Creating prefix util module '/BA/workspace/install/_local_setup_util_sh.py'
[1.482s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.sh'
[1.484s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.bash'
[1.484s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.bash'
[1.486s] INFO:colcon.colcon_core.shell:Creating prefix script '/BA/workspace/install/local_setup.zsh'
[1.487s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/BA/workspace/install/setup.zsh'

2
workspace/log/latest_build
View File

@ -1 +1 @@
build_2025-05-08_11-12-16 build_2025-05-08_20-39-10

12
workspace/src/joint_angles.py
View File

@ -12,15 +12,17 @@ def main():
#osc_udp_client("192.168.1.24", 8000, "osc_client") #osc_udp_client("192.168.1.24", 8000, "osc_client")
osc_udp_client("127.0.0.1", 8000, "osc_client") osc_udp_client("127.0.0.1", 8000, "osc_client")
# Example joint positions to send # Example joint positions to send
joint_positions1 = [0.4,0.3, 0.5, 0.0, 0.0, 0.0]#, 6.0] joint_positions1 = [0.4,0.4, 1.0, 0.0, 0.0, 0.0, 3]
joint_positions2 = [0.4,-0.4, 0.6, 0.0,0.0, 0.0] joint_positions2 = [0.4,-0.4, 0.2, 0.0,0.0, 0.0]
joint_positions3 = [-0.4,-0.4, 0.6, 0.0, 0.0, 0.0]#, 6.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] joint_positions4 = [0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0]
joint_positions5 = [0.4,0.4, 0.6, 0.0, 0.0, 0.0]#, 6.0] joint_positions5 = [0.4,0.4, 0.2, 0.0, 0.0, 0.0]#, 6.0]
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions1) msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions1)
osc_send(msg, "osc_client") osc_send(msg, "osc_client")
osc_process() osc_process()
print(time.time())
print(joint_positions1)
print("Sending joint positions") print("Sending joint positions")
''' '''
time.sleep(2) time.sleep(2)

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

Binary file not shown.

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

Binary file not shown.

2
workspace/src/joint_control/joint_control/trajectory_server_new.py
View File

@ -81,7 +81,7 @@ class ScaledJointTrajectoryPublisher(Node):
dt = 0.01 dt = 0.01
tacc = 0.5 tacc = 0.5
print(f'length viapoints: {len(viapoints)}') print(f'length viapoints: {len(viapoints)}')
traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[1 * i for i in self.joint_velocity_limits]) traj = rtb.mstraj(np.array(viapoints), q0 = self.current_joint_positions ,dt=dt, tacc=tacc, qdmax=[0.2 * i for i in self.joint_velocity_limits])
print(len(traj.q)) print(len(traj.q))
print(len(traj.t)) print(len(traj.t))
print(traj.t) print(traj.t)

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

Binary file not shown.

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

@ -14,6 +14,7 @@ import time
import os import os
import re import re
import socket import socket
import csv
class JointNameListener(Node): class JointNameListener(Node):
def __init__(self): def __init__(self):
@ -91,6 +92,9 @@ class OSC_ROS2_interface(Node):
self.new = False self.new = False
self.latency = []
while True: while True:
try: try:
print('+-' * 50) print('+-' * 50)
@ -308,6 +312,9 @@ class OSC_ROS2_interface(Node):
self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}') self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}') self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
self.create_timer(1/self.hz, self.update_position) # Timer to update the position self.create_timer(1/self.hz, self.update_position) # Timer to update the position
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
def speed_scaling_handler(self, *args): def speed_scaling_handler(self, *args):
"""Handles incoming OSC messages for speed scaling.""" """Handles incoming OSC messages for speed scaling."""
@ -369,7 +376,6 @@ class OSC_ROS2_interface(Node):
def tcp_coordinates_handler(self, *args): def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits # Ensure the desired joint positions are within the specified limits
print("tcp_coordinates_handler")
if self.robot: if self.robot:
try: try:
if len(args) == 6: if len(args) == 6:
@ -453,41 +459,22 @@ class OSC_ROS2_interface(Node):
def send_joint_positions(self): def send_joint_positions(self):
pass pass
self.previous_desired = None
def trapezoidal_timestamps(self, n_points, total_duration, accel_fraction=0.2): def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
"""
Generate timestamps for a trapezoidal velocity profile.
Parameters: if num_points == 2:
n_points (int): Total number of trajectory points. return [0, total_duration]
total_duration (float): Duration of the full motion [seconds]. n = int(num_points*(1-flat_ratio)/2)
accel_fraction (float): Fraction of the total time spent accelerating and decelerating. start = np.cos(np.linspace(0, np.pi, n))+2
Default is 0.2 (i.e., 20% accel, 60% cruise, 20% decel) end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
Returns: timestamps = np.concatenate((start, flat, end))
list of float: Timestamps for each point [seconds]. timestamps *= total_duration / timestamps.sum()
""" timestamps = np.cumsum(timestamps)
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)
return timestamps.tolist()
def send_tcp_coordinates(self): def send_tcp_coordinates(self):
@ -496,18 +483,64 @@ class OSC_ROS2_interface(Node):
msg = JointTrajectory() msg = JointTrajectory()
msg.joint_names = self.joint_names msg.joint_names = self.joint_names
steps_per_m = 100 steps_per_m = 100
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t [x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:-1]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
#self.prev_pose = self.desired[1:] self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m) steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2 if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)] cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
'''if self.previous_desired:
[x,y,z] = self.previous_desired[1:4]
q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])
else:
[x, y, z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2:
steps = 2
cart_traj = []
for i in range(steps):
alpha = i / (steps - 1)
# Convert to arrays for robust interpolation if needed
q0_array = q0.vec
q1_array = q1.vec
dot = np.dot(q0_array, q1_array)
dot = np.clip(dot, -1.0, 1.0)
if abs(dot) > 0.9995:
# Linear interpolation + normalization
q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
q_interp = sm.UnitQuaternion(q_interp_array)
else:
q_interp = q0.interp(q1, alpha)
# Interpolate translation
pos_interp = [
x + (x1 - x) * alpha,
y + (y1 - y) * alpha,
z + (z1 - z) * alpha
]
# Compose SE3 transform
cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
if self.desired[-1]: if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1]) timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
print(f'timestamps: {timestamps}')
for j in range(steps): 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') 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: if sol[1] == 1:
@ -553,12 +586,12 @@ class OSC_ROS2_interface(Node):
else: else:
prev_duration = 0 prev_duration = 0
''' '''
if self.prev_pose == None: if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t [x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy() [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else: else:
[x,y,z] = self.prev_pose[:3] [x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.prev_pose[3:] [roll, pitch, yaw] = self.previous_desired[3:]
''' '''
for j in range(steps): 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') 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')
@ -609,32 +642,41 @@ class OSC_ROS2_interface(Node):
def send_joint_trajectory(self): def send_joint_trajectory(self):
pass pass
self.previous_desired = None
def send_cartesian_trajectory(self): def send_cartesian_trajectory(self):
pass pass
self.previous_desired = None
def update_position(self): def update_position(self):
"""Calls the appropriate function to update the robot's position.""" """Calls the appropriate function to update the robot's position."""
try: try:
if self.desired is None or not(self.new): if self.desired is None or not(self.new):
return return
start_time = time.time()
if self.desired[0] == "joint_positions": if self.desired[0] == "joint_positions":
self.new = False self.new = False
self.send_joint_positions() self.send_joint_positions()
return #return
elif self.desired[0] == "tcp_coordinates": elif self.desired[0] == "tcp_coordinates":
self.new = False self.new = False
self.send_tcp_coordinates() self.send_tcp_coordinates()
return #return
elif self.desired[0] == "joint_trajectory": elif self.desired[0] == "joint_trajectory":
self.new = False self.new = False
self.send_joint_trajectory() self.send_joint_trajectory()
return #return
elif self.desired[0] == "cartesian_trajectory": elif self.desired[0] == "cartesian_trajectory":
self.new = False self.new = False
self.send_cartesian_trajectory() self.send_cartesian_trajectory()
#return
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return return
self.latency.append(time.time()-start_time)
except Exception as e: except Exception as e:
self.get_logger().fatal(f'update_position: {e}') self.get_logger().fatal(f'update_position: {e}')
@ -746,6 +788,7 @@ def main():
break break
print("Invalid input. Please enter 'y' or 'n'.") print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask) 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 # Run ROS 2 spin, and osc_process will be handled by the timer
@ -754,6 +797,17 @@ def main():
except KeyboardInterrupt: except KeyboardInterrupt:
print("") print("")
finally: finally:
file_path = "./latency_log.csv"
# If file doesn't exist, create with header
if not os.path.exists(file_path):
with open(file_path, mode='w', newline='') as f:
writer = csv.writer(f)
writer.writerow('latency')
with open(file_path, mode='a', newline='') as f:
writer = csv.writer(f)
for i in node.latency:
writer.writerow([i])
node.destroy_node() node.destroy_node()
rclpy.shutdown() rclpy.shutdown()
osc_terminate() osc_terminate()

770
workspace/src/osc_ros2/osc_ros2/osc_ros2_latency.py Normal file
View File

@ -0,0 +1,770 @@
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
import csv
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
self.latency = []
self.time1 = None
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+osm.OSCARG_READTIME)
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
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
def speed_scaling_handler(self, *args):
"""Handles incoming OSC messages for speed scaling."""
try:
if len(args) == 1:
if args[0] < 0:
self.speed_scaling = -float(args[0])
else:
self.speed_scaling = float(args[0])
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
"""Handles incoming OSC messages for TCP coordinates."""
print(args)
recv_time = args[-1]
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, recv_time]
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
self.previous_desired = None
def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
if num_points == 2:
return [0, total_duration]
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
def send_tcp_coordinates(self):
"""Send the desired TCP coordinates to the robot."""
try:
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:7]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
if False:#self.desired[-2]:
print(f"self.desired: {self.desired}")
timestamps = self.trapezoidal_timestamps(steps, self.desired[-2], 0.8)
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = timestamps[j]
if duration == 0:
prev_sol = list(sol[0])
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
prev_sol = list(sol[0])
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
else:
prev_duration = 0
'''
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.previous_desired[3:]
'''
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = 0
prev = self.current_joint_positions if j == 0 else prev_sol
for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
prev_sol = list(sol[0])
if duration == 0:
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
duration /= 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
self.previous_desired = None
def send_cartesian_trajectory(self):
pass
self.previous_desired = None
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
if self.desired is None or not(self.new):
return
if self.desired[0] == "joint_positions":
self.new = False
self.send_joint_positions()
#return
elif self.desired[0] == "tcp_coordinates":
self.new = False
self.send_tcp_coordinates()
self.latency.append(time.time()-self.desired[-1])
#return
elif self.desired[0] == "joint_trajectory":
self.new = False
self.send_joint_trajectory()
#return
elif self.desired[0] == "cartesian_trajectory":
self.new = False
self.send_cartesian_trajectory()
#return
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return
except Exception as e:
self.get_logger().fatal(f'update_position: {e}')
def clean_log_string(self, s):
s = str(s)
# Remove ANSI escape sequences (e.g., \x1b[31m)
ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
s = ansi_escape.sub('', s)
# Replace tabs/newlines with spaces
s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
# Strip leading/trailing whitespace
s = s.strip()
# Optionally enforce ASCII only (replace non-ASCII chars with '?')
s = s.encode('ascii', 'replace').decode('ascii')
return s
def log_callback(self, msg: Log):
"""Callback function to handle incoming log messages."""
# Send the log message as an OSC message
msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
osc_send(msg_log, "osc_log_client")
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
rclpy.init()
while True:
use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
if use_urdf == 'y':
while True:
robot_urdf = input("Enter the path to the URDF file: ")
if os.path.isfile(robot_urdf):
if not robot_urdf.endswith('.urdf'):
print("The file is not a URDF file. Please enter a valid URDF file.")
continue
break
else:
print("Invalid path. Please enter a valid path to the URDF file.")
tree = ET.parse(robot_urdf)
root = tree.getroot()
robot = rtb.ERobot.URDF(robot_urdf)
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
print(robot)
joint_velocity_limits = {}
# Iterate over all joints in the URDF
for joint in root.findall('.//joint'):
joint_name = joint.get('name') # Get the name of the joint
# Look for the <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:
file_path = "./latency_log.csv"
with open(file_path, mode='a', newline='') as f:
writer = csv.writer(f)
writer.writerow('latency')
for i in node.latency:
writer.writerow([i])
node.destroy_node()
rclpy.shutdown()
osc_terminate()
if __name__ == '__main__':
main()

811
workspace/src/osc_ros2/osc_ros2/osc_ros2_osc_record.py Normal file
View File

@ -0,0 +1,811 @@
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
import csv
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
self.commands =[]
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_READTIME+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
self.create_timer(3, self.reset_prev) # reset the previous desired position
def reset_prev(self): self.previous_desired = None
def speed_scaling_handler(self, *args):
"""Handles incoming OSC messages for speed scaling."""
try:
if len(args) == 1:
if args[0] < 0:
self.speed_scaling = -float(args[0])
else:
self.speed_scaling = float(args[0])
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, readtime, *args):
# Ensure the desired joint positions are within the specified limits
if self.robot:
try:
self.commands.append([readtime] + list(args))
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
self.previous_desired = None
def trapezoidal_timestamps(self, num_points,total_duration, flat_ratio = 0.3):
if num_points == 2:
return [0, total_duration]
n = int(num_points*(1-flat_ratio)/2)
start = np.cos(np.linspace(0, np.pi, n))+2
end = np.cos(np.linspace(-np.pi, 0, n))+2
flat = np.ones(num_points-2*n)
timestamps = np.concatenate((start, flat, end))
timestamps *= total_duration / timestamps.sum()
timestamps = np.cumsum(timestamps)
return timestamps.tolist()
def send_tcp_coordinates(self):
"""Send the desired TCP coordinates to the robot."""
try:
msg = JointTrajectory()
msg.joint_names = self.joint_names
steps_per_m = 100
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[1:4]
[roll, pitch, yaw] = self.previous_desired[4:-1]
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
self.previous_desired = self.desired
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
'''if self.previous_desired:
[x,y,z] = self.previous_desired[1:4]
q0 = sm.UnitQuaternion.RPY(self.previous_desired[3], self.previous_desired[4], self.previous_desired[5])
else:
[x, y, z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R)
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1)
steps = int(np.linalg.norm(np.array([x1, y1, z1]) - self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2:
steps = 2
cart_traj = []
for i in range(steps):
alpha = i / (steps - 1)
# Convert to arrays for robust interpolation if needed
q0_array = q0.vec
q1_array = q1.vec
dot = np.dot(q0_array, q1_array)
dot = np.clip(dot, -1.0, 1.0)
if abs(dot) > 0.9995:
# Linear interpolation + normalization
q_interp_array = (1 - alpha) * q0_array + alpha * q1_array
q_interp_array = q_interp_array / np.linalg.norm(q_interp_array)
q_interp = sm.UnitQuaternion(q_interp_array)
else:
q_interp = q0.interp(q1, alpha)
# Interpolate translation
pos_interp = [
x + (x1 - x) * alpha,
y + (y1 - y) * alpha,
z + (z1 - z) * alpha
]
# Compose SE3 transform
cart_traj.append(sm.SE3(pos_interp) * q_interp.SE3())'''
if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.8)
print(f'timestamps: {timestamps}')
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = timestamps[j]
if duration == 0:
prev_sol = list(sol[0])
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
msg.points.append(point)
prev_sol = list(sol[0])
else:
self.get_logger().warn(f"send_tcp_coordinates: IK could not find a solution for (x,y,z) = {cart_traj[j].t} and (r,p,y) = {cart_traj[j].rpy()}!")
prev_sol = self.current_joint_positions
if len(msg.points) == 0:
self.get_logger().warn("send_tcp_coordinates: The resulting trajectory is empty. Either the IK failed or the trajectory is too short.")
self.previous_desired = self.desired
return
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
else:
prev_duration = 0
'''
if self.previous_desired == None:
[x,y,z] = self.robot.fkine(self.current_joint_positions).t
[roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
else:
[x,y,z] = self.previous_desired[:3]
[roll, pitch, yaw] = self.previous_desired[3:]
'''
for j in range(steps):
sol = self.robot.ik_LM(cart_traj[j], q0=self.current_joint_positions, mask = self.cost_mask, joint_limits = True, method = 'chan') if j == 0 else self.robot.ik_LM(cart_traj[j], q0=prev_sol, mask = self.cost_mask, joint_limits = True, method = 'chan')
if sol[1] == 1:
fowards = self.robot.fkine_all(sol[0])
out_of_bounds = (fowards.t[1:,0] > self.x_limits[1] if self.x_limits[1] != None else False) | (fowards.t[1:,0] < self.x_limits[0] if self.x_limits[0] != None else False) | (fowards.t[1:,1] > self.y_limits[1] if self.y_limits[1] != None else False) | (fowards.t[1:,1] < self.y_limits[0] if self.y_limits[0] != None else False) | (fowards.t[1:,2] > self.z_limits[1] if self.z_limits[1] != None else False) | (fowards.t[1:,2] < self.z_limits[0] if self.z_limits[0] != None else False)
if np.any(out_of_bounds):
#print(fowards.t)
#indices = np.where(out_of_bounds)[0]
#print(f"indices: {indices}")
self.get_logger().warn("send_tcp_coordinates: One or more links moved out of bounds!")
'''
for i in indices:
try:
print(f"Joint {self.robot.links[i].name} is out of bounds: (x,y,z) = {fowards.t[i]}")
except IndexError:
print(f"index {i} is out of bounds, but no corresponding joint found.")
self.previous_desired_tcp_position = self.desired_tcp_position
'''
break
duration = 0
prev = self.current_joint_positions if j == 0 else prev_sol
for p1, p2, max_vel in zip(sol[0], prev, self.joint_velocity_limits.values()):
duration = max(duration, abs(p1 - p2) / max_vel)#, 1/self.hz) # as minimun
prev_sol = list(sol[0])
if duration == 0:
continue
point = JointTrajectoryPoint()
point.positions = list(sol[0])
duration /= 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
self.previous_desired = None
def send_cartesian_trajectory(self):
pass
self.previous_desired = None
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
if self.desired is None or not(self.new):
return
if self.desired[0] == "joint_positions":
self.new = False
self.send_joint_positions()
return
elif self.desired[0] == "tcp_coordinates":
self.new = False
self.send_tcp_coordinates()
return
elif self.desired[0] == "joint_trajectory":
self.new = False
self.send_joint_trajectory()
return
elif self.desired[0] == "cartesian_trajectory":
self.new = False
self.send_cartesian_trajectory()
return
else:
self.get_logger().warn(f"update_position: Unknown desired type '{self.desired[0]}'.")
return
except Exception as e:
self.get_logger().fatal(f'update_position: {e}')
def clean_log_string(self, s):
s = str(s)
# Remove ANSI escape sequences (e.g., \x1b[31m)
ansi_escape = re.compile(r'\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])')
s = ansi_escape.sub('', s)
# Replace tabs/newlines with spaces
s = s.replace('\n', ' ').replace('\r', ' ').replace('\t', ' ').replace("'", ' '). replace('"', ' ').replace('`', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ').replace('´', ' ').replace('`', ' ').replace('“', ' ').replace('”', ' ').replace('‘', ' ').replace('’', ' ')
# Strip leading/trailing whitespace
s = s.strip()
# Optionally enforce ASCII only (replace non-ASCII chars with '?')
s = s.encode('ascii', 'replace').decode('ascii')
return s
def log_callback(self, msg: Log):
"""Callback function to handle incoming log messages."""
# Send the log message as an OSC message
msg_log = oscbuildparse.OSCMessage(f"/log/{self.log_dict.get(msg.level, 'UNKNOWN')}", ',isss', [int(msg.level), str(msg.stamp.sec+msg.stamp.nanosec*1e-9) , str(msg.name), self.clean_log_string(msg.msg)])
osc_send(msg_log, "osc_log_client")
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
rclpy.init()
while True:
use_urdf = input("Do you have a URDF file you want to use? (y/n): ").strip().lower()
if use_urdf == 'y':
while True:
robot_urdf = input("Enter the path to the URDF file: ")
if os.path.isfile(robot_urdf):
if not robot_urdf.endswith('.urdf'):
print("The file is not a URDF file. Please enter a valid URDF file.")
continue
break
else:
print("Invalid path. Please enter a valid path to the URDF file.")
tree = ET.parse(robot_urdf)
root = tree.getroot()
robot = rtb.ERobot.URDF(robot_urdf)
joint_names = [joint.get('name') for joint in root.findall('joint') if joint.get('type') == 'revolute' or joint.get('type') == 'continuous' or joint.get('type') == 'prismatic']
print(robot)
joint_velocity_limits = {}
# Iterate over all joints in the URDF
for joint in root.findall('.//joint'):
joint_name = joint.get('name') # Get the name of the joint
# Look for the <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:
csv_file = './pose_log.csv'
with open(csv_file, 'w', newline='') as f:
writer = csv.writer(f)
writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
with open(csv_file, 'a', newline='') as f:
writer = csv.writer(f)
for command in node.commands:
writer.writerow(command)
node.destroy_node()
rclpy.shutdown()
osc_terminate()
if __name__ == '__main__':
main()

74
workspace/src/osc_ros2/osc_ros2/osc_ros2_unit_quater.py → workspace/src/osc_ros2/osc_ros2/osc_ros2_rpy.py
View File

@ -369,6 +369,7 @@ class OSC_ROS2_interface(Node):
def tcp_coordinates_handler(self, *args): def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits # Ensure the desired joint positions are within the specified limits
print("tcp_coordinates_handler")
if self.robot: if self.robot:
try: try:
if len(args) == 6: if len(args) == 6:
@ -453,39 +454,38 @@ class OSC_ROS2_interface(Node):
def send_joint_positions(self): def send_joint_positions(self):
pass pass
def trapezoidal_timestamps(self, n_points, total_duration, accel_fraction=0.2): def trapezoidal_timestamps(self, num_points, total_duration, flat_ratio):
""" """
Generate timestamps for a trapezoidal velocity profile. Generate symmetric timestamps with increasing → decreasing → flat → increasing spacing pattern.
Parameters: Args:
n_points (int): Total number of trajectory points. total_duration (float): The total duration (last timestamp).
total_duration (float): Duration of the full motion [seconds]. num_points (int): Total number of timestamps (must be >= 3).
accel_fraction (float): Fraction of the total time spent accelerating and decelerating. flat_ratio (float): Fraction of timestamps in the constant-spacing center (0.0–0.9).
Default is 0.2 (i.e., 20% accel, 60% cruise, 20% decel)
Returns: Returns:
list of float: Timestamps for each point [seconds]. List[float]: List of timestamps from 0 to total_duration.
""" """
if num_points < 3:
raise ValueError("Need at least 3 points for symmetry.")
if n_points == 2: # Calculate how many are in the flat middle section
return [0.0, total_duration] flat_count = int(num_points * flat_ratio)
if flat_count % 2 == 0:
flat_count += 1 # ensure symmetry
edge_count = (num_points - flat_count) // 2
# Time fractions # Create increasing durations on the sides
ta = accel_fraction * total_duration # acceleration time edge = np.linspace(1.5, 0.5, edge_count)
tc = total_duration - 2 * ta # constant velocity time durations = np.concatenate([edge, np.full(flat_count, 0.5), edge[::-1]])
# Number of points per segment # Normalize durations so they sum to total_duration
n_accel = int(n_points * accel_fraction) durations *= total_duration / durations.sum()
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]) # Convert to timestamps
return list(timestamps) timestamps = np.cumsum(np.insert(durations, 0, 0.0))
return timestamps.tolist()
@ -495,29 +495,18 @@ class OSC_ROS2_interface(Node):
msg = JointTrajectory() msg = JointTrajectory()
msg.joint_names = self.joint_names msg.joint_names = self.joint_names
steps_per_m = 100 steps_per_m = 100
[x, y, z] = self.robot.fkine(self.current_joint_positions).t [x,y,z] = self.robot.fkine(self.current_joint_positions).t
q0 = sm.UnitQuaternion(self.robot.fkine(self.current_joint_positions).R) [roll, pitch, yaw] = self.robot.fkine(self.current_joint_positions).rpy()
x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7] x1, y1, z1, roll1, pitch1, yaw1 = self.desired[1:7]
q1 = sm.UnitQuaternion.RPY(roll1, pitch1, yaw1) #self.prev_pose = self.desired[1:]
steps = int(np.linalg.norm(np.array([x1, y1, z1])- self.robot.fkine(self.current_joint_positions).t) * steps_per_m)
if steps < 2: steps = 2
cart_traj = [sm.SE3([x+(x1-x)/(steps-1)*i, y+(y1-y)/(steps-1)*i, z+(z1-z)/(steps-1)*i]) * sm.SE3.RPY([roll+(roll1-roll)/(steps-1)*i, pitch+(pitch1-pitch)/(steps-1)*i, yaw+(yaw1-yaw)/(steps-1)*i]) for i in range(steps)]
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]: if self.desired[-1]:
timestamps = self.trapezoidal_timestamps(steps, self.desired[-1]) timestamps = self.trapezoidal_timestamps(steps, self.desired[-1], 0.3)
for j in range(steps): 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') 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: if sol[1] == 1:
@ -756,7 +745,6 @@ def main():
break break
print("Invalid input. Please enter 'y' or 'n'.") print("Invalid input. Please enter 'y' or 'n'.")
node = OSC_ROS2_interface(joint_names, joint_velocity_limits, robot, cost_mask) 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 # Run ROS 2 spin, and osc_process will be handled by the timer

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

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