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

[AN] (feat) Interface with config fully works

Browse Source
This commit is contained in:
Ali
2025-06-08 22:13:18 +02:00
parent 6db7143bd5
commit 4ab2ff3c3b
7 changed files with 275 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
workspace/src/osc_ros2/osc_ros2/interactive_input.py
View File

@@ -68,15 +68,15 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot]:
print('-+'*50)
print("The cost mask determines which coordinates are used for the IK.\nEach element of the cost mask corresponds to a Cartesian coordinate [x, y, z, roll, pitch, yaw].")
print("The cost mask 111000 means that the IK will only consider translation and no rotaion.")
rconf["cost_mask_string"] = input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")
cost_mask = [int(i) for i in rconf["cost_mask_string"]]
if sum(cost_mask) <= robot.n and len(cost_mask) == 6:
cost_mask_string = input(f"Enter the cost mask (6 integers (1 or 0), of which <= {robot.n} are 1): ")
rconf["cost_mask"] = [int(i) for i in cost_mask_string]
if sum(rconf["cost_mask"]) <= robot.n and len(rconf["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"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if cost_mask[i]==1]}")
print(f"The following coordinates will be used for the IK: {[j for i,j in enumerate(['x','y','z','roll','pitch','yaw']) if rconf['cost_mask'][i]==1]}")
break
elif use_urdf == 'n':
node = JointNameListener()
@@ -140,7 +140,7 @@ def interactive_input() -> tuple[dict[str, Any], rtb.robot]:
except ValueError:
print("Invalid input. Please enter numeric values or leave blank to skip.")
robot = None
cost_mask = None
rconf["cost_mask"] = None
break
print("Invalid input. Please enter 'y' or 'n'.")
if robot:

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

