From c6d87a8878feed07e3557c7f42bece5496e5ddb1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 13 Jul 2026 23:11:31 +0200 Subject: [PATCH 1/3] Add NamedPoseArrayV2 as /poses topic that includes camera timestamps and latency information --- motion_capture_tracking/config/cfg.yaml | 2 + motion_capture_tracking/deps/libmotioncapture | 2 +- .../src/motion_capture_tracking_node.cpp | 67 +++++++++++++++---- .../CMakeLists.txt | 2 + .../msg/LatencyInfo.msg | 2 + .../msg/NamedPoseArrayV2.msg | 4 ++ 6 files changed, 66 insertions(+), 13 deletions(-) create mode 100644 motion_capture_tracking_interfaces/msg/LatencyInfo.msg create mode 100644 motion_capture_tracking_interfaces/msg/NamedPoseArrayV2.msg diff --git a/motion_capture_tracking/config/cfg.yaml b/motion_capture_tracking/config/cfg.yaml index 106c73d..4cd87cb 100644 --- a/motion_capture_tracking/config/cfg.yaml +++ b/motion_capture_tracking/config/cfg.yaml @@ -9,7 +9,9 @@ # Specify additional settings for the /poses topic here topics: frame_id: "mocap" + header_time: "ros" # one of "ros" (current ROS time when message arrives) or "camera" (timestamp from the camera) poses: + version: 2 qos: mode: "none" # one of "none" or "sensor" deadline: 100.0 # Hz diff --git a/motion_capture_tracking/deps/libmotioncapture b/motion_capture_tracking/deps/libmotioncapture index 7d8a6d5..fb74118 160000 --- a/motion_capture_tracking/deps/libmotioncapture +++ b/motion_capture_tracking/deps/libmotioncapture @@ -1 +1 @@ -Subproject commit 7d8a6d5a2e51e174a169014b17204fffc698802b +Subproject commit fb7411826a7328a4404cec5fdcbd673137c89207 diff --git a/motion_capture_tracking/src/motion_capture_tracking_node.cpp b/motion_capture_tracking/src/motion_capture_tracking_node.cpp index ace9626..32445ed 100644 --- a/motion_capture_tracking/src/motion_capture_tracking_node.cpp +++ b/motion_capture_tracking/src/motion_capture_tracking_node.cpp @@ -7,6 +7,7 @@ #include #include #include +#include // Motion Capture #include @@ -57,6 +58,8 @@ int main(int argc, char **argv) node->declare_parameter("type", "vicon"); node->declare_parameter("hostname", "localhost"); node->declare_parameter("topics.frame_id", "world"); + node->declare_parameter("topics.header_time", "ros"); + node->declare_parameter("topics.poses.version", 1); node->declare_parameter("topics.poses.qos.mode", "none"); node->declare_parameter("topics.poses.qos.deadline", 100.0); node->declare_parameter("topics.tf.child_frame_id", "{}"); @@ -66,6 +69,8 @@ int main(int argc, char **argv) std::string motionCaptureType = node->get_parameter("type").as_string(); std::string motionCaptureHostname = node->get_parameter("hostname").as_string(); std::string frame_id = node->get_parameter("topics.frame_id").as_string(); + std::string header_time = node->get_parameter("topics.header_time").as_string(); + uint8_t poses_version = node->get_parameter("topics.poses.version").as_int(); std::string poses_qos = node->get_parameter("topics.poses.qos.mode").as_string(); double poses_deadline = node->get_parameter("topics.poses.qos.deadline").as_double(); std::string tf_child_frame_id = node->get_parameter("topics.tf.child_frame_id").as_string(); @@ -120,13 +125,23 @@ int main(int argc, char **argv) // prepare pose array publisher rclcpp::Publisher::SharedPtr pubPoses; + rclcpp::Publisher::SharedPtr pubPosesV2; + if (poses_qos == "none") { - pubPoses = node->create_publisher("poses", 1); + if (poses_version == 1) { + pubPoses = node->create_publisher("poses", 1); + } else if (poses_version == 2) { + pubPosesV2 = node->create_publisher("poses", 1); + } } else if (poses_qos == "sensor") { rclcpp::SensorDataQoS sensor_data_qos; sensor_data_qos.keep_last(1); sensor_data_qos.deadline(rclcpp::Duration(0/*s*/, (int)1e9/poses_deadline /*ns*/)); - pubPoses = node->create_publisher("poses", sensor_data_qos); + if (poses_version == 1) { + pubPoses = node->create_publisher("poses", sensor_data_qos); + } else if (poses_version == 2) { + pubPosesV2 = node->create_publisher("poses", sensor_data_qos); + } } else { throw std::runtime_error("Unknown QoS mode! " + poses_qos); } @@ -134,6 +149,9 @@ int main(int argc, char **argv) motion_capture_tracking_interfaces::msg::NamedPoseArray msgPoses; msgPoses.header.frame_id = frame_id; + motion_capture_tracking_interfaces::msg::NamedPoseArrayV2 msgPosesV2; + msgPosesV2.header.frame_id = frame_id; + // prepare rigid body tracker auto dynamics_config_names = extract_names(parameter_overrides, "dynamics_configurations"); @@ -209,7 +227,12 @@ int main(int argc, char **argv) // Get a frame mocap->waitForNextFrame(); auto chrono_now = std::chrono::high_resolution_clock::now(); - auto time = node->now(); + rclcpp::Time time; + if (header_time == "ros") { + time = node->now(); + } else if (header_time == "camera") { + time = rclcpp::Time(mocap->timeStamp() * 1000); + } auto pointcloud = mocap->pointCloud(); @@ -294,16 +317,36 @@ int main(int argc, char **argv) if (transforms.size() > 0) { // publish poses - msgPoses.header.stamp = time; - msgPoses.poses.resize(transforms.size()); - for (size_t i = 0; i < transforms.size(); ++i) { - msgPoses.poses[i].name = transforms[i].child_frame_id; - msgPoses.poses[i].pose.position.x = transforms[i].transform.translation.x; - msgPoses.poses[i].pose.position.y = transforms[i].transform.translation.y; - msgPoses.poses[i].pose.position.z = transforms[i].transform.translation.z; - msgPoses.poses[i].pose.orientation = transforms[i].transform.rotation; + if (poses_version == 1) { + msgPoses.header.stamp = time; + msgPoses.poses.resize(transforms.size()); + for (size_t i = 0; i < transforms.size(); ++i) { + msgPoses.poses[i].name = transforms[i].child_frame_id; + msgPoses.poses[i].pose.position.x = transforms[i].transform.translation.x; + msgPoses.poses[i].pose.position.y = transforms[i].transform.translation.y; + msgPoses.poses[i].pose.position.z = transforms[i].transform.translation.z; + msgPoses.poses[i].pose.orientation = transforms[i].transform.rotation; + } + pubPoses->publish(msgPoses); + } else if (poses_version == 2) { + msgPosesV2.header.stamp = time; + msgPosesV2.timestamp = mocap->timeStamp(); + const auto& latencies = mocap->latency(); + msgPosesV2.latencies.resize(latencies.size()); + for (size_t i = 0; i < latencies.size(); ++i) { + msgPosesV2.latencies[i].source = latencies[i].name(); + msgPosesV2.latencies[i].latency = latencies[i].value(); + } + msgPosesV2.poses.resize(transforms.size()); + for (size_t i = 0; i < transforms.size(); ++i) { + msgPosesV2.poses[i].name = transforms[i].child_frame_id; + msgPosesV2.poses[i].pose.position.x = transforms[i].transform.translation.x; + msgPosesV2.poses[i].pose.position.y = transforms[i].transform.translation.y; + msgPosesV2.poses[i].pose.position.z = transforms[i].transform.translation.z; + msgPosesV2.poses[i].pose.orientation = transforms[i].transform.rotation; + } + pubPosesV2->publish(msgPosesV2); } - pubPoses->publish(msgPoses); // send TF diff --git a/motion_capture_tracking_interfaces/CMakeLists.txt b/motion_capture_tracking_interfaces/CMakeLists.txt index 1193034..88e2f20 100644 --- a/motion_capture_tracking_interfaces/CMakeLists.txt +++ b/motion_capture_tracking_interfaces/CMakeLists.txt @@ -18,8 +18,10 @@ find_package(std_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/LatencyInfo.msg" "msg/NamedPose.msg" "msg/NamedPoseArray.msg" + "msg/NamedPoseArrayV2.msg" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs ADD_LINTER_TESTS ) diff --git a/motion_capture_tracking_interfaces/msg/LatencyInfo.msg b/motion_capture_tracking_interfaces/msg/LatencyInfo.msg new file mode 100644 index 0000000..eb60e6b --- /dev/null +++ b/motion_capture_tracking_interfaces/msg/LatencyInfo.msg @@ -0,0 +1,2 @@ +string source +uint32 latency # estimated, in microseconds diff --git a/motion_capture_tracking_interfaces/msg/NamedPoseArrayV2.msg b/motion_capture_tracking_interfaces/msg/NamedPoseArrayV2.msg new file mode 100644 index 0000000..fa9383d --- /dev/null +++ b/motion_capture_tracking_interfaces/msg/NamedPoseArrayV2.msg @@ -0,0 +1,4 @@ +std_msgs/Header header # depending on the setting, the timestamp might be in ROS time when the information arrived or in camera time +uint64 timestamp # vendor specific, in microseconds +LatencyInfo[] latencies +NamedPose[] poses From 3cf5d40a0b74f186a2d4dd0c8ebfc39c199343d1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 17 Jul 2026 14:23:10 +0200 Subject: [PATCH 2/3] fix wrong unit for latency estimates --- motion_capture_tracking/src/motion_capture_tracking_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion_capture_tracking/src/motion_capture_tracking_node.cpp b/motion_capture_tracking/src/motion_capture_tracking_node.cpp index 32445ed..f9d061c 100644 --- a/motion_capture_tracking/src/motion_capture_tracking_node.cpp +++ b/motion_capture_tracking/src/motion_capture_tracking_node.cpp @@ -335,7 +335,7 @@ int main(int argc, char **argv) msgPosesV2.latencies.resize(latencies.size()); for (size_t i = 0; i < latencies.size(); ++i) { msgPosesV2.latencies[i].source = latencies[i].name(); - msgPosesV2.latencies[i].latency = latencies[i].value(); + msgPosesV2.latencies[i].latency = latencies[i].value() * 1e6; } msgPosesV2.poses.resize(transforms.size()); for (size_t i = 0; i < transforms.size(); ++i) { From 917aad0be66a87724ff512d6430dcd3c51f83b1f Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 17 Jul 2026 14:26:10 +0200 Subject: [PATCH 3/3] update submodule --- motion_capture_tracking/deps/libmotioncapture | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motion_capture_tracking/deps/libmotioncapture b/motion_capture_tracking/deps/libmotioncapture index fb74118..24321e4 160000 --- a/motion_capture_tracking/deps/libmotioncapture +++ b/motion_capture_tracking/deps/libmotioncapture @@ -1 +1 @@ -Subproject commit fb7411826a7328a4404cec5fdcbd673137c89207 +Subproject commit 24321e4c1c923a1bc5d6cecdaa7834f11b79d8f2