[AN] (feat) Interface with config fully works
This commit is contained in:
Ali
parent
6db7143bd5
commit
4ab2ff3c3b
19
examples/log.py
Normal file
19
examples/log.py
Normal file
@ -0,0 +1,19 @@
|
||||
import socket
|
||||
from ssl import SOCK_STREAM
|
||||
from oscpy import *
|
||||
|
||||
UDP_IP = "172.18.0.1"
|
||||
UDP_PORT = 5005
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.bind((UDP_IP, UDP_PORT))
|
||||
|
||||
while True:
|
||||
data, addr = sock.recvfrom(10*1024)
|
||||
if "#bundle" in data[:8].decode():
|
||||
b = Bundle(data)
|
||||
print(b)
|
||||
else:
|
||||
m = Message(data)
|
||||
print(m)
|
||||
|
||||
45
examples/move.py
Normal file
45
examples/move.py
Normal file
@ -0,0 +1,45 @@
|
||||
import socket
|
||||
import time
|
||||
from oscpy import *
|
||||
|
||||
|
||||
SEND_IP = "172.18.0.2"
|
||||
SEND_PORT = 8000
|
||||
RECV_IP = "172.18.0.1"
|
||||
RECV_PORT = 7000
|
||||
|
||||
pub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
|
||||
msg = Message("/joint_positions", [-1]*6).encode()
|
||||
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
|
||||
if n > 0:
|
||||
print(f"INFO: Sent {n} bytes")
|
||||
|
||||
time.sleep(10)
|
||||
|
||||
msg = Message("/tcp_coordinates", [-1]*6).encode()
|
||||
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
|
||||
if n > 0:
|
||||
print(f"INFO: Sent {n} bytes")
|
||||
|
||||
time.sleep(10)
|
||||
|
||||
sub.bind((RECV_IP, RECV_PORT))
|
||||
joint_names = []
|
||||
found = False
|
||||
while not found:
|
||||
data, addr = sub.recvfrom(5*1024)
|
||||
if "#bundle" in data[:8].decode():
|
||||
b = Bundle().decode(data)
|
||||
for msg in b.msgs:
|
||||
if "/joint_state/name" in msg.address:
|
||||
joint_names = msg.args
|
||||
found = True
|
||||
|
||||
if joint_names:
|
||||
msg = Message(f"/joint_position/{joint_names[2]}", [ 0 ])
|
||||
n = pub.sendto(msg.encode(), (SEND_IP, SEND_PORT))
|
||||
if n > 0:
|
||||
print(f"INFO: Sent {n} bytes")
|
||||
|
||||
138
examples/oscpy/__init__.py
Normal file
138
examples/oscpy/__init__.py
Normal file
@ -0,0 +1,138 @@
|
||||
import struct
|
||||
from typing import final, override
|
||||
|
||||
OSC_TYPES = str | int | float | bool | bytes
|
||||
|
||||
def get_string_size(data: bytes) -> int:
|
||||
n = data.find(b'\x00')
|
||||
n = n + 4 - n%4
|
||||
return n
|
||||
|
||||
def to_osc_string(s: str) -> bytes:
|
||||
n = len(s)
|
||||
b = struct.pack(f'{n}s{4 - n%4}x', s.encode())
|
||||
return b
|
||||
|
||||
def to_osc_blob(b: bytes) -> bytes:
|
||||
n = len(b)
|
||||
b = struct.pack(f'i{4 - n%4}p', n, b)
|
||||
return b
|
||||
|
||||
def parse_string(data: bytes) -> tuple[bytes, str]:
|
||||
n = get_string_size(data)
|
||||
values: tuple[bytes] = struct.unpack(f'>{n}s', data[:n])
|
||||
value = values[0].split(b"\x00", 1)
|
||||
return data[n:], value[0].decode()
|
||||
|
||||
def parse_float(data: bytes) -> tuple[bytes, float]:
|
||||
values: tuple[float] = struct.unpack('>f', data[:4])
|
||||
return data[4:], values[0]
|
||||
|
||||
def parse_int(data: bytes) -> tuple[bytes, int]:
|
||||
values: tuple[int] = struct.unpack('>i', data[:4])
|
||||
return data[4:], values[0]
|
||||
|
||||
def parse_blob(data: bytes) -> tuple[bytes, bytes]:
|
||||
n_values: tuple[int] = struct.unpack('>i', data[:4])
|
||||
n: int = n_values[0]
|
||||
values: tuple[bytes] = struct.unpack(f'>{n}p', data[4:4+n])
|
||||
return data[4+n:], values[0]
|
||||
|
||||
def parse_args(tt: str, data: bytes) -> list[OSC_TYPES]:
|
||||
tt = tt[1:]
|
||||
args: list[OSC_TYPES] = []
|
||||
for c in tt:
|
||||
match c:
|
||||
case 's':
|
||||
data, val = parse_string(data)
|
||||
args.append(val)
|
||||
case 'b':
|
||||
data, val = parse_blob(data)
|
||||
args.append(val)
|
||||
case 'f':
|
||||
data, val = parse_float(data)
|
||||
args.append(val)
|
||||
case 'i':
|
||||
data, val = parse_int(data)
|
||||
args.append(val)
|
||||
case _:
|
||||
print(f"[ERROR]: Got {c}")
|
||||
return args
|
||||
|
||||
def encode_args(args: list[OSC_TYPES]) -> bytes:
|
||||
encoded = b''
|
||||
for arg in args:
|
||||
match arg:
|
||||
case str(): encoded += to_osc_string(arg)
|
||||
case float(): encoded += struct.pack('>f', arg)
|
||||
case int(): encoded += struct.pack('>i', arg)
|
||||
case bytes(): encoded += to_osc_blob(arg)
|
||||
return encoded
|
||||
|
||||
def parse_type(arg: OSC_TYPES) -> str:
|
||||
match arg:
|
||||
case str(): return "s"
|
||||
case float(): return "f"
|
||||
case int(): return "i"
|
||||
case bytes(): return "b"
|
||||
|
||||
@final
|
||||
class Message:
|
||||
def __init__(self, address: str = "/", args: list[OSC_TYPES] = []):
|
||||
self.address: str = address
|
||||
self.type_tags = ","
|
||||
for arg in args:
|
||||
self.type_tags = self.type_tags + parse_type(arg)
|
||||
self.args: list[OSC_TYPES] = args
|
||||
|
||||
def decode(self, data: bytes):
|
||||
data, self.address = parse_string(data)
|
||||
data, self.type_tags = parse_string(data)
|
||||
self.args = parse_args(self.type_tags, data)
|
||||
return self
|
||||
|
||||
def encode(self) -> bytes:
|
||||
msg = to_osc_string(self.address)
|
||||
msg += to_osc_string(self.type_tags)
|
||||
msg += encode_args(self.args)
|
||||
return msg
|
||||
|
||||
@override
|
||||
def __str__(self) -> str:
|
||||
return f"{self.address} [{self.type_tags}]: {self.args}"
|
||||
|
||||
@final
|
||||
class Bundle:
|
||||
def __init__(self, time_tag: float = 0.0, msgs: list[Message] = []):
|
||||
self.header = "#bundle"
|
||||
self.ttag = time_tag
|
||||
self.msgs = msgs
|
||||
|
||||
def decode(self, data: bytes):
|
||||
data, self.header = parse_string(data)
|
||||
data[4:]
|
||||
data, self.ttag = parse_float(data)
|
||||
data = data[4:]
|
||||
self.msgs: list[Message] = []
|
||||
while len(data) > 0:
|
||||
data, n = parse_int(data)
|
||||
msg_data = data[:n]
|
||||
data = data[n:]
|
||||
self.msgs.append(Message().decode(msg_data))
|
||||
return self
|
||||
|
||||
def encode(self) -> bytes:
|
||||
bundle = to_osc_string("#bundle")
|
||||
bundle += struct.pack('4xf4x', self.ttag)
|
||||
for msg in self.msgs:
|
||||
msg_data = msg.encode()
|
||||
bundle += struct.pack('i', len(msg_data))
|
||||
bundle += msg_data
|
||||
return bundle
|
||||
|
||||
@override
|
||||
def __str__(self) -> str:
|
||||
out = f"{self.header} ({self.ttag}):\n"
|
||||
for msg in self.msgs:
|
||||
out += f" - {msg}\n"
|
||||
return out
|
||||
27
examples/state.py
Normal file
27
examples/state.py
Normal file
@ -0,0 +1,27 @@
|
||||
import socket
|
||||
import struct
|
||||
from oscpy import *
|
||||
|
||||
UDP_IP = "172.18.0.1"
|
||||
UDP_PORT = 7000
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.bind((UDP_IP, UDP_PORT))
|
||||
|
||||
|
||||
while True:
|
||||
data, addr = sock.recvfrom(10*1024)
|
||||
n = get_string_size(data)
|
||||
[tag] = struct.unpack(f'{n}s', data[:n])
|
||||
if "#bundle" in tag.decode():
|
||||
b = Bundle(data)
|
||||
print("Bundle:")
|
||||
for msg in b.msgs:
|
||||
print(" -", msg)
|
||||
else:
|
||||
msg = Message(data)
|
||||
if msg.address == "/time":
|
||||
msg.args[0] = float(msg.args[0])
|
||||
print(msg)
|
||||
|
||||
|
||||
7
examples/test.py
Normal file
7
examples/test.py
Normal file
@ -0,0 +1,7 @@
|
||||
from oscpy import *
|
||||
|
||||
|
||||
encoded = Message("/", ["test", 0.1, 9]).encode()
|
||||
new_msg = Message().decode(encoded).encode()
|
||||
assert(encoded == new_msg)
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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]")
|
||||
|
||||
Loading…
Reference in New Issue
Block a user