AS: creating final node

test/ros_osc/joint_info/osc4py3/osc_joint_states_pub.py

@@ -0,0 +1,92 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from osc4py3.as_eventloop import *
+from osc4py3 import oscbuildparse
+
+class JointStateOSC(Node):
+    def __init__(self):
+        super().__init__('joint_states_osc')
+
+        # Create a ROS 2 subscriber to /joint_states topic
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            1  # Queue size
+        )
+
+        # Open Sound Control (OSC) Client settings
+        self.osc_ip = "127.0.0.1"   # Replace with the target IP
+        self.osc_port = 8000        # Replace with the target port
+
+        # Start the OSC system
+        osc_startup()
+
+        # Make client channels to send packets
+        osc_udp_client(self.osc_ip, self.osc_port, "osc_client")
+
+    def joint_states_callback(self, msg):
+        """Callback function to handle incoming joint states."""
+        header = msg.header
+        joint_names = msg.name
+        joint_positions = msg.position
+        joint_velocity = msg.velocity
+        joint_effort = msg.effort
+
+        joint_names_str = "\n- ".join(joint_names)
+        joint_positions_str = "\n- ".join(map(str, joint_positions))
+        joint_velocity_str = "\n- ".join(map(str, joint_velocity))
+        joint_effort_str = "\n- ".join(map(str, joint_effort))
+
+        info = f"""
+---
+header:
+    stamp:
+        sec: {header.stamp.sec}
+        nanosec: {header.stamp.nanosec}
+name:
+- {joint_names_str}
+position:
+- {joint_positions_str}
+velocity:
+- {joint_velocity_str}
+effort:
+- {joint_effort_str}
+---"""
+
+        # Send the info message
+        msg = oscbuildparse.OSCMessage("/joint_states", None, [info])
+        osc_send(msg, "osc_client")
+        osc_process()
+        #print(f"Publishing: {info}")
+
+
+        # Send each joint state as an OSC message
+        for i, name in enumerate(joint_names):
+            #msg_sec = oscbuildparse.OSCMessage(f"/joint_states/header/sec", None, [header.stamp.sec])
+            #msg_nanosec = oscbuildparse.OSCMessage(f"/joint_states/header/nanosec", None, [header.stamp.nanosec])
+            msg_position = oscbuildparse.OSCMessage(f"/joint_states/{name}/position", None, [joint_positions[i]])
+            msg_velocity = oscbuildparse.OSCMessage(f"/joint_states/{name}/velocity", None, [joint_velocity[i]])
+            msg_effort = oscbuildparse.OSCMessage(f"/joint_states/{name}/effort", None, [joint_effort[i]])
+
+            bun = oscbuildparse.OSCBundle(oscbuildparse.unixtime2timetag(header.stamp.sec + header.stamp.nanosec), [msg_position, msg_velocity, msg_effort])
+            #bun = oscbuildparse.OSCBundle(oscbuildparse.OSC_IMMEDIATELY , [msg_position, msg_velocity, msg_effort])
+            osc_send(bun, "osc_client")
+            osc_process()
+            #print(f"OSC bundle sent for joint {name}")
+
+def main():
+    rclpy.init()
+    node = JointStateOSC()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()

test/ros_osc/joint_info/osc4py3/osc_joint_states_sub.py

@@ -0,0 +1,41 @@
+from osc4py3.as_eventloop import *
+from osc4py3 import oscmethod as osm
+from osc4py3 import oscbuildparse
+import time
+
+def joint_states_handler(*args):
+    """Handler function to process incoming joint states."""
+    print(f"Received message")
+    for arg in args:
+        print(arg)
+
+def main():
+    ip = "0.0.0.0"  # IP address to listen on
+    port = 8000       # Port to listen on
+
+    # Start the OSC system
+    osc_startup()
+
+    # Make server channels to receive packets
+    osc_udp_server(ip, port, "osc_server")
+
+    # Associate Python functions with message address patterns
+    osc_method("/joint_states", joint_states_handler, argscheme=osm.OSCARG_MESSAGE)
+
+    print(f"Listening for OSC messages on {ip}:{port}...")
+
+    try:
+        # Run the event loop
+        while True:
+            osc_process()  # Process OSC messages
+            time.sleep(0.01)  # Sleep to avoid high CPU usage
+
+    except KeyboardInterrupt:
+        print("")
+
+    finally:
+        # Properly close the system
+        osc_terminate()
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/osc4py3/tcp_cart_pos.py

