AS: osc4py3

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()

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()

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()

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()

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()

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)