@@ -23,27 +23,37 @@ class OSC_ROS2_interface(Node):
def __init__(self, robot, config):
super().__init__('scaled_joint_trajectory_publisher')
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.joint_names = config["robot"]["joint_names"]
self.joint_velocity_limits = config["robot"]["joint_velocity_limits"]
self.cost_mask = config["robot"]["cost_mask"]
self.trajectory_topic_name = config["robot"]["trajectory_topic_name"]
self.hz = config["robot"]["hz"]
self.state_ip = config["network"]["state_ip"]
self.state_port = config["network"]["state_port"]
self.log_ip = config["network"]["log_ip"]
self.log_port = config["network"]["log_port"]
self.commands_port = config["network"]["commands_port"]
float_or_none = lambda v: None if "None" in v else float(v)
self.x_limits = list(map(float_or_none, config["robot"]["x_limits"]))
self.y_limits = list(map(float_or_none, config["robot"]["y_limits"]))
self.z_limits = list(map(float_or_none, config["robot"]["z_limits"]))
self.x_limits_workspace = list(map(float_or_none, config["robot"]["x_limits_workspace"]))
self.y_limits_workspace = list(map(float_or_none, config["robot"]["y_limits_workspace"]))
self.z_limits_workspace = list(map(float_or_none, config["robot"]["z_limits_workspace"]))
self.robot = robot
self.desired = None
self.previous_desired = None
@@ -56,9 +66,7 @@ class OSC_ROS2_interface(Node):
}
self.speed_scaling = 0.2
self.new = False
self.n_joints = len(joint_names)
self.n_joints = len(self.joint_names)
# ROS2 Publisher
self.publisher = self.create_publisher(
JointTrajectory,
@@ -66,10 +74,9 @@ class OSC_ROS2_interface(Node):
1
)
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")
osc_udp_client(self.state_ip, self.state_port, "osc_client")
osc_udp_client(self.log_ip, self.log_port, "osc_log_client")
osc_udp_server('0.0.0.0', self.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_ADDRESS+osm.OSCARG_DATAUNPACK)
@@ -77,11 +84,10 @@ class OSC_ROS2_interface(Node):
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)
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}')
self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{self.commands_port}')
self.get_logger().info(f'Sending joint states to {self.state_ip}:{self.state_port}')
self.get_logger().info(f'Sending log messages to {self.log_ip}:{self.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
@@ -105,17 +111,16 @@ class OSC_ROS2_interface(Node):
def joint_trajectory_handler(self, *args):
try:
if len(args[0]) == 6:
points = [[float(j) for j in i] for i in args]
elif len(args[0]) >= 7:
points = [[float(j) for j in i[:6]] for i in args]
self.get_logger().warn("joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
else:
self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.")
return
self.desired = ["joint_trajectory"] + points
self.new = True
if len(args[0]) == 6:
points = [[float(j) for j in i] for i in args]
elif len(args[0]) >= 7:
points = [[float(j) for j in i[:6]] for i in args]
self.get_logger().warn("joint_trajectory_handler: Duration is not supported for joint trajectory yet. Ignoring duration.")
else:
self.get_logger().warn(f"joint_trajectory_handler: Invalid number of arguments for joint trajectory. Expected {self.n_joints} ([q0, q1, q2, ..., q{self.n_joints}]) or {self.n_joints+1} ([q0, q1, q2, ..., q{self.n_joints}, duration]), but got {len(args[0])}.")
return
self.desired = ["joint_trajectory"] + points
self.new = True
except Exception as e:
self.get_logger().fatal(f"joint_trajectory_handler: {e}")
@@ -123,7 +128,6 @@ class OSC_ROS2_interface(Node):
"""Handles incoming OSC messages for joint positions."""
try:
joint_name = address.split("/")[-1]
if joint_name in self.joint_names:
if len(args) == 1:
position = float(args[0])
@@ -190,7 +194,6 @@ class OSC_ROS2_interface(Node):
self.get_logger().warn(f"joint_position_handler: Joint '{joint_name}' not found in the robot model.")
except Exception as e:
self.get_logger().fatal(f"joint_position_handler: {e}")
def cartesian_trajectory_handler(self, *args):
"""Handles incoming OSC messages for cartesian trajectory."""
@@ -204,7 +207,6 @@ class OSC_ROS2_interface(Node):
else:
self.get_logger().warn("cartesian_trajectory_handler: Invalid number of arguments for cartesian trajectory. Expected 6 ([x, y, z, roll, pitch, yaw]) or 7 ([x, y, z, roll, pitch, yaw, duration]), but got {len(args[0])}.")
return
self.desired = ["cartesian_trajectory"] + points
self.new = True
except Exception as e:
@@ -223,7 +225,6 @@ class OSC_ROS2_interface(Node):
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
if self.robot:
for i, position in enumerate(desired_joint_positions[:len(self.joint_names)]): # Exclude duration if present
@@ -252,7 +253,6 @@ class OSC_ROS2_interface(Node):
self.get_logger().warn(
f"joint_positions_handler: Joint '{self.joint_names[i]}' position {position} is out of bounds. Using {self.joint_lim[1][i]}."
)
self.desired = ["joint_positions"] + desired_joint_positions
self.new = True
except Exception as e:
@@ -271,7 +271,6 @@ class OSC_ROS2_interface(Node):
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:
@@ -284,13 +283,11 @@ class OSC_ROS2_interface(Node):
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:
@@ -299,7 +296,6 @@ class OSC_ROS2_interface(Node):
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:
@@ -311,11 +307,9 @@ class OSC_ROS2_interface(Node):
self.current_joint_positions = [float(joint_position_dict[name]) for name in self.joint_names]
joint_position_dict = dict(zip(msg.name, msg.velocity))
self.current_joint_velocities = [float(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("/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]])
@@ -326,21 +320,18 @@ class OSC_ROS2_interface(Node):
#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")
osc_send(msg_tcp, "osc_client")
msg_position = oscbuildparse.OSCMessage("/joint_state/position", f',{"f"*self.n_joints}', [i for i in msg.position])
msg_velocity = oscbuildparse.OSCMessage("/joint_state/velocity", f',{"f"*self.n_joints}', [i for i in msg.velocity])
msg_effort = oscbuildparse.OSCMessage("/joint_state/effort", f',{"f"*self.n_joints}', [i for i in msg.effort])
msg_name = oscbuildparse.OSCMessage("/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}")
@@ -385,21 +376,17 @@ class OSC_ROS2_interface(Node):
return
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:
@@ -460,9 +447,6 @@ class OSC_ROS2_interface(Node):
# 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)
for j in range(steps):
@@ -504,9 +488,6 @@ class OSC_ROS2_interface(Node):
msg.header.stamp = self.get_clock().now().to_msg()
self.publisher.publish(msg)
self.previous_desired = self.desired
else:
prev_duration = 0
'''
@@ -566,7 +547,6 @@ class OSC_ROS2_interface(Node):
self.get_logger().fatal(f"send_tcp_coordinates: {e}")
def send_joint_trajectory(self):
try:
self.new = False
viapoints = np.array([i for i in self.desired[1:]])
@@ -587,7 +567,6 @@ class OSC_ROS2_interface(Node):
print(f'Error in joint_angles_handler: {e}')
def send_cartesian_trajectory(self):
try:
self.new = False
viapoints = np.array([i[:6] for i in self.desired[1:]])
@@ -620,7 +599,6 @@ class OSC_ROS2_interface(Node):
self.previous_desired = None
except Exception as e:
print(f'Error in joint_angles_handler: {e}')
self.previous_desired = None
def update_position(self):
@@ -628,7 +606,6 @@ class OSC_ROS2_interface(Node):
try:
if self.desired is None or not(self.new):
return
if self.desired[0] == "joint_positions":
self.new = False
self.send_joint_positions()
@@ -648,42 +625,31 @@ class OSC_ROS2_interface(Node):
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."""
parser = argparse.ArgumentParser(description="An interface between ROS2 and OSC for robotic arm manipulators", usage="ros2 run osc_ros2 interface [-c CONFIG.TOML]")