@@ -0,0 +1,62 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from osc4py3.as_eventloop import *
+from osc4py3 import oscbuildparse
+import roboticstoolbox as rtb
+
+class JointStateOSC(Node):
+    def __init__(self):
+        super().__init__('joint_states_osc')
+
+        # Create a ROS 2 subscriber to /joint_states topic
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            1  # Queue size
+        )
+
+        # Open Sound Control (OSC) Client settings
+        self.osc_ip = "127.0.0.1"   # Replace with the target IP
+        self.osc_port = 8000        # Replace with the target port
+
+        # Start the OSC system
+        osc_startup()
+
+        # Make client channels to send packets
+        osc_udp_client(self.osc_ip, self.osc_port, "osc_client")
+
+        # Load the robot model
+        self.robot = rtb.ERobot.URDF('BA/robot.urdf')
+
+    def joint_states_callback(self, msg):
+        """Callback function to handle incoming joint states."""
+        header = msg.header
+        joint_names = msg.name
+        joint_positions = msg.position
+
+        # Compute the forward kinematics to get the TCP position
+        tcp_pos = self.robot.fkine(joint_positions).t
+
+        # Prepare the OSC message with the TCP position
+        msg = oscbuildparse.OSCMessage("/tcp_position", None, tcp_pos.tolist())
+        osc_send(msg, "osc_client")
+        osc_process()
+        print(f"Published TCP position: {tcp_pos}")
+
+def main():
+    rclpy.init()
+    node = JointStateOSC()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        osc_terminate()
+
+if __name__ == '__main__':
+    main()

test/ros_osc/joint_info/pythonosc/bundles/bundle_server_test.py

@@ -0,0 +1,26 @@
+from pythonosc import dispatcher
+from pythonosc import osc_server
+import time
+
+def joint_handler(address, *args):
+    print(args)
+    time.sleep(0.3)
+
+def main():
+    ip = "0.0.0.0"  # Listen on all network interfaces
+    port = 5005    # Must match the sender's port in `joint_state_osc.py`
+
+    # Create dispatcher and register callback
+    disp = dispatcher.Dispatcher()
+    #disp.map("/joint_states", joint_handler)
+    disp.map("/SYNC", joint_handler)
+
+    server = osc_server.ThreadingOSCUDPServer((ip, port), disp) # Start OSC server
+    
+    try:
+        server.serve_forever()  # Keep server running
+    except KeyboardInterrupt:
+        print("")
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/bundles/bundle_timetag.py

@@ -0,0 +1,30 @@
+from pythonosc import dispatcher
+from pythonosc import osc_server
+from pythonosc.osc_bundle import OscBundle
+from pythonosc.osc_message import OscMessage
+
+def joint_handler(address, *args):
+    if isinstance(args[0], OscBundle):
+        bundle = args[0]
+        print(f"Received OSC bundle with timestamp: {bundle.timestamp}")
+        for element in bundle.bundle_elements:
+            if isinstance(element, OscMessage):
+                print(f"Message: {element.address}, Args: {element.params}")
+
+def main():
+    ip = "0.0.0.0"  # Listen on all network interfaces
+    port = 5005    # Must match the sender's port in `joint_state_osc.py`
+
+    # Create dispatcher and register callback
+    disp = dispatcher.Dispatcher()
+    disp.map("/joint_states/*", joint_handler)
+
+    server = osc_server.ThreadingOSCUDPServer((ip, port), disp) # Start OSC server
+    
+    try:
+        server.serve_forever()  # Keep server running
+    except KeyboardInterrupt:
+        print("")
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/bundles/different_lib.py

@@ -0,0 +1,38 @@
+# Import needed modules from osc4py3
+import osc4py3.as_eventloop 
+from osc4py3 import oscmethod as osm
+
+def handlerfunction(*args):
+    for arg in args:
+        print(arg)
+    pass
+"""
+def handlerfunction2(address, s, x, y):
+    # Will receive message address, and message data flattened in s, x, y
+    print(f's: {s}')
+    print(f'x: {x}')
+    print(f'y: {y}')
+    pass
+"""
+# Start the system.
+osc_startup()
+
+# Make server channels to receive packets.
+osc_udp_server("0.0.0.0", 5005, "aservername")
+osc_udp_server("0.0.0.0", 5005, "anotherserver")
+
+# Associate Python functions with message address patterns, using default
+# argument scheme OSCARG_DATAUNPACK.
+osc_method("SYNC", handlerfunction)
+# Too, but request the message address pattern before in argscheme
+#osc_method("SYNC", handlerfunction2, argscheme=osm.OSCARG_ADDRESS + osm.OSCARG_DATAUNPACK)
+
+# Periodically call osc4py3 processing method in your event loop.
+finished = False
+while not finished:
+    # …
+    osc_process()
+    # …
+
+# Properly close the system.
+osc_terminate()

