├── .github └── workflows │ └── ROS.yml ├── .gitmodules ├── LICENSE ├── README.md ├── motion_capture_tracking ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── cfg.yaml │ └── rviz.rviz ├── launch │ ├── launch.py │ └── node.launch ├── package.xml └── src │ └── motion_capture_tracking_node.cpp └── motion_capture_tracking_interfaces ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg ├── NamedPose.msg └── NamedPoseArray.msg └── package.xml /.github/workflows/ROS.yml: -------------------------------------------------------------------------------- 1 | name: ROS2 2 | 3 | on: 4 | push: 5 | branches: [ ros2 ] 6 | pull_request: 7 | branches: [ ros2 ] 8 | 9 | # Based on example provided at https://github.com/ros-tooling/setup-ros 10 | 11 | jobs: 12 | # build: # Docker is not supported on macOS and Windows. 13 | # runs-on: ${{ matrix.os }} 14 | # strategy: 15 | # matrix: 16 | # os: [macOS-latest, windows-latest] 17 | # ros_distribution: 18 | # - galactic 19 | # steps: 20 | # - uses: ros-tooling/setup-ros@v0.2 21 | # with: 22 | # required-ros-distributions: ${{ matrix.ros_distribution }} 23 | # - name: build and test 24 | # uses: ros-tooling/action-ros-ci@v0.2 25 | # with: 26 | # package-name: crazyswarm 27 | # target-ros2-distro: ${{ matrix.ros_distribution }} 28 | 29 | build_docker: # On Linux, use docker 30 | runs-on: ubuntu-latest 31 | strategy: 32 | matrix: 33 | ros_distribution: 34 | - humble 35 | - jazzy 36 | 37 | # Define the Docker image(s) associated with each ROS distribution. 38 | include: 39 | # Humble Hawksbill (May 2022 - May 2027) 40 | - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest 41 | ros_distribution: humble 42 | ros_version: 2 43 | 44 | # Jazzy Jalisco (May 2024 - May 2029) 45 | - docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest 46 | ros_distribution: jazzy 47 | ros_version: 2 48 | 49 | container: 50 | image: ${{ matrix.docker_image }} 51 | steps: 52 | - name: build and test ROS 2 53 | uses: ros-tooling/action-ros-ci@v0.4 54 | with: 55 | package-name: motion_capture_tracking 56 | target-ros2-distro: ${{ matrix.ros_distribution }} 57 | skip-tests: true 58 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "deps/libmotioncapture"] 2 | path = motion_capture_tracking/deps/libmotioncapture 3 | url = https://github.com/IMRCLab/libmotioncapture.git 4 | [submodule "deps/librigidbodytracker"] 5 | path = motion_capture_tracking/deps/librigidbodytracker 6 | url = https://github.com/IMRCLab/librigidbodytracker.git 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Wolfgang Hönig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![ROS2](https://github.com/IMRCLab/motion_capture_tracking/actions/workflows/ROS.yml/badge.svg?branch=ros2)](https://github.com/IMRCLab/motion_capture_tracking/actions/workflows/ROS.yml) 2 | 3 | # motion_capture_tracking 4 | 5 | This repository is a ROS 2 package that can receive data from various motion capture systems: 6 | 7 | - VICON 8 | - Qualisys 9 | - OptiTrack 10 | - VRPN 11 | - NOKOV 12 | - FZMotion 13 | - Motion Analysis 14 | 15 | For most systems, different tracking modes are available: 16 | 17 | 1. Tracking of rigid body poses via the official software (e.g., Vicon Tracker) using **unique marker arrangements**. 18 | 2. Tracking of rigid body poses with custom frame-to-frame tracking with **identical marker arrangements**. 19 | 3. Tracking of **single unlabeled marker** positions using custom frame-to-frame tracking. 20 | 21 | The different modes can be combined, e.g., one can use the official software for some rigid bodies and the custom frame-by-frame tracking for others. 22 | 23 | The data is directly published via tf2 and a `/poses` topic that supports different QoS settings. 24 | 25 | This package was originally developed for [Crazyswarm](https://imrclab.github.io/crazyswarm2/) to track up to 50 flying robots. 26 | 27 | ## Building 28 | 29 | To build from source, clone the latest version from this repository into your ROS 2 workspace and compile the package using 30 | 31 | ``` 32 | cd ros_ws/src 33 | git clone --recurse-submodules https://github.com/IMRCLab/motion_capture_tracking 34 | cd ../ 35 | colcon build 36 | ``` 37 | 38 | ## Usage 39 | 40 | ``` 41 | ros2 launch motion_capture_tracking launch.py 42 | ``` 43 | 44 | The various options can be configured in `config/cfg.yaml`. 45 | 46 | ### Choice of Tracking Mode 47 | 48 | #### Unique Marker Arrangements 49 | 50 | With a unique marker arrangement for each rigid body, you rely on the motion capture vendor's software. 51 | This is generally preferred. However, if you have lots of rigid bodies, it can be hard to design enough unique configurations – there are not many places to put a marker on small rigid bodies. 52 | 53 | If your arrangements are too similar, motion capture software may not fail gracefully. For example, it may rapidly switch back and forth between recognizing two different objects at a single physical location. 54 | 55 | To use this mode, simply enable the rigid body / asset in your vendor's software and do *not* include it in the `rigid_bodies` section of `cfg.yaml`. 56 | 57 | #### Identical Marker Arrangement 58 | 59 | If more than one rigid body has the same marker arrangement, standard motion capture software will refuse to track them. Instead, motion_capture_tracking can use the raw point cloud from the motion capture system and track the rigid bodies frame-by-frame. Here we use Iterative Closest Point (ICP) to greedily match the known marker arrangements to the pointcloud. There are two main consequences of this option: 60 | 61 | - The initial positions of the rigid bodies must be known, to establish a mapping between the name and physical locations. 62 | 63 | - The tracking is done frame-by-frame, so if markers are occluded for a significant amount of time, the algorithm may not be able to re-establish the ID-location mapping once they are visible again. 64 | 65 | To use this mode, disable the rigid body in your vendor's software and add an entry in the `rigid_bodies` section of `cfg.yaml`. Note that you'll need to specify the marker configuration (i.e., where the markers are placed in the local coordinate frame of the robot) and a dynamics configuration (first-order dynamics limits for filtering outliers). 66 | 67 | #### Single Marker Arrangement 68 | 69 | A special case of duplicated marker arrangements is the case where we only use a single marker per robot. As before, motion_capture_tracking will use the raw point cloud from the motion capture system and track the CFs frame-by-frame. In this mode, we use optimal task assignment at every frame, which makes this mode more robust to motion capture outliers compared to the duplicate marker arrangements. The main disadvantage is that the yaw angle cannot be observed without moving in the xy-plane. 70 | 71 | #### Hybrid Mode 72 | 73 | The three different modes can be mixed. Note that when mixing identical marker arrangement and single marker arrangement, a different backend for tracking is used, which might be computationally slower and is generally less well tested. 74 | 75 | ### Vendor-specific Instructions 76 | 77 | ### Optitrack 78 | 79 | There are two possible backends. 80 | 81 | * "optitrack" uses the Direct Depacketizers option. This works on all platforms, but often has compatibility issues with untested Motive versions and doesn't support all features. 82 | * "optitrack_closed_source" uses the official SDK (version 4.1.0) (only available on x64 Linux; distributed as a binary library) 83 | 84 | Make sure that you have the following settings in Motive: 85 | 86 | menu Edit/Settings/Streaming: 87 | * Enable NatNet 88 | * Use "Transmission Type" Multicast 89 | * Enable Unlabeled Markers and Rigid Bodies 90 | * Use "Up Axis": Z-Axis 91 | * Use the default ports (1510 command, 1511 data) 92 | 93 | We recommend that you first try "optitrack" and switch to "optitrack_closed_source" if you encounter any issues. 94 | 95 | ### NOKOV 96 | 97 | Since the SDK is not publically available, adding the SDK and building from source is required. 98 | 99 | 1. Place the SDK in `motion_capture_tracking/deps/libmotioncapture/deps/nokov_sdk` (such that there is for example the file `motion_capture_tracking/deps/libmotioncapture/deps/nokov_sdk/lib/libSeekerSDKClient.so`) 100 | 2. In `motion_capture_tracking/CMakeLists.txt` change `set(LIBMOTIONCAPTURE_ENABLE_NOKOV OFF)` to `set(LIBMOTIONCAPTURE_ENABLE_NOKOV ON)` 101 | 3. Rebuild using `colcon build` 102 | 103 | ## Technical Background 104 | 105 | The ROS package is a wrapper around [libmotioncapture](https://github.com/IMRCLab/libmotioncapture) and [librigidbodytracker](https://github.com/IMRCLab/librigidbodytracker). 106 | The former is a C++ library that provides a unified interface over different motion capture SDKs to gather poses of rigid bodies and/or pointclouds of unlabeled markers. 107 | The latter is a C++ library that takes the following inputs: i) a first-order dynamics model, ii) initial poses of rigid bodies, and iii) at every frame a point cloud. It outputs for every frame the best estimate of the robots' poses. 108 | 109 | Some more information on the rigid body pose tracking is available in 110 | 111 | ``` 112 | @inproceedings{crazyswarm, 113 | author = {James A. Preiss* and 114 | Wolfgang H\"onig* and 115 | Gaurav S. Sukhatme and 116 | Nora Ayanian}, 117 | title = {Crazyswarm: {A} large nano-quadcopter swarm}, 118 | booktitle = {{IEEE} International Conference on Robotics and Automation ({ICRA})}, 119 | pages = {3299--3304}, 120 | publisher = {{IEEE}}, 121 | year = {2017}, 122 | url = {https://doi.org/10.1109/ICRA.2017.7989376}, 123 | doi = {10.1109/ICRA.2017.7989376}, 124 | note = {Software available at \url{https://github.com/USC-ACTLab/crazyswarm}}, 125 | } 126 | ``` 127 | 128 | Information about unlabeled marker tracking (optimal assignment with a min-cost max-flow formulation for each frame) and hybrid tracking (CBS-based optimization) is available in 129 | 130 | ``` 131 | @article{cai2025tracking, 132 | title={Optimal Assignment for Multi-Robot Tracking Using Motion Capture Systems}, 133 | author={Nan Cai and 134 | Wolfgang Hönig}, 135 | journal={1st German Robotics Conference}, 136 | code={https://github.com/IMRCLab/motion_capture_tracking}, 137 | pdf={https://imrclab.github.io/assets/pdf/cai2025tracking.pdf}, 138 | year={2025}, 139 | } 140 | ``` 141 | 142 | ## Related Work 143 | 144 | These are current alternatives if no custom tracking is needed: 145 | 146 | - https://github.com/MOCAP4ROS2-Project (VRPN, Vicon, Optitrack, Qualisys, Technaid each in separate packages) 147 | - https://github.com/ros-drivers/mocap_optitrack (only Optitrack; Direct Depacketizers) 148 | - https://github.com/alvinsunyixiao/vrpn_mocap (only VRPN) 149 | - https://github.com/ros-drivers/vrpn_client_ros (only VRPN) 150 | -------------------------------------------------------------------------------- /motion_capture_tracking/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package motion_capture_tracking 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.5 (2024-03-12) 6 | ------------------ 7 | * Install libNatNet.so only on x64 Linux 8 | * Should fix ROS 2 build farm errors. 9 | * Contributors: Wolfgang Hoenig 10 | 11 | 1.0.4 (2024-03-06) 12 | ------------------ 13 | * install libNatNet.so as part of package 14 | Fixes `#14 `_ 15 | * Add support for the "mock" motion capture type 16 | In "mock" mode, the rigid bodies defined in the cfg.yaml will be published at a fixed rate. This is useful for testing without access to a motion capture system. 17 | * cfg: remove unused "mode" 18 | Mode is not being used in the code, and is therefore removed from the config. 19 | * Contributors: Wolfgang Hoenig 20 | 21 | 1.0.3 (2024-01-29) 22 | ------------------ 23 | 24 | 1.0.2 (2024-01-23) 25 | ------------------ 26 | 27 | 1.0.1 (2024-01-20) 28 | ------------------ 29 | * initial release 30 | * Contributors: Wolfgang Hönig 31 | -------------------------------------------------------------------------------- /motion_capture_tracking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_capture_tracking) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | set(CMAKE_INSTALL_RPATH "$ORIGIN") 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(tf2_ros REQUIRED) 14 | find_package(sensor_msgs REQUIRED) 15 | find_package(motion_capture_tracking_interfaces REQUIRED) 16 | 17 | find_package(eigen3_cmake_module REQUIRED) 18 | find_package(Eigen3 REQUIRED) 19 | find_package(PCL REQUIRED COMPONENTS common) 20 | find_package(fmt REQUIRED) 21 | 22 | # add dependencies 23 | add_subdirectory(deps/librigidbodytracker EXCLUDE_FROM_ALL) 24 | set(LIBMOTIONCAPTURE_BUILD_PYTHON_BINDINGS OFF) 25 | set(LIBMOTIONCAPTURE_BUILD_EXAMPLE OFF) 26 | 27 | set(LIBMOTIONCAPTURE_ENABLE_QUALISYS ON) 28 | set(LIBMOTIONCAPTURE_ENABLE_OPTITRACK ON) 29 | set(LIBMOTIONCAPTURE_ENABLE_OPTITRACK_CLOSED_SOURCE ON) 30 | set(LIBMOTIONCAPTURE_ENABLE_VICON ON) 31 | set(LIBMOTIONCAPTURE_ENABLE_VRPN ON) 32 | set(LIBMOTIONCAPTURE_ENABLE_FZMOTION ON) 33 | 34 | # To use NOKOV, place their SDK in 35 | # motion_capture_tracking/deps/libmotioncapture/deps/nokov_sdk 36 | # and set this variable to "ON" 37 | set(LIBMOTIONCAPTURE_ENABLE_NOKOV OFF) 38 | 39 | # To use Motion Analysis, place their SDK in 40 | # motion_capture_tracking/deps/libmotioncapture/deps/cortex_sdk_linux 41 | # and set this variable to "ON" 42 | set(LIBMOTIONCAPTURE_ENABLE_MOTIONANALYSIS OFF) 43 | 44 | add_subdirectory(deps/libmotioncapture EXCLUDE_FROM_ALL) 45 | 46 | include_directories( 47 | deps/librigidbodytracker/include 48 | deps/libmotioncapture/include 49 | ${EIGEN3_INCLUDE_DIRS} 50 | ${PCL_INCLUDE_DIRS} 51 | ) 52 | 53 | add_executable(motion_capture_tracking_node 54 | src/motion_capture_tracking_node.cpp 55 | ) 56 | target_link_libraries(motion_capture_tracking_node 57 | librigidbodytracker 58 | libmotioncapture 59 | fmt::fmt 60 | ) 61 | target_link_libraries(motion_capture_tracking_node 62 | rclcpp::rclcpp 63 | tf2_ros::tf2_ros 64 | ${sensor_msgs_TARGETS} 65 | ${motion_capture_tracking_interfaces_TARGETS} 66 | ) 67 | target_include_directories(motion_capture_tracking_node PUBLIC 68 | $ 69 | $) 70 | target_compile_features(motion_capture_tracking_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 71 | 72 | install(TARGETS motion_capture_tracking_node 73 | DESTINATION lib/${PROJECT_NAME}) 74 | 75 | # The precompiled libNatNet library is only available for x64 Linux 76 | if ((CMAKE_SYSTEM_NAME STREQUAL "Linux") AND (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64")) 77 | install(FILES 78 | ${CMAKE_CURRENT_SOURCE_DIR}/deps/libmotioncapture/deps/NatNetSDKCrossplatform/lib/ubuntu/libNatNet.so 79 | DESTINATION lib/${PROJECT_NAME} 80 | ) 81 | endif() 82 | 83 | # if(BUILD_TESTING) 84 | # find_package(ament_lint_auto REQUIRED) 85 | # # the following line skips the linter which checks for copyrights 86 | # # uncomment the line when a copyright and license is not present in all source files 87 | # #set(ament_cmake_copyright_FOUND TRUE) 88 | # # the following line skips cpplint (only works in a git repo) 89 | # # uncomment the line when this package is not in a git repo 90 | # #set(ament_cmake_cpplint_FOUND TRUE) 91 | # ament_lint_auto_find_test_dependencies() 92 | # endif() 93 | 94 | # Install launch files. 95 | install(DIRECTORY 96 | launch 97 | DESTINATION share/${PROJECT_NAME}/ 98 | ) 99 | 100 | # Install config files. 101 | install(DIRECTORY 102 | config 103 | DESTINATION share/${PROJECT_NAME}/ 104 | ) 105 | 106 | ament_package() 107 | -------------------------------------------------------------------------------- /motion_capture_tracking/config/cfg.yaml: -------------------------------------------------------------------------------- 1 | /motion_capture_tracking: 2 | ros__parameters: 3 | # one of "optitrack", "optitrack_closed_source", "vicon", "qualisys", "nokov", "vrpn", "motionanalysis" 4 | type: "optitrack" 5 | # Specify the hostname or IP of the computer running the motion capture software 6 | hostname: "130.149.82.37" 7 | 8 | # The information will always be broadcasted via tf2 9 | # Specify additional settings for the /poses topic here 10 | topics: 11 | frame_id: "mocap" 12 | poses: 13 | qos: 14 | mode: "none" # one of "none" or "sensor" 15 | deadline: 100.0 # Hz 16 | tf: 17 | reference_frame: "mocap" 18 | child_frame_fmt: "%s_mocap" 19 | 20 | # If desired, log the pointcloud to a binary log file for analysis with libRigidBodyTracker 21 | # Use "" to disable. 22 | logfilepath: '' 23 | 24 | # Define the placement of spherical markers on rigid bodies, in the local coordinate frame. 25 | marker_configurations: 26 | default: # for standard Crazyflie 27 | offset: [0.0, 0.0, 0.0] 28 | points: 29 | p0: [ 0.0, 0.0, 0.022] # top 30 | p1: [-0.042, 0.042, 0.0 ] # back left (M3) 31 | p2: [-0.042, -0.042, 0.0 ] # back right (M2) 32 | p3: [ 0.042, -0.042, 0.0 ] # front right (M1) 33 | default_single_marker: 34 | offset: [0.0, -0.01, -0.04] 35 | points: 36 | p0: [0.0177184,0.0139654,0.0557585] 37 | medium_frame: 38 | offset: [0.0, 0.0, -0.03] 39 | points: 40 | p0: [-0.00896228,-0.000716753,0.0716129] 41 | p1: [-0.0156318,0.0997402,0.0508162] 42 | p2: [0.0461693,-0.0881012,0.0380672] 43 | p3: [-0.0789959,-0.0269793,0.0461144] 44 | big_frame: 45 | offset: [0.0, 0.0, -0.06] 46 | points: 47 | p0: [0.0558163,-0.00196302,0.0945539] 48 | p1: [-0.0113941,0.00945842,0.0984811] 49 | p2: [-0.0306277,0.0514879,0.0520456] 50 | p3: [0.0535816,-0.0400775,0.0432799] 51 | 52 | # Define the first-order dynamic limits 53 | dynamics_configurations: 54 | default: 55 | max_velocity: [2, 2, 3] # m/s 56 | max_angular_velocity: [20, 20, 10] # rad/s 57 | max_roll: 1.4 #rad 58 | max_pitch: 1.4 #rad 59 | max_fitness_score: 0.001 60 | 61 | # Define your rigid bodies to track with known initial position guess, name of the marker and name of the dynamics 62 | # from the above settings. 63 | rigid_bodies: 64 | crazyflie: 65 | initial_position: [0,0,0] 66 | marker: "default" 67 | # marker: "default_single_marker" 68 | dynamics: "default" -------------------------------------------------------------------------------- /motion_capture_tracking/config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 549 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: PointCloud2 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Autocompute Intensity Bounds: true 52 | Autocompute Value Bounds: 53 | Max Value: 10 54 | Min Value: -10 55 | Value: true 56 | Axis: Z 57 | Channel Name: intensity 58 | Class: rviz_default_plugins/PointCloud2 59 | Color: 255; 255; 255 60 | Color Transformer: Intensity 61 | Decay Time: 0 62 | Enabled: true 63 | Invert Rainbow: false 64 | Max Color: 255; 255; 255 65 | Max Intensity: 4096 66 | Min Color: 0; 0; 0 67 | Min Intensity: 0 68 | Name: PointCloud2 69 | Position Transformer: XYZ 70 | Selectable: true 71 | Size (Pixels): 3 72 | Size (m): 0.009999999776482582 73 | Style: Flat Squares 74 | Topic: 75 | Depth: 5 76 | Durability Policy: Volatile 77 | Filter size: 10 78 | History Policy: Keep Last 79 | Reliability Policy: Reliable 80 | Value: /pointCloud 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: true 84 | - Class: rviz_default_plugins/TF 85 | Enabled: true 86 | Frame Timeout: 15 87 | Frames: 88 | All Enabled: true 89 | crazyflie: 90 | Value: true 91 | world: 92 | Value: true 93 | Marker Scale: 1 94 | Name: TF 95 | Show Arrows: true 96 | Show Axes: true 97 | Show Names: true 98 | Tree: 99 | world: 100 | crazyflie: 101 | {} 102 | Update Interval: 0 103 | Value: true 104 | Enabled: true 105 | Global Options: 106 | Background Color: 48; 48; 48 107 | Fixed Frame: world 108 | Frame Rate: 30 109 | Name: root 110 | Tools: 111 | - Class: rviz_default_plugins/Interact 112 | Hide Inactive Objects: true 113 | - Class: rviz_default_plugins/MoveCamera 114 | - Class: rviz_default_plugins/Select 115 | - Class: rviz_default_plugins/FocusCamera 116 | - Class: rviz_default_plugins/Measure 117 | Line color: 128; 128; 0 118 | - Class: rviz_default_plugins/SetInitialPose 119 | Covariance x: 0.25 120 | Covariance y: 0.25 121 | Covariance yaw: 0.06853891909122467 122 | Topic: 123 | Depth: 5 124 | Durability Policy: Volatile 125 | History Policy: Keep Last 126 | Reliability Policy: Reliable 127 | Value: /initialpose 128 | - Class: rviz_default_plugins/SetGoal 129 | Topic: 130 | Depth: 5 131 | Durability Policy: Volatile 132 | History Policy: Keep Last 133 | Reliability Policy: Reliable 134 | Value: /goal_pose 135 | - Class: rviz_default_plugins/PublishPoint 136 | Single click: true 137 | Topic: 138 | Depth: 5 139 | Durability Policy: Volatile 140 | History Policy: Keep Last 141 | Reliability Policy: Reliable 142 | Value: /clicked_point 143 | Transformation: 144 | Current: 145 | Class: rviz_default_plugins/TF 146 | Value: true 147 | Views: 148 | Current: 149 | Class: rviz_default_plugins/Orbit 150 | Distance: 2.0959959030151367 151 | Enable Stereo Rendering: 152 | Stereo Eye Separation: 0.05999999865889549 153 | Stereo Focal Distance: 1 154 | Swap Stereo Eyes: false 155 | Value: false 156 | Focal Point: 157 | X: 0 158 | Y: 0 159 | Z: 0 160 | Focal Shape Fixed Size: true 161 | Focal Shape Size: 0.05000000074505806 162 | Invert Z Axis: false 163 | Name: Current View 164 | Near Clip Distance: 0.009999999776482582 165 | Pitch: 0.3953985869884491 166 | Target Frame: 167 | Value: Orbit (rviz) 168 | Yaw: 2.855405330657959 169 | Saved: ~ 170 | Window Geometry: 171 | Displays: 172 | collapsed: false 173 | Height: 846 174 | Hide Left Dock: false 175 | Hide Right Dock: false 176 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 177 | Selection: 178 | collapsed: false 179 | Time: 180 | collapsed: false 181 | Tool Properties: 182 | collapsed: false 183 | Views: 184 | collapsed: false 185 | Width: 1200 186 | X: 1215 187 | Y: 303 188 | -------------------------------------------------------------------------------- /motion_capture_tracking/launch/launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description(): 8 | rviz_config = os.path.join( 9 | get_package_share_directory('motion_capture_tracking'), 10 | 'config', 11 | 'rviz.rviz') 12 | 13 | node_config = os.path.join( 14 | get_package_share_directory('motion_capture_tracking'), 15 | 'config', 16 | 'cfg.yaml') 17 | 18 | return LaunchDescription([ 19 | Node( 20 | package='motion_capture_tracking', 21 | executable='motion_capture_tracking_node', 22 | name='motion_capture_tracking', 23 | output='screen', 24 | parameters=[node_config] 25 | ), 26 | Node( 27 | package='rviz2', 28 | executable='rviz2', 29 | name='rviz2', 30 | arguments=['-d', rviz_config] 31 | ) 32 | ]) 33 | -------------------------------------------------------------------------------- /motion_capture_tracking/launch/node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | # Tracking 7 | motion_capture_type: "qualisys" # one of vicon,optitrack,qualisys,vrpn 8 | motion_capture_hostname: "localhost" 9 | object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker 10 | 11 | save_point_clouds_path: "" # leave empty to not write point cloud to file 12 | 13 | numMarkerConfigurations: 1 14 | markerConfigurations: 15 | "0": # active marker deck 16 | numPoints: 4 17 | offset: [0.0, 0.0, 0.0] 18 | points: 19 | "0": [ 0.035, 0.000,0.000] # front 20 | "1": [ 0.000, 0.035,0.000] # left 21 | "2": [-0.035, 0.000,0.000] # back 22 | "3": [ 0.000,-0.035,0.000] # right 23 | 24 | numDynamicsConfigurations: 1 25 | dynamicsConfigurations: 26 | "0": 27 | maxXVelocity: 2.0 28 | maxYVelocity: 2.0 29 | maxZVelocity: 3.0 30 | maxPitchRate: 20.0 31 | maxRollRate: 20.0 32 | maxYawRate: 10.0 33 | maxRoll: 1.4 34 | maxPitch: 1.4 35 | maxFitnessScore: 0.001 36 | 37 | objects: 38 | - name: crazyflie 39 | initialPosition: [0.0,0.0,0.0] 40 | markerConfiguration: 0 41 | dynamicsConfiguration: 0 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /motion_capture_tracking/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_capture_tracking 5 | 1.0.5 6 | ROS Package for different motion capture systems, including custom rigid body tracking support 7 | Wolfgang Hönig 8 | MIT 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | tf2_ros 14 | sensor_msgs 15 | eigen 16 | eigen3_cmake_module 17 | libpcl-all-dev 18 | motion_capture_tracking_interfaces 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /motion_capture_tracking/src/motion_capture_tracking_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // ROS 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // Motion Capture 12 | #include 13 | 14 | // Rigid Body tracker 15 | #include 16 | #include 17 | 18 | void logWarn(rclcpp::Logger logger, const std::string& msg) 19 | { 20 | RCLCPP_WARN(logger, "%s", msg.c_str()); 21 | } 22 | 23 | std::set extract_names( 24 | const std::map ¶meter_overrides, 25 | const std::string& pattern) 26 | { 27 | std::set result; 28 | for (const auto &i : parameter_overrides) 29 | { 30 | if (i.first.find(pattern) == 0) 31 | { 32 | size_t start = pattern.size() + 1; 33 | size_t end = i.first.find(".", start); 34 | result.insert(i.first.substr(start, end - start)); 35 | } 36 | } 37 | return result; 38 | } 39 | 40 | std::vector get_vec(const rclcpp::ParameterValue& param_value) 41 | { 42 | if (param_value.get_type() == rclcpp::PARAMETER_INTEGER_ARRAY) { 43 | const auto int_vec = param_value.get>(); 44 | std::vector result; 45 | for (int v : int_vec) { 46 | result.push_back(v); 47 | } 48 | return result; 49 | } 50 | return param_value.get>(); 51 | } 52 | 53 | int main(int argc, char **argv) 54 | { 55 | rclcpp::init(argc, argv); 56 | auto node = rclcpp::Node::make_shared("motion_capture_tracking_node"); 57 | node->declare_parameter("type", "vicon"); 58 | node->declare_parameter("hostname", "localhost"); 59 | node->declare_parameter("topics.frame_id", "world"); 60 | node->declare_parameter("topics.poses.qos.mode", "none"); 61 | node->declare_parameter("topics.poses.qos.deadline", 100.0); 62 | node->declare_parameter("topics.tf.child_frame_id", "{}"); 63 | 64 | node->declare_parameter("logfilepath", ""); 65 | 66 | std::string motionCaptureType = node->get_parameter("type").as_string(); 67 | std::string motionCaptureHostname = node->get_parameter("hostname").as_string(); 68 | std::string frame_id = node->get_parameter("topics.frame_id").as_string(); 69 | std::string poses_qos = node->get_parameter("topics.poses.qos.mode").as_string(); 70 | double poses_deadline = node->get_parameter("topics.poses.qos.deadline").as_double(); 71 | std::string tf_child_frame_id = node->get_parameter("topics.tf.child_frame_id").as_string(); 72 | std::string logFilePath = node->get_parameter("logfilepath").as_string(); 73 | 74 | auto node_parameters_iface = node->get_node_parameters_interface(); 75 | const std::map ¶meter_overrides = 76 | node_parameters_iface->get_parameter_overrides(); 77 | 78 | librigidbodytracker::PointCloudLogger pointCloudLogger(logFilePath); 79 | const bool logClouds = !logFilePath.empty(); 80 | std::cout << "logClouds=" < cfg; 84 | cfg["hostname"] = motionCaptureHostname; 85 | 86 | // if the mock type is selected, add the defined rigid bodies 87 | if (motionCaptureType == "mock") { 88 | auto rigid_body_names = extract_names(parameter_overrides, "rigid_bodies"); 89 | for (const auto &name : rigid_body_names) 90 | { 91 | const auto pos = get_vec(parameter_overrides.at("rigid_bodies." + name + ".initial_position")); 92 | cfg["rigid_bodies"] += name + "(" + std::to_string(pos[0]) + "," + std::to_string(pos[1]) + "," + std::to_string(pos[2]) +",1,0,0,0);"; 93 | } 94 | } 95 | 96 | libmotioncapture::MotionCapture *mocap = libmotioncapture::MotionCapture::connect(motionCaptureType, cfg); 97 | 98 | // prepare point cloud publisher 99 | auto pubPointCloud = node->create_publisher("pointCloud", 1); 100 | 101 | sensor_msgs::msg::PointCloud2 msgPointCloud; 102 | msgPointCloud.header.frame_id = frame_id; 103 | msgPointCloud.height = 1; 104 | 105 | sensor_msgs::msg::PointField field; 106 | field.name = "x"; 107 | field.offset = 0; 108 | field.datatype = sensor_msgs::msg::PointField::FLOAT32; 109 | field.count = 1; 110 | msgPointCloud.fields.push_back(field); 111 | field.name = "y"; 112 | field.offset = 4; 113 | msgPointCloud.fields.push_back(field); 114 | field.name = "z"; 115 | field.offset = 8; 116 | msgPointCloud.fields.push_back(field); 117 | msgPointCloud.point_step = 12; 118 | msgPointCloud.is_bigendian = false; 119 | msgPointCloud.is_dense = true; 120 | 121 | // prepare pose array publisher 122 | rclcpp::Publisher::SharedPtr pubPoses; 123 | if (poses_qos == "none") { 124 | pubPoses = node->create_publisher("poses", 1); 125 | } else if (poses_qos == "sensor") { 126 | rclcpp::SensorDataQoS sensor_data_qos; 127 | sensor_data_qos.keep_last(1); 128 | sensor_data_qos.deadline(rclcpp::Duration(0/*s*/, (int)1e9/poses_deadline /*ns*/)); 129 | pubPoses = node->create_publisher("poses", sensor_data_qos); 130 | } else { 131 | throw std::runtime_error("Unknown QoS mode! " + poses_qos); 132 | } 133 | 134 | motion_capture_tracking_interfaces::msg::NamedPoseArray msgPoses; 135 | msgPoses.header.frame_id = frame_id; 136 | 137 | // prepare rigid body tracker 138 | 139 | auto dynamics_config_names = extract_names(parameter_overrides, "dynamics_configurations"); 140 | std::vector dynamicsConfigurations(dynamics_config_names.size()); 141 | std::map dynamics_name_to_index; 142 | size_t i = 0; 143 | for (const auto& name : dynamics_config_names) { 144 | const auto max_vel = get_vec(parameter_overrides.at("dynamics_configurations." + name + ".max_velocity")); 145 | dynamicsConfigurations[i].maxXVelocity = max_vel.at(0); 146 | dynamicsConfigurations[i].maxYVelocity = max_vel.at(1); 147 | dynamicsConfigurations[i].maxZVelocity = max_vel.at(2); 148 | const auto max_angular_velocity = get_vec(parameter_overrides.at("dynamics_configurations." + name + ".max_angular_velocity")); 149 | dynamicsConfigurations[i].maxRollRate = max_angular_velocity.at(0); 150 | dynamicsConfigurations[i].maxPitchRate = max_angular_velocity.at(1); 151 | dynamicsConfigurations[i].maxYawRate = max_angular_velocity.at(2); 152 | dynamicsConfigurations[i].maxRoll = parameter_overrides.at("dynamics_configurations." + name + ".max_roll").get(); 153 | dynamicsConfigurations[i].maxPitch = parameter_overrides.at("dynamics_configurations." + name + ".max_pitch").get(); 154 | dynamicsConfigurations[i].maxFitnessScore = parameter_overrides.at("dynamics_configurations." + name + ".max_fitness_score").get(); 155 | dynamics_name_to_index[name] = i; 156 | ++i; 157 | } 158 | 159 | auto marker_config_names = extract_names(parameter_overrides, "marker_configurations"); 160 | std::vector markerConfigurations; 161 | std::map marker_name_to_index; 162 | i = 0; 163 | for (const auto &name : marker_config_names) 164 | { 165 | markerConfigurations.push_back(pcl::PointCloud::Ptr(new pcl::PointCloud)); 166 | const auto offset = get_vec(parameter_overrides.at("marker_configurations." + name + ".offset")); 167 | for (const auto ¶m : parameter_overrides) 168 | { 169 | if (param.first.find("marker_configurations." + name + ".points") == 0) 170 | { 171 | const auto points = get_vec(param.second); 172 | markerConfigurations.back()->push_back(pcl::PointXYZ(points[0] + offset[0], points[1] + offset[1], points[2] + offset[2])); 173 | } 174 | } 175 | marker_name_to_index[name] = i; 176 | ++i; 177 | } 178 | 179 | std::vector rigidBodies; 180 | // only add the rigid bodies to the tracker if we are not using the "mock" mode 181 | if (motionCaptureType != "mock") { 182 | auto rigid_body_names = extract_names(parameter_overrides, "rigid_bodies"); 183 | for (const auto &name : rigid_body_names) 184 | { 185 | const auto pos = get_vec(parameter_overrides.at("rigid_bodies." + name + ".initial_position")); 186 | Eigen::Affine3f m; 187 | m = Eigen::Translation3f(pos[0], pos[1], pos[2]); 188 | const auto marker = parameter_overrides.at("rigid_bodies." + name + ".marker").get(); 189 | const auto dynamics = parameter_overrides.at("rigid_bodies." + name + ".dynamics").get(); 190 | 191 | rigidBodies.push_back(librigidbodytracker::RigidBody(marker_name_to_index.at(marker), dynamics_name_to_index.at(dynamics), m, name)); 192 | } 193 | } 194 | 195 | librigidbodytracker::RigidBodyTracker tracker( 196 | dynamicsConfigurations, 197 | markerConfigurations, 198 | rigidBodies); 199 | tracker.setLogWarningCallback(std::bind(logWarn, node->get_logger(), std::placeholders::_1)); 200 | 201 | // prepare TF broadcaster 202 | tf2_ros::TransformBroadcaster tfbroadcaster(node); 203 | std::vector transforms; 204 | 205 | pcl::PointCloud::Ptr markers(new pcl::PointCloud); 206 | 207 | for (size_t frameId = 0; rclcpp::ok(); ++frameId) { 208 | 209 | // Get a frame 210 | mocap->waitForNextFrame(); 211 | auto chrono_now = std::chrono::high_resolution_clock::now(); 212 | auto time = node->now(); 213 | 214 | auto pointcloud = mocap->pointCloud(); 215 | 216 | // publish as pointcloud 217 | msgPointCloud.header.stamp = time; 218 | msgPointCloud.width = pointcloud.rows(); 219 | msgPointCloud.data.resize(pointcloud.rows() * 3 * 4); // width * height * pointstep 220 | memcpy(msgPointCloud.data.data(), pointcloud.data(), msgPointCloud.data.size()); 221 | msgPointCloud.row_step = msgPointCloud.data.size(); 222 | 223 | pubPointCloud->publish(msgPointCloud); 224 | if (logClouds) { 225 | // pointCloudLogger.log(timestamp/1000, markers); // point cloud log format: infinite repetitions of: timestamp (milliseconds) : uint32 226 | // std::cout << "0000000000000before log" << std::endl; 227 | pointCloudLogger.log(markers); 228 | } 229 | 230 | 231 | // run tracker 232 | markers->clear(); 233 | for (long int i = 0; i < pointcloud.rows(); ++i) 234 | { 235 | const auto &point = pointcloud.row(i); 236 | markers->push_back(pcl::PointXYZ(point(0), point(1), point(2))); 237 | } 238 | tracker.update(markers); 239 | 240 | transforms.clear(); 241 | transforms.reserve(mocap->rigidBodies().size()); 242 | for (const auto &iter : mocap->rigidBodies()) 243 | { 244 | const auto& rigidBody = iter.second; 245 | 246 | // const auto& transform = rigidBody.transformation(); 247 | // transforms.emplace_back(eigenToTransform(transform)); 248 | transforms.resize(transforms.size() + 1); 249 | transforms.back().header.stamp = time; 250 | transforms.back().header.frame_id = frame_id; 251 | transforms.back().child_frame_id = rigidBody.name(); 252 | transforms.back().transform.translation.x = rigidBody.position().x(); 253 | transforms.back().transform.translation.y = rigidBody.position().y(); 254 | transforms.back().transform.translation.z = rigidBody.position().z(); 255 | transforms.back().transform.rotation.x = rigidBody.rotation().x(); 256 | transforms.back().transform.rotation.y = rigidBody.rotation().y(); 257 | transforms.back().transform.rotation.z = rigidBody.rotation().z(); 258 | transforms.back().transform.rotation.w = rigidBody.rotation().w(); 259 | } 260 | 261 | for (const auto& rigidBody : tracker.rigidBodies()) 262 | { 263 | if (rigidBody.lastTransformationValid()) 264 | { 265 | const Eigen::Affine3f &transform = rigidBody.transformation(); 266 | Eigen::Quaternionf q(transform.rotation()); 267 | const auto &translation = transform.translation(); 268 | 269 | transforms.resize(transforms.size() + 1); 270 | transforms.back().header.stamp = time; 271 | transforms.back().header.frame_id = frame_id; 272 | transforms.back().child_frame_id = rigidBody.name(); 273 | transforms.back().transform.translation.x = translation.x(); 274 | transforms.back().transform.translation.y = translation.y(); 275 | transforms.back().transform.translation.z = translation.z(); 276 | if (rigidBody.orientationAvailable()) { 277 | transforms.back().transform.rotation.x = q.x(); 278 | transforms.back().transform.rotation.y = q.y(); 279 | transforms.back().transform.rotation.z = q.z(); 280 | transforms.back().transform.rotation.w = q.w(); 281 | } else { 282 | transforms.back().transform.rotation.x = std::nan(""); 283 | transforms.back().transform.rotation.y = std::nan(""); 284 | transforms.back().transform.rotation.z = std::nan(""); 285 | transforms.back().transform.rotation.w = std::nan(""); 286 | } 287 | } 288 | else 289 | { 290 | std::chrono::duration elapsedSeconds = chrono_now - rigidBody.lastValidTime(); 291 | RCLCPP_WARN(node->get_logger(), "No updated pose for %s for %f s.", rigidBody.name().c_str(), elapsedSeconds.count()); 292 | } 293 | } 294 | 295 | if (transforms.size() > 0) { 296 | // publish poses 297 | msgPoses.header.stamp = time; 298 | msgPoses.poses.resize(transforms.size()); 299 | for (size_t i = 0; i < transforms.size(); ++i) { 300 | msgPoses.poses[i].name = transforms[i].child_frame_id; 301 | msgPoses.poses[i].pose.position.x = transforms[i].transform.translation.x; 302 | msgPoses.poses[i].pose.position.y = transforms[i].transform.translation.y; 303 | msgPoses.poses[i].pose.position.z = transforms[i].transform.translation.z; 304 | msgPoses.poses[i].pose.orientation = transforms[i].transform.rotation; 305 | } 306 | pubPoses->publish(msgPoses); 307 | 308 | // send TF 309 | 310 | // Since RViz and others can't handle nan's, report a fake orientation if needed 311 | for (auto& tf : transforms) { 312 | if (std::isnan(tf.transform.rotation.x)) { 313 | tf.transform.rotation.x = 0; 314 | tf.transform.rotation.y = 0; 315 | tf.transform.rotation.z = 0; 316 | tf.transform.rotation.w = 1; 317 | } 318 | } 319 | 320 | // allow custom child_frame_ids before sending 321 | for (auto& tf : transforms) { 322 | std::string name = tf.child_frame_id; 323 | tf.child_frame_id = fmt::format(tf_child_frame_id, name); 324 | } 325 | 326 | tfbroadcaster.sendTransform(transforms); 327 | } 328 | if (logClouds) { 329 | pointCloudLogger.flush(); 330 | } 331 | rclcpp::spin_some(node); 332 | } 333 | 334 | if (logClouds) { 335 | pointCloudLogger.flush(); 336 | } 337 | 338 | return 0; 339 | } 340 | -------------------------------------------------------------------------------- /motion_capture_tracking_interfaces/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package motion_capture_tracking_interfaces 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.5 (2024-03-12) 6 | ------------------ 7 | 8 | 1.0.4 (2024-03-06) 9 | ------------------ 10 | 11 | 1.0.3 (2024-01-29) 12 | ------------------ 13 | * add ament_lint_common test dependency 14 | * Contributors: Wolfgang Hoenig 15 | 16 | 1.0.2 (2024-01-23) 17 | ------------------ 18 | * fix package dependencies in package.xml 19 | * Contributors: Wolfgang Hoenig 20 | 21 | 1.0.1 (2024-01-20) 22 | ------------------ 23 | * initial motion_capture_tracking_interfaces package 24 | * Contributors: Wolfgang Hoenig 25 | -------------------------------------------------------------------------------- /motion_capture_tracking_interfaces/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(motion_capture_tracking_interfaces) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(builtin_interfaces REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | 18 | find_package(rosidl_default_generators REQUIRED) 19 | 20 | rosidl_generate_interfaces(${PROJECT_NAME} 21 | "msg/NamedPose.msg" 22 | "msg/NamedPoseArray.msg" 23 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs 24 | ADD_LINTER_TESTS 25 | ) 26 | 27 | 28 | ament_export_dependencies(rosidl_default_runtime) 29 | 30 | ament_package() 31 | -------------------------------------------------------------------------------- /motion_capture_tracking_interfaces/msg/NamedPose.msg: -------------------------------------------------------------------------------- 1 | string name 2 | geometry_msgs/Pose pose 3 | -------------------------------------------------------------------------------- /motion_capture_tracking_interfaces/msg/NamedPoseArray.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | NamedPose[] poses 3 | -------------------------------------------------------------------------------- /motion_capture_tracking_interfaces/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | motion_capture_tracking_interfaces 5 | 1.0.5 6 | Interfaces for motion_capture_tracking package. 7 | Wolfgang Hönig 8 | MIT 9 | 10 | ament_cmake 11 | rosidl_default_generators 12 | 13 | builtin_interfaces 14 | geometry_msgs 15 | std_msgs 16 | 17 | rosidl_default_generators 18 | 19 | rosidl_default_runtime 20 | 21 | ament_lint_common 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | --------------------------------------------------------------------------------