From 17c70195c926b8ce7eab32368781da23550a063c Mon Sep 17 00:00:00 2001
From: Kai Junge <kai.junge@epfl.ch>
Date: Thu, 6 Feb 2025 15:28:34 +0100
Subject: [PATCH] Added launch file and fixed some terminal output.
---
launch/streamer.launch.py | 24 +++++++++
optitrack_streamer/optitrack_streamer_node.py | 2 +-
optitrack_streamer/rigid_body_reader.py | 53 -------------------
optitrack_streamer/wrist_streamer_node.py | 5 +-
setup.py | 1 +
5 files changed, 28 insertions(+), 57 deletions(-)
create mode 100644 launch/streamer.launch.py
delete mode 100644 optitrack_streamer/rigid_body_reader.py
diff --git a/launch/streamer.launch.py b/launch/streamer.launch.py
new file mode 100644
index 0000000..ad4ea52
--- /dev/null
+++ b/launch/streamer.launch.py
@@ -0,0 +1,24 @@
+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,
+ ])
diff --git a/optitrack_streamer/optitrack_streamer_node.py b/optitrack_streamer/optitrack_streamer_node.py
index 41ecc83..acb86e0 100644
--- a/optitrack_streamer/optitrack_streamer_node.py
+++ b/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:
diff --git a/optitrack_streamer/rigid_body_reader.py b/optitrack_streamer/rigid_body_reader.py
deleted file mode 100644
index 83155b0..0000000
--- a/optitrack_streamer/rigid_body_reader.py
+++ /dev/null
@@ -1,53 +0,0 @@
-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...")
diff --git a/optitrack_streamer/wrist_streamer_node.py b/optitrack_streamer/wrist_streamer_node.py
index 31f8e4b..783f916 100644
--- a/optitrack_streamer/wrist_streamer_node.py
+++ b/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)
diff --git a/setup.py b/setup.py
index 7b808ec..2cb0da5 100644
--- a/setup.py
+++ b/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'],
--
GitLab