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

AS: plugdata example added

Browse Source
This commit is contained in:
Alexander Schaefer 2025-05-15 14:08:11 +02:00
parent c7cb035d4d
commit ff8d5ee281
6 changed files with 206 additions and 105 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
workspace/.DS_Store vendored
View File

Binary file not shown.

BIN
workspace/src/.DS_Store vendored
View File

Binary file not shown.

52
workspace/src/6_DOF_example.pd Normal file
View File

@ -0,0 +1,52 @@
#N canvas 827 239 527 327 12;
#X obj 942 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X msg 1226 778 disconnect;
#X obj 989 837 netsend -u -b;
#X obj 897 787 osc.format;
#X text 954 457 x;
#X text 1001 457 y;
#X text 1046 457 z;
#X text 918 585 0;
#X text 918 484 +1;
#X text 918 686 -1;
#X obj 989 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X obj 1034 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X obj 1090 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X obj 1134 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X obj 1178 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1;
#X msg 1189 754 connect localhost 8000;
#X text 1099 457 roll;
#X text 1134 457 pitch;
#X text 1189 457 yaw;
#X msg 708 718 /speed_scaling;
#X obj 1034 768 pack s f f f f f f;
#X obj 815 487 vsl 43 215 0.1 0.9 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 0;
#X text 815 457 speed;
#X text 1222 484 +pi;
#X text 1222 580 0;
#X text 1226 686 -pi;
#X text 776 484 90%;
#X text 776 686 10%;
#X obj 764 768 pack s f;
#X msg 856 741 /tcp_coordinates;
#X connect 0 0 29 0;
#X connect 0 0 20 1;
#X connect 1 0 2 0;
#X connect 3 0 2 0;
#X connect 10 0 29 0;
#X connect 10 0 20 2;
#X connect 11 0 29 0;
#X connect 11 0 20 3;
#X connect 12 0 29 0;
#X connect 12 0 20 4;
#X connect 13 0 29 0;
#X connect 13 0 20 5;
#X connect 14 0 29 0;
#X connect 14 0 20 6;
#X connect 15 0 2 0;
#X connect 19 0 28 0;
#X connect 20 0 3 0;
#X connect 21 0 28 1;
#X connect 21 0 19 0;
#X connect 28 0 3 0;
#X connect 29 0 20 0;

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

Binary file not shown.

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

@ -27,7 +27,6 @@ class JointNameListener(Node):
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):
@ -36,27 +35,6 @@ class OSC_ROS2_interface(Node):
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,
@ -89,52 +67,8 @@ class OSC_ROS2_interface(Node):
}
self.speed_scaling = 0.2
self.new = False
self.n_joints = len(joint_names)
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:
print('+-' * 50)
@ -240,39 +174,105 @@ class OSC_ROS2_interface(Node):
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:
print('+-' * 50)
update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower()
if update_limits == 'y':
self.joint_lim = [[None]*self.n_joints,[None]*self.n_joints]
for i, joint in enumerate(self.joint_names):
while True:
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':
try:
print("-" * 50)
lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter to keep current): ").strip()
upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").strip()
if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit):
print('--' * 50)
print(" !!! Invalid input. Lower limit must be less than upper limit. !!! ")
continue
self.joint_lim[0][i] = float(lower_limit) if lower_limit!="" else None
self.joint_lim[1][i] = float(upper_limit) if upper_limit!="" else None
break
else:
print("Please re-enter the joint names.")
print('invalid input. Please enter "y" or "n".')
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to keep current limits.")
break
elif update_limits == 'n':
self.joint_lim = None
print("Invalid input. Please enter 'y' or 'n'.")
print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}')
while True:
try:
print('+-' * 50)
self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip()
if self.trajectory_topic_name == "":
self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory'
break
elif self.trajectory_topic_name.startswith("/"):
break
else:
print("Invalid topic name. A valid topic name should start with '/'.")
except Exception as e:
print(f"An error occurred: {e}")
# ROS2 Publisher
self.publisher = self.create_publisher(
JointTrajectory,
self.trajectory_topic_name,
1
)
while True:
try:
print('+-' * 50)
log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if log_ip == "":
log_ip = "127.0.0.1"
print('--' * 50)
log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ")
if log_port == "":
log_port = 5005
else:
log_port = int(log_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): "))
if state_ip == "":
state_ip = "127.0.0.1"
print('--' * 50)
state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ")
if state_port == "":
state_port = 7000
else:
state_port = int(state_port)
break
except ValueError:
print("Invalid input. Please enter a valid IP address.")
continue
while True:
try:
print('+-' * 50)
commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ")
if commands_port == state_port:
print("The commands port must be different from the state port.")
continue
if commands_port == "":
commands_port = 8000
else:
commands_port = int(commands_port)
break
except ValueError:
print("Invalid input. Please enter a valid port.")
continue
self.n_joints = len(joint_names)
osc_startup()
osc_udp_client(state_ip, state_port, "osc_client")
@ -289,7 +289,18 @@ class OSC_ROS2_interface(Node):
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): "))
while True:
try:
print('+-' * 50)
self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ")
if self.hz == "":
self.hz = 100
else:
self.hz = float(self.hz)
break
except ValueError:
print("Invalid input. Please enter a valid number.")
continue
print()
print('=-=' * 50)
print()
@ -306,6 +317,7 @@ class OSC_ROS2_interface(Node):
print('=-=' * 50)
print()
self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}")
self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}')
self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
@ -854,16 +866,53 @@ def main():
rclpy.spin_once(node)
joint_names = node.joint_names
node.destroy_node()
'''
if joint_names:
correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower()
while True:
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}
if correct_names == 'y':
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to skip.")
'''
joint_velocity_limits = None
elif correct_names == 'n':
joint_names = None
break
correct_names = input("Invalid input. Please enter 'y' or 'n'.")
if not(joint_names):
print("Please enter the joint names manually.")
while True:
joint_names = []
print('-+'*50)
print("Enter the joint names manually one by one. Type 'done' when you are finished:")
print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.")
while True:
print('-'*50)
joint_name = input("Enter joint name (or 'done' to finish): ").strip()
if joint_name.lower() == 'done':
break
if joint_name:
joint_names.append(joint_name)
print('-+'*50)
correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip()
if correct_names.lower() == 'y':
break
else:
print("Please re-enter the joint names.")
while True:
try:
joint_velocity_limits = {}
for name in joint_names:
while True:
try:
print('--'*50)
limit = input(f"Enter the velocity limit for joint '{name}': ").strip()
if limit == "":
continue
else:
joint_velocity_limits[name] = float(limit)
break
except ValueError:
print("Invalid input. Please enter a numeric value or press Enter to skip.")
break
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to skip.")
robot = None
cost_mask = None
break