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

AS: code update

Browse Source
This commit is contained in:
Alexander Schaefer 2025-05-07 23:12:12 +02:00
parent e8d48c0a4d
commit b67b2a5174
7 changed files with 151 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
osc_recieve.py
View File

@ -6,7 +6,7 @@ def handler(address,*args):
osc_startup()
osc_udp_server("0.0.0.0", 7000, "osc_server")
osc_method("/joint_states/counter", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
osc_method("/time", handler, argscheme=osm.OSCARG_ADDRESS+ osm.OSCARG_DATAUNPACK)
while True:

29
test/collision.py
View File

@ -1,29 +0,0 @@
import pybullet as p
# Start PyBullet simulation
physicsClient = p.connect(p.DIRECT) # or p.GUI for graphical interface
# Load your robot URDF
robot_id = p.loadURDF("/ur10e.urdf", useFixedBase=True)
# Set joint angles
joint_angles = [0, 0, 3.141/2, 0, 0, 0]
for i, angle in enumerate(joint_angles):
p.setJointMotorControl2(robot_id, i, p.POSITION_CONTROL, targetPosition=angle)
# Step the simulation to update the robot's state
p.stepSimulation()
# Check if any link is colliding
collision = False
for i in range(p.getNumBodies()):
contact_points = p.getContactPoints(robot_id)
print(contact_points)
if len(contact_points) > 0:
collision = True
break
if collision:
print("The robot has collided with itself.")
else:
print("No collision detected.")

26
test/commands_recording.py Normal file
View File

@ -0,0 +1,26 @@
from pythonosc.dispatcher import Dispatcher
from pythonosc import osc_server
import csv
import time
# CSV file setup
csv_file = open("osc_log.csv", mode="w", newline="")
csv_writer = csv.writer(csv_file)
csv_writer.writerow(["timestamp", "x", "y", "z", "roll", "pitch", "yaw"])
# Handler for OSC messages
def handle_pose(address, *args):
timestamp = time.time()
csv_writer.writerow([timestamp] + list(args))
csv_file.flush() # Optional: ensures data is written immediately
# Setup dispatcher and bind address
dispatcher = Dispatcher()
dispatcher.map("/tcp_coordinates", handle_pose) # Accept messages sent to address /pose
# Start server
ip = "0.0.0.0"
port = 8000
server = osc_server.ThreadingOSCUDPServer((ip, port), dispatcher)
print(f"Listening for OSC messages on {ip}:{port}...")
server.serve_forever()

14
test/get_ip.py Normal file
View File

@ -0,0 +1,14 @@
import socket
def get_ip_address():
try:
# Connect to an external address — no data is actually sent
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
s.connect(("8.8.8.8", 80)) # Google's DNS
ip = s.getsockname()[0]
s.close()
return ip
except Exception:
return "127.0.0.1" # fallback to localhost
print("Local IP address:", get_ip_address())

31
test/ros2_ws/src/joint_states_control/move_scaled_joint.py
View File

@ -12,28 +12,31 @@ class ScaledJointTrajectoryPublisher(Node):
10
)
def send_trajectory(self, joint_positions, duration=2.0):
"""Publish a joint trajectory command to move the UR10e."""
msg = JointTrajectory()
msg.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
try:
msg = JointTrajectory()
msg.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
point = JointTrajectoryPoint()
point.positions = joint_positions # Target joint positions
point.time_from_start.sec = int(duration) # Duration in seconds
point = JointTrajectoryPoint()
point.positions = [2.0]*6 # Target joint positions
point.velocities = [2.0] * 6
point.accelerations = [2.0] * 6
#point.time_from_start.sec = int(2)
#point.time_from_start.nanosec = int(0)
msg.points.append(point)
msg.points.append(point)
self.publisher.publish(msg)
self.publisher.publish(msg)
except Exception as e:
print(f"Error publishing joint trajectory: {e}")
def main():
rclpy.init()
node = ScaledJointTrajectoryPublisher()
# Move to a target position
node.send_trajectory([0.0]*6)
rclpy.spin(node)
node.destroy_node()

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

