Better sending of PSN packets

README.md

@@ -25,6 +25,7 @@ python -m pip install https://codeload.github.com/open-stage/python-psn/zip/refs
 
 ## Usage
 
+### Receiving PSN data
 ```python
 import pypsn
 
@@ -45,15 +46,54 @@ receiver.start()  # start the receiving thread
 
 receiver.stop() # stop receiving
 
+```
+
+### Senfing PSN data
+```python
+import pypsn
+
+# create a psn_info object with appropriate data
+psn_info = pypsn.PsnInfoPacket(
+    info=pypsn.PsnInfo(
+        timestamp=1312,
+        version_high=2,
+        version_low=0,
+        frame_id=1,
+        packet_count=1,
+    ),
+    name="system_name_001",
+    trackers=(
+        [
+            pypsn.PsnTrackerInfo(
+                tracker_id=i,
+                tracker_name="tracker_" + str(i),
+            )
+            for i in range(0, 8))
+        ]
+    ),
+)
+
+# convert the object to a byte string
+psn_info_packet_bytes = pypsn.prepare_psn_info_packet_bytes(psn_info)
+
+# send the PSN info via multicast
+pypsn.send_psn_packet(
+    psn_packet=psn_info_packet_bytes,
+    mcast_ip="236.10.10.10",
+    ip_addr="192.168.1.42",
+    mcast_port=56565,
+)
+
 ```
 See examples folder for some more examples.
 
 ## Development, status
 
 - Supporting PSN V2
-- Parsing and sending (via [Multicast Expert](https://github.com/multiplemonomials/multicast_expert))
+- Parsing and sending (via [Multicast Expert](https://github.com/multiplemonomials/multicast_expert)) or via Unicast
 - Using threading module
 - Linux (Rpi OS incl.), Windows and macOS tested
+- PSN messages recognized by GrandMa3 2.1
 - Typed, no-strict
 - Initial pytest testing provided together with CI/CD setup
 

examples/simple_send_and_receive/send_psn.py

@@ -1,19 +1,49 @@
 #! /bin/env python3
 """
-Usage: python send_psn.py 192.168.1.4 <--- change IP address
+Usage:
+
+multicast: python send_psn.py 1 192.168.1.4 236.10.10.10
+unicast: python send_psn.py 1 192.168.1.4
+
+Args:
+- number of trackers to generate
+- local IP adress
+- multricast adress (opt)
+
+Variables mapping for GrandMa3 users:
+
+- pos.xyz -> Position X Y Z
+- speed.xyz -> Speed X Y Z
+- ori.xyz -> Rot X Y Z
+- accel.xyz -> None
+- trgtpos.xyz -> None
 
 """
 
 import sys
+import time
 import pypsn
 
+try:
+    tracker_num = int(sys.argv[1])
+    ip_addr = sys.argv[2]
+    mcast_ip = None
+
+    if sys.argv[3]:
+        mcast_ip = sys.argv[3]
+
+except Exception as e:
+    print("Args: tracker_num ip_addr mcast_ip (opt).")
+    print(e)
+
+start_time_us = time.time_ns() // 1000
 
 psn_info = pypsn.PsnInfoPacket(
     info=pypsn.PsnInfo(
-        timestamp=1312,
+        timestamp=start_time_us,
         version_high=2,
         version_low=0,
-        frame_id=56,
+        frame_id=1,
         packet_count=1,
     ),
     name="system_name_001",
@@ -23,13 +53,19 @@
                 tracker_id=i,
                 tracker_name="tracker_" + str(i),
             )
-            for i in range(0, 7)
+            for i in range(0, tracker_num)
         ]
     ),
 )
 
 psn_data = pypsn.PsnDataPacket(
-    info=psn_info.info,
+    info=pypsn.PsnInfo(
+        timestamp=start_time_us,
+        version_high=2,
+        version_low=0,
+        frame_id=1,
+        packet_count=1,
+    ),
     trackers=(
         [
             pypsn.PsnTracker(
@@ -50,6 +86,7 @@
                     y=0.0,
                     z=0.0,
                 ),
+                status=0.5,
                 accel=pypsn.PsnVector3(
                     x=0.0,
                     y=0.0,
@@ -60,15 +97,14 @@
                     y=0.0,
                     z=0.0,
                 ),
-                status=0,
-                timestamp=psn_info.info.timestamp,
+                timestamp=start_time_us,
             )
             for tracker in psn_info.trackers
         ]
     ),
 )
 
-print("\n--- PSN infos to be sent ---")
+print("\n--- PSN infos to be sent at 1Hz ---")
 print("system name: " + psn_info.name)
 print("timestamp: " + str(psn_info.info.timestamp))
 print("version_high: " + str(psn_info.info.version_high))
@@ -79,7 +115,7 @@
 for tracker in psn_info.trackers:
     print(str(tracker.tracker_id) + ": " + str(tracker.tracker_name))
 
-print("\n--- PSN data to be sent ---")
+print("\n--- PSN data to be sent at 60Hz ---")
 print("timestamp: " + str(psn_data.info.timestamp))
 print("version_high: " + str(psn_data.info.version_high))
 print("version_low: " + str(psn_data.info.version_low))
@@ -108,19 +144,48 @@
         + str(tracker.timestamp)
     )
 
-psn_info_packet_bytes = pypsn.prepare_psn_info_packet_bytes(psn_info)
-psn_data_packet_bytes = pypsn.prepare_psn_data_packet_bytes(psn_data)
+print("\n--- Sendin PSN data in loop ---")
+counter = 0.0
 
-pypsn.send_psn_packet(
-    psn_packet=psn_info_packet_bytes,
-    mcast_ip="236.10.10.10",
-    ip_addr=sys.argv[1],
-    mcast_port=56565,
-)
+while True:
+    time.sleep(1 / 60)
+    elapsed_time_us = time.time_ns() // 1000 - start_time_us
 
-pypsn.send_psn_packet(
-    psn_packet=psn_data_packet_bytes,
-    mcast_ip="236.10.10.10",
-    ip_addr=sys.argv[1],
-    mcast_port=56565,
-)
+    if counter < 6.0:
+        counter += 0.1
+    else:
+        counter = 0.0
+
+        psn_info.info.timestamp = elapsed_time_us
+        psn_info_packet_bytes = pypsn.prepare_psn_info_packet_bytes(psn_info)
+
+        pypsn.send_psn_packet(
+            psn_packet=psn_info_packet_bytes,
+            mcast_ip=mcast_ip,
+            ip_addr=ip_addr,
+            port=56565,
+        )
+
+    psn_data.info.timestamp = elapsed_time_us
+
+    if psn_data.info.frame_id < 255:
+        psn_data.info.frame_id += 1
+    else:
+        psn_data.info.frame_id = 0
+
+    for tracker in psn_data.trackers:
+        tracker.timestamp = elapsed_time_us
+        tracker.pos.x = counter
+        tracker.speed.x = counter
+        tracker.ori.x = counter
+        tracker.accel.x = counter
+        tracker.trgtpos.x = counter
+
+    psn_data_packet_bytes = pypsn.prepare_psn_data_packet_bytes(psn_data)
+
+    pypsn.send_psn_packet(
+        psn_packet=psn_data_packet_bytes,
+        mcast_ip=mcast_ip,
+        ip_addr=ip_addr,
+        port=56565,
+    )