From c040816724ab00f092b6f0e327b50c0645f2eb57 Mon Sep 17 00:00:00 2001
From: Kai Junge <kai.junge@epfl.ch>
Date: Fri, 21 Feb 2025 18:04:35 +0100
Subject: [PATCH] Added sperate launch file for calibration.
---
launch/calib_optitrack.launch.py | 53 +++++++++++++++++++++++
launch/streamer.launch.py | 12 +++++
optitrack_streamer/wrist_streamer_node.py | 31 ++++++++-----
3 files changed, 86 insertions(+), 10 deletions(-)
create mode 100644 launch/calib_optitrack.launch.py
diff --git a/launch/calib_optitrack.launch.py b/launch/calib_optitrack.launch.py
new file mode 100644
index 0000000..363f256
--- /dev/null
+++ b/launch/calib_optitrack.launch.py
@@ -0,0 +1,53 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ # Declare simulation mode and VSA module launch arguments
+ calibration_mode_arg = DeclareLaunchArgument(
+ 'calib',
+ default_value='true',
+ description='Whether we map the pedals for calibration or not'
+ )
+
+ calibration_mode = LaunchConfiguration('calib')
+
+ pedal_node = Node(
+ package="input_devices",
+ executable="pedal_driver_node",
+ name="pedal_driver_node",
+ output="screen",
+ )
+
+ 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",
+ parameters=[{'calib': calibration_mode}]
+ )
+
+ rviz_node = Node(
+ package="rviz2",
+ executable="rviz2",
+ name="rviz2",
+ arguments=["-d", "/home/kaijunge/.rviz2/default.rviz"],
+ output="screen",
+ )
+
+ return LaunchDescription([
+ pedal_node,
+ calibration_mode_arg,
+ optitrack_streamer_node,
+ wrist_streamer_node,
+ rviz_node
+ ])
diff --git a/launch/streamer.launch.py b/launch/streamer.launch.py
index ad4ea52..56303e0 100644
--- a/launch/streamer.launch.py
+++ b/launch/streamer.launch.py
@@ -1,8 +1,18 @@
from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
+ # Declare simulation mode and VSA module launch arguments
+ calibration_mode_arg = DeclareLaunchArgument(
+ 'calib',
+ default_value='false',
+ description='Whether we map the pedals for calibration or not'
+ )
+
+ calibration_mode = LaunchConfiguration('calib')
optitrack_streamer_node = Node(
package="optitrack_streamer",
@@ -16,9 +26,11 @@ def generate_launch_description():
executable="wrist_streamer_node",
name="wrist_streamer_node",
output="screen",
+ parameters=[{'calib': calibration_mode}]
)
return LaunchDescription([
+ calibration_mode_arg,
optitrack_streamer_node,
wrist_streamer_node,
])
diff --git a/optitrack_streamer/wrist_streamer_node.py b/optitrack_streamer/wrist_streamer_node.py
index 3cffda2..934f7cc 100644
--- a/optitrack_streamer/wrist_streamer_node.py
+++ b/optitrack_streamer/wrist_streamer_node.py
@@ -12,16 +12,20 @@ class WristStreamerNode(Node):
def __init__(self):
super().__init__('Wrist_Streamer_Node')
self.get_logger().info('Starting Wrist Streamer')
-
+
+ # Parameters
+ self.declare_parameter('calib', False)
+ self.calib_setting = self.get_parameter('calib').get_parameter_value().bool_value
+
# Variables
self.calib_rotation = {"right":None, "left":None}
self.calib_translation = {"right":None, "left":None}
self.state = {"right":"No message", "left":"No message"}
- self.is_pedal = False
+ self.is_pedal = {"right":False, "left":False}
# 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.12]), "left":np.array([-0.05, 0.0, 0.0])}
+ self.rigid_body_to_wirst = {"right":np.array([0.0, -0.06, -0.12]),"left":np.array([0.0, -0.06, -0.12])}
+ self.calib_world_position = {"right":np.array([-0.05, 0.06, 0.12]), "left":np.array([-0.05, 0.06, 0.12])}
# tf related
self.tf_broadcaster = TransformBroadcaster(self)
@@ -34,7 +38,7 @@ class WristStreamerNode(Node):
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)
+ self.mainloop = self.create_timer(1.5, self.mainloop_callback)
def broadcast_tf(self, base_name, frame_name, position, quaternion):
@@ -86,9 +90,14 @@ class WristStreamerNode(Node):
def pedal_callback(self, msg:Int16MultiArray):
if msg.data[0] == 1:
- self.is_pedal = True
+ self.is_pedal["left"] = True
+ else:
+ self.is_pedal["left"] = False
+
+ if msg.data[1] == 1:
+ self.is_pedal["right"] = True
else:
- self.is_pedal = False
+ self.is_pedal["right"] = False
def wrist_raw_callback(self, msg:PoseStamped):
for which_hand in ["right", "left"]:
@@ -98,8 +107,8 @@ class WristStreamerNode(Node):
raw_quat = np.array([msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w])
- if self.is_pedal:
- self.get_logger().info("Calibration of glove pose!", throttle_duration_sec=1)
+ if self.is_pedal[which_hand] and self.calib_setting:
+ self.get_logger().info(which_hand + ": Calibration of glove pose!", throttle_duration_sec=1)
self.save_calibration(which_hand, list(raw_trans), list(raw_quat))
self.calib = self.get_calibration(which_hand)
@@ -119,7 +128,7 @@ class WristStreamerNode(Node):
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[which_hand])
+ trans_vec = np.matmul(aligned.as_matrix(), self.rigid_body_to_wirst[which_hand])
# Zero the translation
aligned_trans = raw_trans - self.calib_translation[which_hand] + trans_vec + self.calib_world_position[which_hand]
@@ -132,6 +141,8 @@ class WristStreamerNode(Node):
msg = "\n\n"
msg += "Right: " + self.state["right"] + "\n"
msg += "Left: " + self.state["left"] + "\n"
+ if self.calib_setting:
+ msg += "\nPedal A, B for left, Right hands\n"
self.get_logger().info(msg)
--
GitLab