Binary file not shown.

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

@ -13,6 +13,7 @@ from osc4py3 import oscbuildparse
import time
import os
import re
import socket
class JointNameListener(Node):
def __init__(self):
@ -35,11 +36,25 @@ 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,
'/scaled_joint_trajectory_controller/joint_trajectory',
self.trajectory_topic_name,
1
)
@ -73,6 +88,7 @@ class OSC_ROS2_interface(Node):
50: "FATAL",
}
self.speed_scaling = 0.2
self.new = False
while True:
@ -182,9 +198,29 @@ class OSC_ROS2_interface(Node):
continue
if lower_limit:
lim[0][i] = float(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:
lim[1][i] = float(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)
@ -245,10 +281,11 @@ class OSC_ROS2_interface(Node):
# 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("/joint_position/*", self.joint_position_handler, argscheme=osm.OSCARG_DATAUNPACK)
osc_method("/tcp_coordinates", self.tcp_coordinates_handler, argscheme=osm.OSCARG_DATAUNPACK)
osc_method("/joint_trajectory", self.joint_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK)
osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK)
print('--' * 50)
self.hz = float(input("Enter the desired refresh frequency (Hz): "))
print()
@ -262,7 +299,7 @@ class OSC_ROS2_interface(Node):
print()
print('=-=' * 50)
print()
print(f'Ready to receive OSC messages on 0.0.0.0:{commands_port}')
print(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}')
print()
print('=-=' * 50)
print()
@ -272,9 +309,26 @@ class OSC_ROS2_interface(Node):
self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}')
self.create_timer(1/self.hz, self.update_position) # Timer to update the position
def speed_scaling_handler(self, *args):
"""Handles incoming OSC messages for speed scaling."""
try:
if len(args) == 1:
if args[0] < 0:
self.speed_scaling = -float(args[0])
else:
self.speed_scaling = float(args[0])
self.get_logger().info(f"Speed scaling set to {self.speed_scaling}")
else:
self.get_logger().warn(f"Invalid number of arguments for speed scaling. Expected 1, but got {len(args)}.")
except Exception as e:
self.get_logger().fatal(f"speed_scaling_handler: {e}")
def joint_trajectory_handler(self, *args):
pass
def joint_position_handler(self, *args):
pass
def cartesian_trajectory_handler(self, *args):
"""Handles incoming OSC messages for cartesian trajectory."""
if self.robot:
@ -285,30 +339,34 @@ class OSC_ROS2_interface(Node):
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
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)}.")
# 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
# 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
def tcp_coordinates_handler(self, *args):
# Ensure the desired joint positions are within the specified limits
if self.robot:
@ -342,6 +400,7 @@ class OSC_ROS2_interface(Node):
)
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:
@ -352,6 +411,8 @@ class OSC_ROS2_interface(Node):
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]
@ -554,23 +615,23 @@ class OSC_ROS2_interface(Node):
def update_position(self):
"""Calls the appropriate function to update the robot's position."""
try:
if self.desired is None:
return
if self.desired == self.previous_desired:
msg_log = oscbuildparse.OSCMessage(f"/log/INFO", ',isss', [20, str(time.time()) , 'interface - update_position', "Desired joint positions are the same as previous."])
osc_send(msg_log, "osc_client")
if self.desired is None or not(self.new):
return
if self.desired[0] == "joint_positions":
self.new = False
self.send_joint_positions()
return
elif self.desired[0] == "tcp_coordinates":
self.new = False
self.send_tcp_coordinates()
return
elif self.desired[0] == "joint_trajectory":
self.new = False
self.send_joint_trajectory()
return
elif self.desired[0] == "cartesian_trajectory":
self.new = False
self.send_cartesian_trajectory()
return
except Exception as e:
@ -622,7 +683,7 @@ def main():
print("Invalid path. Please enter a valid 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']
joint_names = [link.name for link in robot.links if link.isjoint]
robot = rtb.ERobot.URDF(robot_urdf)
print(robot)
joint_velocity_limits = {}