Include wb-joints in transform publisher.
This commit is contained in:
Daniel
@ -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<geometry_msgs::msg::TransformStamped> 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
|
||||
|
||||
Reference in New Issue
Block a user