test/ros_osc/joint_info/pythonosc/bundles/joint_states_bundle.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from pythonosc.udp_client import SimpleUDPClient
+from pythonosc import osc_bundle_builder
+from pythonosc import osc_message_builder
+
+class JointStateOSC(Node):
+    def __init__(self):
+        super().__init__('joint_states_osc')
+
+        # Create a ROS 2 subscriber to /joint_states topic
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            1  # Queue size
+        )
+
+        self.osc_ip = "127.0.0.1"   # target IP
+        self.osc_port = 5005        # target port
+        self.osc_client = SimpleUDPClient(self.osc_ip, self.osc_port)
+
+    def joint_states_callback(self, msg):
+        """Callback function to handle incoming joint states."""
+        header = msg.header
+        #print(f"Received joint states at {header.stamp.sec}.{header.stamp.nanosec}")
+        joint_names = msg.name
+        joint_positions = msg.position
+        joint_velocity = msg.velocity
+        joint_effort = msg.effort
+
+
+        bundle = osc_bundle_builder.OscBundleBuilder(timestamp=header.stamp.sec + header.stamp.nanosec * 1e-9)
+        #bundle = osc_bundle_builder.OscBundleBuilder(osc_bundle_builder.IMMEDIATELY)
+
+        names = osc_message_builder.OscMessageBuilder(address="/joint_states/names")
+        names.add_arg([str(name) for name in joint_names])
+        positions = osc_message_builder.OscMessageBuilder(address="/joint_states/positions")
+        positions.add_arg([float(pos) for pos in joint_positions])
+        velocity = osc_message_builder.OscMessageBuilder(address="/joint_states/velocity")
+        velocity.add_arg([float(vel) for vel in joint_velocity])
+        effort = osc_message_builder.OscMessageBuilder(address="/joint_states/effort")
+        effort.add_arg([float(eff) for eff in joint_effort])
+
+        bundle.add_content(names.build())
+        bundle.add_content(positions.build())
+        bundle.add_content(velocity.build())
+        bundle.add_content(effort.build())
+
+        info = osc_message_builder.OscMessageBuilder(address="/joint_states")
+        info.add_arg("joint names:")
+        info.add_arg([str(name) for name in joint_names])
+        info.add_arg("joint positions:")
+        info.add_arg([float(pos) for pos in joint_positions])
+        info.add_arg("joint velocity:")
+        info.add_arg([float(vel) for vel in joint_velocity])
+        info.add_arg("joint effort:")
+        info.add_arg([float(eff) for eff in joint_effort])
+
+        bundle.add_content(info.build())
+
+        self.osc_client.send(bundle.build())
+
+def main():
+    try: 
+        rclpy.init()
+        node = JointStateOSC()
+        rclpy.spin(node)
+    except KeyboardInterrupt: print("")
+        #node.destroy_node()
+        #rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

test/ros_osc/joint_info/pythonosc/bundles/read_bundle.py

@@ -0,0 +1,28 @@
+import argparse
+from pythonosc import dispatcher, osc_server
+from pythonosc.osc_bundle import OscBundle
+from pythonosc.osc_message import OscMessage
+
+def bundle_handler(address, *args):
+    for arg in args:
+        if isinstance(arg, OscBundle):
+            print(f"Received OSC bundle with timestamp: {arg.timestamp}")
+            for element in arg.bundle_elements:
+                if isinstance(element, OscMessage):
+                    print(f"Message: {element.address}, Args: {element.params}")
+
+def main():
+    parser = argparse.ArgumentParser(description="OSC Bundle Receiver")
+    parser.add_argument("--ip", default="127.0.0.1", help="The IP address to listen on")
+    parser.add_argument("--port", type=int, default=5005, help="The port to listen on")
+    args = parser.parse_args()
+
+    disp = dispatcher.Dispatcher()
+    disp.set_default_handler(bundle_handler)
+
+    server = osc_server.ThreadingOSCUDPServer((args.ip, args.port), disp)
+    print(f"Listening for OSC bundles on {args.ip}:{args.port}...")
+    server.serve_forever()
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/bundles/send_bundle_test.py

