MotionCapture

ROS2 interface for the OptiTrack Motive motion capture system. Receives rigid-body poses from Motive over NatNet and republishes them as ROS2 topics, ready to be consumed by any controller.

What it provides: position and rotation (13 floats per rigid body). NatNet does not expose rigid-body velocity; this package does not publish one. If your controller needs velocity, derive it from consecutive poses on your side (finite-difference + low-pass, or fuse with encoder-derived velocity — that choice belongs to the controller, not to this package).

This package has no dependency on any particular robot: it reads which rigid bodies exist, which network Motive is on, and at what rate to publish from an external YAML file that you own and supply — see Configuration below. Nothing about a specific robot is baked in here.

Quick start

cd MotionCapture
colcon build --symlink-install
source install/setup.bash
ros2 launch optitrack_streamer streamer.launch.py config_file:=/path/to/your_config.yaml

Leaving off config_file:=... runs the bundled example (src/optitrack_streamer/config/optitrack_config.example.yaml) — useful to confirm the package runs at all, useless for real data, since its rigid-body IDs and IPs are placeholders.

For a calibration session (adds pedal input and RViz2 — requires the separate input_devices package and, currently, a hardcoded RViz config path, see Known issues):

ros2 launch optitrack_streamer calib_optitrack.launch.py config_file:=/path/to/your_config.yaml

Configuration

Copy src/optitrack_streamer/config/optitrack_config.example.yaml into the repo of whatever robot you're running (it's deployment data — a different rig means different rigid-body IDs and a different network — so it belongs there, not here), fill in your real values, and pass it with config_file:=.

rigid_bodies:
  "80": {arm: LEFT,  role: base, topic: /optitrack/BL}
  "81": {arm: LEFT,  role: sec1, topic: /optitrack/L1}
  # ... one entry per Motive rigid-body ID you streamed

natnet_server_ip: 169.254.118.69     # the Motive PC
natnet_client_ip: 169.254.118.70     # this machine
natnet_multicast_address: 239.255.42.99
natnet_use_multicast: false

publish_rate_hz: 120

Field	Meaning
rigid_bodies	Motive rigid-body ID (string, matching what Motive streams) → arm/role label (free text, yours to choose) and the ROS2 topic to publish it on
natnet_server_ip / natnet_client_ip	Direct-connection NatNet addressing — must match Motive's Data Streaming panel
natnet_multicast_address / natnet_use_multicast	Multicast alternative to direct connection
publish_rate_hz	How often each rigid body's latest pose is republished

The topic names are just strings in your YAML — anything consuming them just needs to agree on the same names.

If config_file isn't passed, the node falls back to the bundled example above (so it never crashes for lack of a config) and logs which file it actually loaded — check that line if topics don't show up where you expect.

Published topics

One std_msgs/Float64MultiArray topic per rigid body, named from your config's rigid_bodies map.

Message format — 13 floats:

Index	Content
[0]	Rigid body ID (float, matches the Motive ID)
[1:4]	Position (x, y, z) [m], ROS Z-up frame
[4:13]	Rotation matrix, row-major (3×3), ROS Z-up frame

The Motive Y-up → ROS Z-up conversion is applied inside the node before publishing. No quaternions are exposed on the ROS interface.

Coordinate frames

Frame	Convention
Motive global	Y-up (defined by calibration square)
ROS global	Z-up (REP-103)

The node applies the Motive→ROS axis swap to both position and rotation before publishing: (x, y, z)_motive → (-x, z, y)_ros, with the equivalent quaternion permutation used internally to build the rotation matrix.

Source files

File	Role
src/optitrack_streamer/optitrack_streamer/optitrack_streamer_node.py	ROS2 node — loads the config, receives from NatNet, converts frames, publishes
src/optitrack_streamer/optitrack_streamer/NatNetClient.py	NatNet UDP client (OptiTrack SDK, third-party)
src/optitrack_streamer/optitrack_streamer/MoCapData.py	NatNet frame data structures
src/optitrack_streamer/optitrack_streamer/DataDescriptions.py	NatNet session asset descriptions
src/optitrack_streamer/config/optitrack_config.example.yaml	Template config — copy it, don't edit it in place
src/optitrack_streamer/launch/streamer.launch.py	Normal operation launch
src/optitrack_streamer/launch/calib_optitrack.launch.py	Calibration launch (adds pedals + RViz2)

Known issues

• calib_optitrack.launch.py hardcodes an RViz config path (/home/kaijunge/.rviz2/default.rviz) from the machine it was written on. Override it or edit the launch file for your own machine before using that launch file.
• calib_optitrack.launch.py also depends on a separate input_devices package for pedal input, which isn't part of this repo.

History

Extracted from the BimanualHelyxVMC monorepo (2026-09) so it can be reused by other robots without dragging that repo's controller code along. At the same time, the rigid-body/network config moved from a Python file imported via a hand-rolled sys.path search into the external YAML file described above — the old mechanism only worked because this package lived at a fixed relative path inside that one repo, which stops being true once it's a repo of its own. A wrist_streamer_node referenced in both launch files and in setup.py's entry points was removed at the same time: no such module existed in the source tree, so launching either file would have failed trying to start it.