Added launch file and fixed some terminal output.

launch/streamer.launch.py

+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    optitrack_streamer_node = Node(
+        package="optitrack_streamer",
+        executable="optitrack_streamer_node",
+        name="optitrack_streamer_node",
+        output="screen",
+    )
+    
+    wrist_streamer_node = Node(
+        package="optitrack_streamer",
+        executable="wrist_streamer_node",
+        name="wrist_streamer_node",
+        output="screen",
+    )
+
+    return LaunchDescription([
+        optitrack_streamer_node,
+        wrist_streamer_node,
+     ])

optitrack_streamer/optitrack_streamer_node.py

@@ -50,7 +50,7 @@ class OptitrackStreamerNode(Node):
         self.rigid_bodies[str(rigid_body_id)] = {"pos":position, "rot":rotation}
 
     def mainloop_callback(self):
-        self.get_logger().info("Received: " + str(list(self.rigid_bodies.keys())), throttle_duration_sec=10)
+        self.get_logger().info("Received RAW: " + str(list(self.rigid_bodies.keys())), throttle_duration_sec=2)
         
         for id, frame_id in self.rb_id_to_frame_id.items():
             try:

optitrack_streamer/rigid_body_reader.py

-import time
-import threading
-from NatNetClient import NatNetClient
-
-# Dictionary to store rigid body data
-rigid_body_data = {}
-
-class CustomNatNetClient(NatNetClient):
-    def __init__(self, rigid_body_listener=None):
-        super().__init__()
-        self.rigid_body_listener = rigid_body_listener  # Assign callback function
-
-    def receive_rigid_body_list(self, rigid_body_list, stamp):
-        """Process rigid body data and call the listener if available."""
-        global rigid_body_data
-        for rigid_body in rigid_body_list:
-            rigid_body_id = rigid_body[0]  # ID of the rigid body
-            position = tuple(rigid_body[1])  # Position (x, y, z)
-            rotation = tuple(rigid_body[2])  # Rotation (qx, qy, qz, qw)
-
-            new_data = {"position": position, "rotation": rotation}
-
-            # Only print if data has changed
-            if rigid_body_id not in rigid_body_data or rigid_body_data[rigid_body_id] != new_data:
-                rigid_body_data[rigid_body_id] = new_data
-                print(f"Rigid Body {rigid_body_id} Updated: {new_data}")
-
-            # Call the listener function if it exists
-            if self.rigid_body_listener:
-                self.rigid_body_listener(rigid_body_id, position, rotation)
-
-# Callback function to process rigid body data
-def my_rigid_body_listener(rigid_body_id, position, rotation):
-    print(f"Listener received Rigid Body {rigid_body_id}: Position {position}, Rotation {rotation}")
-
-# Create a custom NatNet client and assign the listener
-client = CustomNatNetClient(rigid_body_listener=my_rigid_body_listener)
-
-# Run the client in a separate thread
-def start_natnet():
-    client.run()
-
-natnet_thread = threading.Thread(target=start_natnet, daemon=True)
-natnet_thread.start()
-
-print("Listening for OptiTrack rigid bodies... (Press Ctrl + C to stop)")
-
-# Keep main script running, but allow Ctrl + C to exit
-try:
-    while True:
-        time.sleep(1)
-except KeyboardInterrupt:
-    print("\nStopping...")

optitrack_streamer/wrist_streamer_node.py

@@ -6,7 +6,6 @@ import json, os
 import numpy as np
 from scipy.spatial.transform import Rotation as R
 
-
 class WristStreamerNode(Node):
 
     def __init__(self):
@@ -116,8 +115,8 @@ class WristStreamerNode(Node):
                 self.broadcast_tf("world", which_hand + "_ref", aligned_trans, aligned_quat)
 
     def mainloop_callback(self):
-        msg = "\n"
-        msg += "Right: " + self.state["right"]
+        msg = "\n\n"
+        msg += "Right: " + self.state["right"] + "\n"
         msg += "Left: " + self.state["left"] + "\n"
 
         self.get_logger().info(msg)

setup.py

@@ -12,6 +12,7 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
         (os.path.join('share', package_name, 'calibration'), glob('*.calib.json'))
     ],
     install_requires=['setuptools'],