@@ -0,0 +1,36 @@
+from pythonosc import osc_bundle_builder
+from pythonosc import osc_message_builder
+import argparse
+import time
+from pythonosc import udp_client
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser()
+  parser.add_argument("--ip", default="127.0.0.1",
+      help="The ip of the OSC server")
+  parser.add_argument("--port", type=int, default=5005,
+      help="The port the OSC server is listening on")
+  args = parser.parse_args()
+
+  client = udp_client.SimpleUDPClient(args.ip, args.port)
+
+bundle = osc_bundle_builder.OscBundleBuilder(timestamp=time.time() + 10)
+msg = osc_message_builder.OscMessageBuilder(address="/SYNC")
+msg.add_arg(4.0)
+# Add 4 messages in the bundle, each with more arguments.
+bundle.add_content(msg.build())
+msg.add_arg(2)
+bundle.add_content(msg.build())
+msg.add_arg("value")
+bundle.add_content(msg.build())
+
+sub_bundle = bundle.build()
+# Now add the same bundle inside itself.
+bundle.add_content(sub_bundle)
+# The bundle has 5 elements in total now.
+
+bundle = bundle.build()
+while True:# You can now send it via a client with the `.send()` method:
+    client.send(bundle)
+    time.sleep(1)

test/ros_osc/joint_info/pythonosc/default_handler.py

@@ -0,0 +1,20 @@
+from pythonosc.dispatcher import Dispatcher
+from pythonosc.osc_server import BlockingOSCUDPServer
+
+# Fallback handler: will catch any OSC address not explicitly mapped
+def default_handler(address, *args):
+    print(f"[RECEIVED] Address: {address}, Args: {args}")
+
+def main():
+    ip = "0.0.0.0"   # Listen on all interfaces
+    port = 5005      # Set to the same port as the sender
+
+    dispatcher = Dispatcher()
+    dispatcher.set_default_handler(default_handler)
+
+    server = BlockingOSCUDPServer((ip, port), dispatcher)
+    print(f"Listening for OSC messages on {ip}:{port}...")
+    server.serve_forever()
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/joint_names_reader.py

@@ -0,0 +1,42 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from rclpy.executors import SingleThreadedExecutor
+
+class JointStateReader(Node):
+    def __init__(self):
+        super().__init__('JointStateReader')
+        self.sub = self.create_subscription(JointState, 'joint_states', self.sub_callback, 1)
+        self.joint_names = []
+        self.got_msg = False
+    
+    def sub_callback(self, msg):
+        """Callback to read joint names from the first received message."""
+        if not self.joint_names:
+            self.got_msg=True
+            self.joint_names = list(msg.name)
+
+def joint_names():
+    """Fetch joint names using an existing ROS2 context and ensure proper cleanup."""
+    rclpy.init()
+    node = JointStateReader()
+    executor = SingleThreadedExecutor()
+    executor.add_node(node)
+    while not node.got_msg:
+        executor.spin_once(timeout_sec=0)
+    joint_list = node.joint_names  # Store names before shutting down
+
+    node.destroy_node()  # Properly destroy the node
+    rclpy.shutdown()  # Shutdown ROS2 context to free memory
+
+    return joint_list  # Return the joint names safely
+
+def main():
+    """Main function to fetch and print joint names."""
+    joints = joint_names()
+    
+    for i, joint_name in enumerate(joints, start=1):
+        print(f"Joint No. {i}: {joint_name}")
+
+if __name__ == '__main__':
+    main()

test/ros_osc/joint_info/pythonosc/joint_state_overview.py

@@ -0,0 +1,43 @@
+from pythonosc import dispatcher
+from pythonosc import osc_server
+import time
+
+last_received = time.time()
+
+def joint_handler(address, *args):
+    global last_received
+    now = time.time()
+    
+    if now - last_received > 0.1:  # Limit updates to every 100ms
+        last_received = now
+        if args:
+            print(args[0])
+
+"""
+def joint_handler(address, *args):
+    if args:
+        print(args[0])
+        gc.collect()  # Force garbage collection
+"""
+        
+def main():
+    ip = "0.0.0.0"  # Listen on all network interfaces
+    port = 8000     # Must match the sender's port in `joint_state_osc.py`
+
+    # Create dispatcher and register callback
+    disp = dispatcher.Dispatcher()
+    disp.map("/joint_states", joint_handler)  # Listen for all joint messages
+
+    # Start OSC server
+    server = osc_server.ThreadingOSCUDPServer((ip, port), disp)
+    
+    try:
+        #server.serve_forever()  # Keep server running
+        while True:
+            server.handle_request()  # Process one request at a time
+
+    except KeyboardInterrupt:
+        print("")
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/joint_state_overview_osc4py3.py

