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

AS: connection with Pd

Browse Source
This commit is contained in:
Alexander Schaefer
2025-04-07 19:20:31 +02:00
parent c95e0f4c0c
commit e00fa0a38a
30 changed files with 127 additions and 635 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,21 +1,13 @@
AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
COLCON=1
COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root
HOSTNAME=3230bc57d699
HOSTNAME=0e38e264ac6b
LANG=C.UTF-8
LC_ALL=C.UTF-8
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
OLDPWD=/BA/workspace/src
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
OLDPWD=/
PATH=/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/BA/workspace/build/joint_control
PYTHONPATH=/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:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/install/joint_control/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
ROS_DISTRO=humble
ROS_LOCALHOST_ONLY=0
ROS_PYTHON_VERSION=3
ROS_VERSION=2
SHLVL=1
TERM=xterm
_=/usr/bin/colcon
_colcon_cd_root=/opt/ros/foxy/

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

@@ -1,9 +1,20 @@
package.xml
setup.cfg
setup.py
../../build/joint_control/joint_control.egg-info/PKG-INFO
../../build/joint_control/joint_control.egg-info/SOURCES.txt
../../build/joint_control/joint_control.egg-info/dependency_links.txt
../../build/joint_control/joint_control.egg-info/entry_points.txt
../../build/joint_control/joint_control.egg-info/requires.txt
../../build/joint_control/joint_control.egg-info/top_level.txt
../../build/joint_control/joint_control.egg-info/zip-safe
joint_control/__init__.py
joint_control/cart_tcp_server.py
joint_control/joint_angles_server.py
joint_control/plugdata.py
joint_control/plugdata2.py
joint_control/plugdata3.py
joint_control/test.py
joint_control/trajectory_server.py
joint_control/trajectory_server_cart.py
joint_control.egg-info/PKG-INFO

4
workspace/build/joint_control/joint_control.egg-info/entry_points.txt
View File

@@ -1,6 +1,10 @@
[console_scripts]
cart_coords = joint_control.cart_tcp_server:main
joint_control = joint_control.joint_angles_server:main
plugdata = joint_control.plugdata:main
plugdata2 = joint_control.plugdata2:main
plugdata3 = joint_control.plugdata3:main
test = joint_control.test:main
trajectory_server = joint_control.trajectory_server:main
trajectory_server_cart = joint_control.trajectory_server_cart:main

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

Binary file not shown.

7
workspace/build/joint_info/colcon_command_prefix_setup_py.sh.env
View File

@@ -1,8 +1,8 @@
AMENT_PREFIX_PATH=/BA/workspace/install/joint_info:/BA/workspace/install/joint_control:/opt/ros/humble
AMENT_PREFIX_PATH=/opt/ros/humble:/BA/workspace/install/mock_robot:/BA/workspace/install/joint_info:/BA/workspace/install/joint_control
COLCON=1
COLCON_PREFIX_PATH=/BA/workspace/install
HOME=/root
HOSTNAME=3230bc57d699
HOSTNAME=0e38e264ac6b
LANG=C.UTF-8
LC_ALL=C.UTF-8
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
@@ -10,7 +10,7 @@ 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
OLDPWD=/BA/workspace/src
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
PWD=/BA/workspace/build/joint_info
PYTHONPATH=/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:/BA/workspace/src/build/joint_info:/BA/workspace/src/install/joint_info/lib/python3.10/site-packages:/BA/workspace/src/build/joint_control:/BA/workspace/src/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=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-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
ROS_DISTRO=humble
ROS_LOCALHOST_ONLY=0
ROS_PYTHON_VERSION=3
@@ -18,4 +18,3 @@ ROS_VERSION=2
SHLVL=1
TERM=xterm
_=/usr/bin/colcon
_colcon_cd_root=/opt/ros/foxy/

7
workspace/build/joint_info/joint_info.egg-info/SOURCES.txt
View File

@@ -1,6 +1,13 @@
package.xml
setup.cfg
setup.py
../../build/joint_info/joint_info.egg-info/PKG-INFO
../../build/joint_info/joint_info.egg-info/SOURCES.txt
../../build/joint_info/joint_info.egg-info/dependency_links.txt
../../build/joint_info/joint_info.egg-info/entry_points.txt
../../build/joint_info/joint_info.egg-info/requires.txt
../../build/joint_info/joint_info.egg-info/top_level.txt
../../build/joint_info/joint_info.egg-info/zip-safe
joint_info/__init__.py
joint_info/osc_joint_states_pub.py
joint_info/osc_joint_states_sub.py

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

Binary file not shown.

5
workspace/install/setup.bash
View File

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

3
workspace/install/setup.ps1
View File

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

6
workspace/install/setup.sh
View File

