diff --git a/optitrack_streamer/optitrack_streamer_node.py b/optitrack_streamer/optitrack_streamer_node.py
index 2a6120cc2a16dfaa0cddff2b6de9bcc7759f04f7..41ecc83c7a1de2468d1b139a24c9bc35b4a0d6d3 100644
--- a/optitrack_streamer/optitrack_streamer_node.py
+++ b/optitrack_streamer/optitrack_streamer_node.py
@@ -1,13 +1,11 @@
import rclpy
from rclpy.node import Node
-from geometry_msgs.msg import Pose, TransformStamped
+from geometry_msgs.msg import PoseStamped
import threading
from .NatNetClient import NatNetClient
from tf2_ros import TransformBroadcaster
from helper_functions.utility import Key
from copy import deepcopy as cp
-import numpy as np
-from scipy.spatial.transform import Rotation as R
rigid_body_data = {}
@@ -31,10 +29,8 @@ class OptitrackStreamerNode(Node):
self.rigid_bodies = {}
# Publisher
- self.right_hand_publisher = self.create_publisher(Pose, '/optitrack/raw_right', rclpy.qos.qos_profile_sensor_data)
- self.left_hand_publisher = self.create_publisher(Pose, '/optitrack/raw_left', rclpy.qos.qos_profile_sensor_data)
-
- self.rb_id_to_publisher = {"1": self.right_hand_publisher, "2": self.left_hand_publisher}
+ self.wrist_raw_publisher = self.create_publisher(PoseStamped, '/wrist/raw', rclpy.qos.qos_profile_sensor_data)
+ self.rb_id_to_frame_id = {"1": "right", "2": "left"}
# Main loop
self.mainloop = self.create_timer(0.01, self.mainloop_callback)
@@ -44,23 +40,6 @@ class OptitrackStreamerNode(Node):
natnet_thread = threading.Thread(target=self.start_natnet, daemon=True)
natnet_thread.start()
-
- def broadcast_tf(self, base_name, frame_name, position, quaternion):
- t = TransformStamped()
- t.header.stamp = self.get_clock().now().to_msg()
- t.header.frame_id = base_name
- t.child_frame_id = frame_name
-
- t.transform.translation.x = position[0]
- t.transform.translation.y = position[1]
- t.transform.translation.z = position[2]
-
- t.transform.rotation.x = quaternion[0]
- t.transform.rotation.y = quaternion[1]
- t.transform.rotation.z = quaternion[2]
- t.transform.rotation.w = quaternion[3]
-
- self.tf_broadcaster.sendTransform(t)
# Run the client in a separate thread
def start_natnet(self):
@@ -71,38 +50,28 @@ class OptitrackStreamerNode(Node):
self.rigid_bodies[str(rigid_body_id)] = {"pos":position, "rot":rotation}
def mainloop_callback(self):
- msg = "Received: " + str(list(self.rigid_bodies.keys()))
- self.get_logger().info(msg, throttle_duration_sec=10)
+ self.get_logger().info("Received: " + str(list(self.rigid_bodies.keys())), throttle_duration_sec=10)
- for id, publisher in self.rb_id_to_publisher.items():
+ for id, frame_id in self.rb_id_to_frame_id.items():
try:
data = cp(self.rigid_bodies[id])
except:
continue
- raw = {"p":np.array([-data["pos"][0], data["pos"][2], data["pos"][1]]),
- "r":np.array([-data["rot"][0], data["rot"][2], data["rot"][1], data["rot"][3]])}
-
- self.broadcast_tf("world", id, raw["p"], raw["r"])
- self.broadcast_tf("world", "base", [0.1, 0.0, 0.0], np.array([0.0, 0.0, 0.0, 1.0]))
+ pose = PoseStamped()
+ pose.header.frame_id = frame_id
+ pose.header.stamp = self.get_clock().now().to_msg()
- if self.key.keyPress == "r":
- self.get_logger().info("Reset!", throttle_duration_sec = 1)
+ pose.pose.position.x = -data["pos"][0]
+ pose.pose.position.y = data["pos"][2]
+ pose.pose.position.z = data["pos"][1]
- initial_rotation = R.from_quat(cp(raw["r"]))
- self.calib_rotation = initial_rotation.inv()
+ pose.pose.orientation.x = -data["rot"][0]
+ pose.pose.orientation.y = data["rot"][2]
+ pose.pose.orientation.z = data["rot"][1]
+ pose.pose.orientation.w = data["rot"][3]
- try:
- aligned = R.from_quat(raw["r"]) * self.calib_rotation
-
- rb_to_wrist_r = np.array([0.0, -0.05, -0.12])
- trans_vec = np.matmul(aligned.as_matrix(), rb_to_wrist_r)
- aligned_new_disp = cp(raw["p"]) + trans_vec
-
- self.broadcast_tf("world", id + "_inv", aligned_new_disp, aligned.as_quat())
-
- except:
- pass
+ self.wrist_raw_publisher.publish(pose)
def main():
rclpy.init()
diff --git a/optitrack_streamer/wrist_streamer_node.py b/optitrack_streamer/wrist_streamer_node.py
new file mode 100644
index 0000000000000000000000000000000000000000..31f8e4b5f0a4f2b8382ca258ceb5b0cbbec6b375
--- /dev/null
+++ b/optitrack_streamer/wrist_streamer_node.py
@@ -0,0 +1,135 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped, TransformStamped
+from tf2_ros import TransformBroadcaster
+import json, os
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+class WristStreamerNode(Node):
+
+ def __init__(self):
+ super().__init__('Wrist_Streamer_Node')
+ self.get_logger().info('Starting Wrist Streamer')
+
+ # Variables
+ self.calib_rotation = {"right":None, "left":None}
+ self.calib_translation = {"right":None, "left":None}
+ self.state = {"right":"No message", "left":"No message"}
+
+ # Static variables
+ self.rigid_body_two_wirst = {"right":np.array([0.0, -0.05, -0.12]),"left":np.array([0.0, -0.05, -0.12])}
+ self.calib_world_position = {"right":np.array([0.05, 0.0, 0.0]), "left":np.array([-0.05, 0.0, 0.0])}
+
+ # tf related
+ self.tf_broadcaster = TransformBroadcaster(self)
+
+ # Subscriber
+ self.wrist_raw_subscriber = self.create_subscription(PoseStamped, '/wrist/raw', self.wrist_raw_callback, rclpy.qos.qos_profile_sensor_data)
+
+ # Publisher
+ self.wrist_publisher = self.create_publisher(PoseStamped, '/wrist/calibrated', rclpy.qos.qos_profile_sensor_data)
+
+ # Main loop
+ self.mainloop = self.create_timer(2, self.mainloop_callback)
+
+
+ def broadcast_tf(self, base_name, frame_name, position, quaternion):
+ t = TransformStamped()
+ t.header.stamp = self.get_clock().now().to_msg()
+ t.header.frame_id = base_name
+ t.child_frame_id = frame_name
+
+ t.transform.translation.x = position[0]
+ t.transform.translation.y = position[1]
+ t.transform.translation.z = position[2]
+
+ t.transform.rotation.x = quaternion[0]
+ t.transform.rotation.y = quaternion[1]
+ t.transform.rotation.z = quaternion[2]
+ t.transform.rotation.w = quaternion[3]
+
+ self.tf_broadcaster.sendTransform(t)
+
+ def publish_wrist(self, frame_name, position, quaternion):
+ pose = PoseStamped()
+ pose.header.stamp = self.get_clock().now().to_msg()
+ pose.header.frame_id = frame_name
+
+ pose.pose.position.x = position[0]
+ pose.pose.position.y = position[1]
+ pose.pose.position.z = position[2]
+
+ pose.pose.orientation.x = quaternion[0]
+ pose.pose.orientation.y = quaternion[1]
+ pose.pose.orientation.z = quaternion[2]
+ pose.pose.orientation.w = quaternion[3]
+
+ self.wrist_publisher.publish(pose)
+
+ def get_calibration(self, which_hand):
+ dir_path = os.path.dirname(os.path.realpath(__file__))
+
+ json_file = dir_path + "/../calibration/" + which_hand + "/" + which_hand + ".calib.json"
+ try:
+ return json.load(open(json_file))
+ except:
+ return None
+
+ def save_calibration(self, which_hand, trans, quat):
+ json_object = json.dumps({"trans": trans,"quat":quat}, indent=4)
+ with open("/../calibration/" + which_hand + ".calib.json", "w") as outfile:
+ outfile.write(json_object)
+
+ def wrist_raw_callback(self, msg:PoseStamped):
+ for which_hand in ["right", "left"]:
+ if msg.header.frame_id == which_hand:
+
+ if self.calib_rotation[which_hand] is None:
+ calib = self.get_calibration(which_hand)
+
+ if calib is None:
+ self.state[which_hand] = "No calibration"
+ continue
+
+ self.calib_rotation[which_hand] = R.from_quat(calib["quat"]).inv()
+ self.calib_translation[which_hand] = calib["trans"]
+
+ self.state[which_hand] = "Calibrated and streaming"
+
+ raw_trans = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
+ raw_quat = np.array([msg.pose.orientation.x, msg.pose.orientation.y,
+ msg.pose.orientation.z, msg.pose.orientation.w])
+
+ # Get the scipy rotation representation of the rigid body aligned with the base coordinate
+ aligned = R.from_quat(raw_quat) * self.calib_rotation[which_hand]
+
+ # Get the translation from the 'tree' to the actual wrist of the human
+ trans_vec = np.matmul(aligned.as_matrix(), self.rigid_body_two_wirst)
+
+ # Zero the translation
+ aligned_trans = raw_trans - self.calib_translation[which_hand] + trans_vec + self.calib_world_position
+ aligned_quat = aligned.as_quat()
+
+ self.publish_wrist(which_hand, aligned_trans, aligned_quat)
+ self.broadcast_tf("world", which_hand + "_ref", aligned_trans, aligned_quat)
+
+ def mainloop_callback(self):
+ msg = "\n"
+ msg += "Right: " + self.state["right"]
+ msg += "Left: " + self.state["left"] + "\n"
+
+ self.get_logger().info(msg)
+
+def main():
+ rclpy.init()
+ node = WristStreamerNode()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
+
+
diff --git a/setup.py b/setup.py
index c3cf2132cf0176fc4437924552362e2c365d8664..7b808ec234b983ead6b7567e3c6326197356d575 100644
--- a/setup.py
+++ b/setup.py
@@ -1,3 +1,5 @@
+import os
+from glob import glob
from setuptools import find_packages, setup
package_name = 'optitrack_streamer'
@@ -10,6 +12,7 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'calibration'), glob('*.calib.json'))
],
install_requires=['setuptools'],
zip_safe=True,
@@ -20,7 +23,8 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
- 'optitrack_streamer_node = optitrack_streamer.optitrack_streamer_node:main'
+ 'optitrack_streamer_node = optitrack_streamer.optitrack_streamer_node:main',
+ 'wrist_streamer_node = optitrack_streamer.wrist_streamer_node:main'
],
},
)