@@ -0,0 +1,43 @@
+from osc4py3.as_eventloop import *
+from osc4py3.oscmethod import *
+import time
+
+last_received = time.time()
+
+def joint_handler(address, info):
+    global last_received
+    now = time.time()
+    
+    if now - last_received > 0.1 and address == "/joint_states":  # Limit updates to every 100ms
+        last_received = now
+        if info:
+            print(info)
+
+def main():
+    ip = "127.0.0.1" # "0.0.0.0" Listen on all network interfaces
+    port = 8000     # Must match the sender's port in `joint_state_osc.py`
+
+    # Start the system
+    osc_startup()
+
+    # Make server channels to receive packets
+    osc_udp_server(ip, port, "joint_state_server")
+
+    # Associate Python functions with message address patterns
+    osc_method("/joint_states", joint_handler, argscheme=OSCARG_ADDRESS + OSCARG_DATAUNPACK)
+
+    try:
+        # Run the event loop
+        while True:
+            osc_process()  # Process OSC messages
+            time.sleep(0.01)  # Sleep to avoid high CPU usage
+
+    except KeyboardInterrupt:
+        print("")
+
+    finally:
+        # Properly close the system
+        osc_terminate()
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/joint_state_reader.py

@@ -0,0 +1,23 @@
+from pythonosc import dispatcher
+from pythonosc import osc_server
+
+def joint_handler(address, *args):
+    print(f"{address} -> {args[0]}")
+
+def main():
+    ip = "0.0.0.0"  # Listen on all network interfaces
+    port = 5005     # Must match the sender's port in `joint_state_osc.py`
+
+    # Create dispatcher and register callback
+    disp = dispatcher.Dispatcher()
+    disp.map("/joint_states/shoulder_lift_joint/*", joint_handler)
+
+    server = osc_server.ThreadingOSCUDPServer((ip, port), disp) # Start OSC server
+    
+    try:
+        server.serve_forever()  # Keep server running
+    except KeyboardInterrupt:
+        print("")
+
+if __name__ == "__main__":
+    main()

test/ros_osc/joint_info/pythonosc/osc_joint_states_pub.py

@@ -0,0 +1,74 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from pythonosc.udp_client import SimpleUDPClient
+
+class JointStateOSC(Node):
+    def __init__(self):
+        super().__init__('joint_states_osc')
+
+        # Create a ROS 2 subscriber to /joint_states topic
+        self.subscription = self.create_subscription(
+            JointState,
+            '/joint_states',
+            self.joint_states_callback,
+            1  # Queue size
+        )
+
+        # Open Sound Control (OSC) Client settings
+        self.osc_ip = "127.0.0.1"   # Replace with the target IP
+        self.osc_port = 8000        # Replace with the target port
+        self.osc_client = SimpleUDPClient(self.osc_ip, self.osc_port)
+
+    def joint_states_callback(self, msg):
+        """Callback function to handle incoming joint states."""
+        header = msg.header
+        joint_names = msg.name
+        joint_positions = msg.position
+        joint_velocity = msg.velocity
+        joint_effort = msg.effort
+
+        joint_names_str = "\n- ".join(joint_names)
+        joint_positions_str = "\n- ".join(map(str, joint_positions))
+        joint_velocity_str = "\n- ".join(map(str, joint_velocity))
+        joint_effort_str = "\n- ".join(map(str, joint_effort))
+
+        info = f"""
+---
+header:
+    stamp:
+        sec: {header.stamp.sec}
+        nanosec: {header.stamp.nanosec}
+name:
+- {joint_names_str}
+position:
+- {joint_positions_str}
+velocity:
+- {joint_velocity_str}
+effort:
+- {joint_effort_str}
+---"""
+
+        self.osc_client.send_message(f"/joint_states", info)
+
+        # Send each joint state as an OSC message
+        for i, name in enumerate(joint_names):
+
+            self.osc_client.send_message(f"/joint_states/header/sec", header.stamp.sec) # Publish Time (sec)
+            self.osc_client.send_message(f"/joint_states/header/nanosec", header.stamp.nanosec) # Publish Time (nanosec)
+
+            self.osc_client.send_message(f"/joint_states/{name}/position", joint_positions[i]) # Publish position
+
+            self.osc_client.send_message(f"/joint_states/{name}/velocity", joint_velocity[i])  # Publish velocity
+
+            self.osc_client.send_message(f"/joint_states/{name}/effort", joint_effort[i])      # Publish effort
+
+def main():
+    rclpy.init()
+    node = JointStateOSC()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()