From 26ec4eedde2b074cafa2ce28f624dae85c4cdd8c Mon Sep 17 00:00:00 2001 From: Daniel Date: Mon, 1 Sep 2025 17:27:22 +0200 Subject: [PATCH] Include wb-joints in transform publisher. --- .../transform_publisher/src/skeleton_tfs.cpp | 138 ++++++++++++++++-- 1 file changed, 125 insertions(+), 13 deletions(-) diff --git a/extras/ros/transform_publisher/src/skeleton_tfs.cpp b/extras/ros/transform_publisher/src/skeleton_tfs.cpp index ec76b3d..2444b56 100644 --- a/extras/ros/transform_publisher/src/skeleton_tfs.cpp +++ b/extras/ros/transform_publisher/src/skeleton_tfs.cpp @@ -28,16 +28,16 @@ public: std::bind(&SkeletonTFPublisher::listener_callback, this, std::placeholders::_1)); pc_connections = { + // main joints {"hip_middle", "shoulder_middle"}, {"shoulder_middle", "head"}, - {"shoulder_middle", "nose"}, - {"nose", "eye_left"}, - {"nose", "eye_right"}, - {"nose", "ear_left"}, - {"nose", "ear_right"}, - {"shoulder_left", "shoulder_right"}, - {"hip_middle", "shoulder_left"}, - {"hip_middle", "shoulder_right"}, + {"head", "nose"}, + {"head", "eye_left"}, + {"head", "eye_right"}, + {"head", "ear_left"}, + {"head", "ear_right"}, + {"shoulder_middle", "shoulder_left"}, + {"shoulder_middle", "shoulder_right"}, {"shoulder_left", "elbow_left"}, {"elbow_left", "wrist_left"}, {"hip_middle", "hip_left"}, @@ -48,6 +48,123 @@ public: {"hip_middle", "hip_right"}, {"hip_right", "knee_right"}, {"knee_right", "ankle_right"}, + // whole-body joints + {"ankle_left", "foot_toe_big_left"}, + {"ankle_left", "foot_toe_small_left"}, + {"ankle_left", "foot_heel_left"}, + {"ankle_right", "foot_toe_big_right"}, + {"ankle_right", "foot_toe_small_right"}, + {"ankle_right", "foot_heel_right"}, + {"ear_right", "face_jaw_right_1"}, + {"ear_right", "face_jaw_right_2"}, + {"ear_right", "face_jaw_right_3"}, + {"ear_right", "face_jaw_right_4"}, + {"ear_right", "face_jaw_right_5"}, + {"ear_right", "face_jaw_right_6"}, + {"ear_right", "face_jaw_right_7"}, + {"ear_right", "face_jaw_right_8"}, + {"head", "face_jaw_middle"}, + {"ear_left", "face_jaw_left_1"}, + {"ear_left", "face_jaw_left_2"}, + {"ear_left", "face_jaw_left_3"}, + {"ear_left", "face_jaw_left_4"}, + {"ear_left", "face_jaw_left_5"}, + {"ear_left", "face_jaw_left_6"}, + {"ear_left", "face_jaw_left_7"}, + {"ear_left", "face_jaw_left_8"}, + {"eye_right", "face_eyebrow_right_1"}, + {"eye_right", "face_eyebrow_right_2"}, + {"eye_right", "face_eyebrow_right_3"}, + {"eye_right", "face_eyebrow_right_4"}, + {"eye_right", "face_eyebrow_right_5"}, + {"eye_left", "face_eyebrow_left_1"}, + {"eye_left", "face_eyebrow_left_2"}, + {"eye_left", "face_eyebrow_left_3"}, + {"eye_left", "face_eyebrow_left_4"}, + {"eye_left", "face_eyebrow_left_5"}, + {"nose", "face_nose_1"}, + {"nose", "face_nose_2"}, + {"nose", "face_nose_3"}, + {"nose", "face_nose_4"}, + {"nose", "face_nose_5"}, + {"nose", "face_nose_6"}, + {"nose", "face_nose_7"}, + {"nose", "face_nose_8"}, + {"nose", "face_nose_9"}, + {"eye_right", "face_eye_right_1"}, + {"eye_right", "face_eye_right_2"}, + {"eye_right", "face_eye_right_3"}, + {"eye_right", "face_eye_right_4"}, + {"eye_right", "face_eye_right_5"}, + {"eye_right", "face_eye_right_6"}, + {"eye_left", "face_eye_left_1"}, + {"eye_left", "face_eye_left_2"}, + {"eye_left", "face_eye_left_3"}, + {"eye_left", "face_eye_left_4"}, + {"eye_left", "face_eye_left_5"}, + {"eye_left", "face_eye_left_6"}, + {"head", "face_mouth_1"}, + {"head", "face_mouth_2"}, + {"head", "face_mouth_3"}, + {"head", "face_mouth_4"}, + {"head", "face_mouth_5"}, + {"head", "face_mouth_6"}, + {"head", "face_mouth_7"}, + {"head", "face_mouth_8"}, + {"head", "face_mouth_9"}, + {"head", "face_mouth_10"}, + {"head", "face_mouth_11"}, + {"head", "face_mouth_12"}, + {"head", "face_mouth_13"}, + {"head", "face_mouth_14"}, + {"head", "face_mouth_15"}, + {"head", "face_mouth_16"}, + {"head", "face_mouth_17"}, + {"head", "face_mouth_18"}, + {"head", "face_mouth_19"}, + {"head", "face_mouth_20"}, + {"wrist_left", "hand_wrist_left"}, + {"wrist_left", "hand_finger_thumb_left_1"}, + {"wrist_left", "hand_finger_thumb_left_2"}, + {"wrist_left", "hand_finger_thumb_left_3"}, + {"wrist_left", "hand_finger_thumb_left_4"}, + {"wrist_left", "hand_finger_index_left_1"}, + {"wrist_left", "hand_finger_index_left_2"}, + {"wrist_left", "hand_finger_index_left_3"}, + {"wrist_left", "hand_finger_index_left_4"}, + {"wrist_left", "hand_finger_middle_left_1"}, + {"wrist_left", "hand_finger_middle_left_2"}, + {"wrist_left", "hand_finger_middle_left_3"}, + {"wrist_left", "hand_finger_middle_left_4"}, + {"wrist_left", "hand_finger_ring_left_1"}, + {"wrist_left", "hand_finger_ring_left_2"}, + {"wrist_left", "hand_finger_ring_left_3"}, + {"wrist_left", "hand_finger_ring_left_4"}, + {"wrist_left", "hand_finger_pinky_left_1"}, + {"wrist_left", "hand_finger_pinky_left_2"}, + {"wrist_left", "hand_finger_pinky_left_3"}, + {"wrist_left", "hand_finger_pinky_left_4"}, + {"wrist_right", "hand_wrist_right"}, + {"wrist_right", "hand_finger_thumb_right_1"}, + {"wrist_right", "hand_finger_thumb_right_2"}, + {"wrist_right", "hand_finger_thumb_right_3"}, + {"wrist_right", "hand_finger_thumb_right_4"}, + {"wrist_right", "hand_finger_index_right_1"}, + {"wrist_right", "hand_finger_index_right_2"}, + {"wrist_right", "hand_finger_index_right_3"}, + {"wrist_right", "hand_finger_index_right_4"}, + {"wrist_right", "hand_finger_middle_right_1"}, + {"wrist_right", "hand_finger_middle_right_2"}, + {"wrist_right", "hand_finger_middle_right_3"}, + {"wrist_right", "hand_finger_middle_right_4"}, + {"wrist_right", "hand_finger_ring_right_1"}, + {"wrist_right", "hand_finger_ring_right_2"}, + {"wrist_right", "hand_finger_ring_right_3"}, + {"wrist_right", "hand_finger_ring_right_4"}, + {"wrist_right", "hand_finger_pinky_right_1"}, + {"wrist_right", "hand_finger_pinky_right_2"}, + {"wrist_right", "hand_finger_pinky_right_3"}, + {"wrist_right", "hand_finger_pinky_right_4"}, }; for (auto &pair : pc_connections) { @@ -180,11 +297,6 @@ std::vector SkeletonTFPublisher::generate_ break; } } - if (parent_idx < 0) - { - // Should never happen if cp_map is consistent - continue; - } if (parent_name != "hip_middle" && body[parent_idx][3] <= 0.0f) { // Parent not visible, skip this child