@@ -29,12 +29,6 @@ _colcon_prefix_chain_sh_source_script() {
fi
}
# source chained prefixes
# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
COLCON_CURRENT_PREFIX="/opt/ros/humble"
_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
# source this prefix
# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"

5
workspace/install/setup.zsh
View File

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

2
workspace/log/latest_build
View File

@@ -1 +1 @@
build_2025-03-24_10-47-50
build_2025-04-07_10-59-55

32
workspace/src/joint_angles.py
View File

@@ -1,12 +1,13 @@
from osc4py3.as_eventloop import *
from osc4py3 import oscbuildparse
import time
def main():
# Start the OSC system
osc_startup()
# Make client channels to send packets
osc_udp_client("127.0.0.1", 8000, "osc_client")
osc_udp_client("172.18.0.3", 8000, "osc_client")
# Example joint positions to send
@@ -17,12 +18,33 @@ def main():
joint_positions5 = [-0.5,-0.6, 0.2,0.0, 0.0, 0.0]
msg = oscbuildparse.OSCMessage("/joint_trajectory", None, [joint_positions1, joint_positions2, joint_positions3, joint_positions4, joint_positions5])
print("Sending joint positions")
# Send the OSC message
osc_send(msg, "osc_client")
print("Sent joint positions")
osc_process()
print("Sending joint positions")
'''
time.sleep(2)
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions2)
osc_send(msg, "osc_client")
osc_process()
print("Sent joint positions2")
time.sleep(3)
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions3)
osc_send(msg, "osc_client")
osc_process()
print("Sent joint positions3")
time.sleep(3)
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions4)
osc_send(msg, "osc_client")
osc_process()
print("Sent joint positions4")
time.sleep(3)
msg = oscbuildparse.OSCMessage("/tcp_coordinates", None, joint_positions5)
osc_send(msg, "osc_client")
osc_process()
print("Sent joint positions5")
time.sleep(3)
'''
osc_terminate()
if __name__ == "__main__":
main()
main()

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

Binary file not shown.

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

Binary file not shown.

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

Binary file not shown.

6
workspace/src/joint_control/joint_control/cart_tcp_server.py
View File

@@ -33,7 +33,7 @@ class ScaledJointTrajectoryPublisher(Node):
def tcp_coordinates_handler(self, *args):
"""Handles incoming OSC messages for tcp position."""
#time1 = time.time()
time1 = time.time()
if len(args) == len(self.joint_positions):
x, y, z, roll, pitch, yaw = args
duration = 4.0 # Default duration
@@ -62,10 +62,10 @@ class ScaledJointTrajectoryPublisher(Node):
if sol[1]:
joint_positions = list(sol[0])
self.send_trajectory(joint_positions, duration)
print(f"Computed joint positions: {joint_positions}")
#print(f"Computed joint positions: {joint_positions}")
else:
print("Inverse kinematics failed")
#print(f"Frequency: {1/(time.time() - time1)} Hz")
print(f"Frequency: {1/(time.time() - time1)} Hz")
def send_trajectory(self, joint_positions, duration=4.0):
"""Publish a joint trajectory command to move the robot."""

11
workspace/src/joint_control/joint_control/joint_angles_server.py
View File

@@ -29,8 +29,10 @@ class ScaledJointTrajectoryPublisher(Node):
def joint_angles_handler(self, *args):
"""Handles incoming OSC messages for joint positions."""
print(args)
if len(args) == len(self.joint_positions):
self.joint_positions = args
self.joint_positions = list(args)
print(self.joint_positions)
self.send_trajectory(self.joint_positions)
elif len(args) == len(self.joint_positions) + 1:
self.joint_positions = args[:-1]
@@ -39,11 +41,12 @@ class ScaledJointTrajectoryPublisher(Node):
def send_trajectory(self, joint_positions, duration=3.0):
def send_trajectory(self, joint_positions, duration=0.01):
"""Publish a joint trajectory command to move the robot."""
msg = JointTrajectory()
msg.joint_names = self.joint_names
point = JointTrajectoryPoint()
joint_positions = [float(joint) for joint in joint_positions]
point.positions = joint_positions # Updated joint positions
point.time_from_start.sec = int(duration)
point.time_from_start.nanosec = int((duration - int(duration)) * 1e9)
@@ -55,8 +58,8 @@ class ScaledJointTrajectoryPublisher(Node):
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
tree = ET.parse('/BA/robot.urdf')
robot_urdf = input("Enter the path to the URDF file: ")
tree = ET.parse(robot_urdf)
root = tree.getroot()
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']

75
workspace/src/joint_control/joint_control/trajectory_server_cart.py
View File

@@ -15,20 +15,26 @@ class ScaledJointTrajectoryPublisher(Node):
super().__init__('scaled_joint_trajectory_publisher')
self.robot = robot
self.trajectroy_topic_name = input("Enter the topic name to which the joint trajectory should be sent to: ")
if self.trajectroy_topic_name == "":
self.trajectroy_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
# ROS2 Publisher
self.publisher = self.create_publisher(
JointTrajectory,
'/scaled_joint_trajectory_controller/joint_trajectory',
self.trajectroy_topic_name,
10
)
# Store received joint positions
self.joint_names = joint_names
self.port = 8000 # UDP port
osc_startup()
osc_udp_server("0.0.0.0", 8000, "osc_server")
print("Server started on 0.0.0.0:8000 \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
osc_udp_server("0.0.0.0", self.port, "osc_server")
print(f"Server started on 0.0.0.0:{str(self.port)} \n ready to receive messages in the following format: /joint_trajectroy [tcp_coordinates0, tcp_coordinates1, ...] optional: timestamp as last element of each tcp_coordinates")
# Register OSC handler
osc_method("/joint_trajectory", self.joint_angles_handler, argscheme=osm.OSCARG_DATAUNPACK)
@@ -41,26 +47,53 @@ class ScaledJointTrajectoryPublisher(Node):
joint_positions = [0.0] * len(self.joint_names)
steps = 30
vel = 0.4
if len(args[0]) == len(self.joint_names):
if True: #len(args[0]) == len(self.joint_names):
n=2.0
for i in range(len(args)-1):
print(f'i = {i}')
x, y, z, roll, pitch, yaw = args[i]
print(1)
Tep1 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
print(2)
x, y, z, roll, pitch, yaw = args[i+1]
print(3)
Tep2 = sm.SE3(x, y, z) * sm.SE3.RPY([roll, pitch, yaw], order='xyz')
print(4)
cart_traj = rtb.ctraj(Tep1, Tep2, steps)
for j in range(steps-1):
print(cart_traj)
print(5)
for j in range(steps):
print(f'j = {j}')
print(6)
sol = self.robot.ik_LM(cart_traj[j], q0=joint_positions)
dist = np.linalg.norm(cart_traj[j].t - cart_traj[j+1].t)
point = JointTrajectoryPoint()
point.positions = list(sol[0])
joint_positions = list(sol[0])
point.time_from_start.sec = int(n)
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
n+=dist/vel
n+=0.1
msg.points.append(point)
print(7)
if sol[1] == 1:
print(8)
if j == 0: dist = vel*n
else: dist = np.linalg.norm(cart_traj[j].t - cart_traj[j-1].t)
print(9)
point = JointTrajectoryPoint()
print(10)
point.positions = list(sol[0])
print(11)
joint_positions = list(sol[0])
print(12)
point.time_from_start.sec = int(n)
print(13)
point.time_from_start.nanosec = int((n - int(n)) * 1e9)
print(14)
n+=dist/vel
print(16)
msg.points.append(point)
print(17)
else: print('IK could not find a solution!')
print(18)
self.publisher.publish(msg)
print(19)
print(f"published joint positions {msg.points[-1]}")
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
'''
elif len(args[0]) == len(self.joint_names) + 1:
for i in range(len(args)):
x, y, z, roll, pitch, yaw, timetag = args[i]
@@ -72,20 +105,18 @@ class ScaledJointTrajectoryPublisher(Node):
sol = self.robot.ik_LM(Tep, q0=joint_positions)
else:
print("Invalid number or format of arguments")
self.publisher.publish(msg)
print("published joint positions")
print(f'Frequency: {round(1/(time.time()-time1),2)} Hz')
print("Invalid number or format of arguments")'''
def main():
"""Main function to get joint names and start the ROS 2 & OSC system."""
tree = ET.parse('/BA/robot.urdf')
path_to_urdf = input("Enter the path to the URDF file: ")
tree = ET.parse(path_to_urdf)
root = tree.getroot()
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']
robot = rtb.ERobot.URDF('/BA/robot.urdf')
robot = rtb.ERobot.URDF(path_to_urdf)
print(robot)
rclpy.init()
node = ScaledJointTrajectoryPublisher(joint_names, robot)

4
workspace/src/joint_control/setup.py
View File

@@ -25,6 +25,10 @@ setup(
'cart_coords = joint_control.cart_tcp_server:main',
'trajectory_server = joint_control.trajectory_server:main',
'trajectory_server_cart = joint_control.trajectory_server_cart:main',
'plugdata = joint_control.plugdata:main',
'plugdata2 = joint_control.plugdata2:main',
'test=joint_control.test:main',
'plugdata3 = joint_control.plugdata3:main',
],
},
)