l2s-lab / natnet_ros_cpp Goto Github PK

View Code? Open in Web Editor NEW

27.0 2.0 15.0 16.38 MB

ROS driver for the NatNet 4.0 protocol used for the OptiTrack motion capture system and Motive application

License: GNU General Public License v3.0

CMake 8.55% C++ 56.39% Shell 0.82% Python 34.24%

driver natnet natnet-sdk optitrack ros motion-capture

natnet_ros_cpp's Introduction

NatNet 4 ROS driver

if you are using software for any publication or article, we would be delighted if you could cite it from here.

NOTE: This is not maintained a lot. For ROS 2 version visit here. Which is continuation of this work.

Introduction

This package contains a ROS driver for the NatNet protocol used by the OptiTrack motion capture system. It supports NatNet versions 4.0 (Motive 2.2 and higher). The NatNet SDK provided by the optitrack can be found here. It will be downloaded under deps/NatnetSDK while building it for the first time. NatNet protocol is used for streaming live motion capture data (rigid bodies, skeletons etc) across the shared network.

This package is only tested with the Natnet 4.0 and ROS noetic but probably will work with the older versions of both as well.

Current Features:

Stable and with more functionality than mocap_optitrack
Rigid bodies are published as geometry_msgs/PoseStamped under name given in the Motive, i.e /natnet_ros/<body-name>/pose. Plus those are also broadcasting as tf frame for rviz
Markers of the rigid bodies are published ad geometry_msgs/PointStamped unuder the name /natnet_ros/<body-name>/marker#/pose
Unlabeled markers with the initial position and the name mentione in the /config/initiate.yamlare published as geometry_msgs/PoseStamped unuder the name /natnet_ros/<name-in-config-file>/pose. Plus those are also broadcasting as tf frame for rviz. The marker position is updated based on Iterative closest point (nearest neighbour)
Unlabled markers can be also published as sensor_msgs/PointCloud
Different options for publishing and logging the data

Work under progress:

Include Skeleton and other devices in the system to make it package as whole.
Considering position and orientation for similar marker configurations (at least 3 markers)
Considering compensation of time delay (~5-6ms) to the ROS time stamp.
Adding an option for the axis orientation (Z UP or Y UP)
Creating package for ROS2.

How to use it

Building the package

requirements

sudo apt install -y ros-$ROS_DISTRO-tf2* wget

Keep your system connected to the internet while building the package for the first time.

cd ~/catkin_ws/src
git clone https://github.com/L2S-lab/natnet_ros_cpp
cd ..
catkin build  #OR catkin_make
. devel/setup.bash

Setup the Motive for this package

Disable the firewall for the network on which the data is being published.
Open the Motive app.
In the motive app, open the streaming panel.
Disable the other streaming Engines like VRPN, Trackd etc.
Under the OptiTrack Streaming Engine, turn on the Broadcast Frame.
Select the correct IP address in the Local Interface.
Select the Up Axis as Z.

Here is an example of how your streaming settings should look.

Easy way

Using GUI tool Here, you can use simple tool and follow the instruction from the output area on the right bottom corner.

roslaunch natnet_ros_cpp gui_natnet_ros.launch

Difficult way

Using Non gui approach roslaunch natnet_ros_cpp natnet_ros.launch

Understanding the launch file

Launch file natnet_ros.launch contains the several configurable arguments. The details are mentioned in the launch file. Following are several important argument for the connection and the data transfer. Other connection arguments are for the advanced option.

serverIP : The IP address of the host PC. (The one selected in the Local Interface in Motive app)
clientIP : The IP address of the PC on which the file will be launched
serverType : Two possible options, multicast and unicast

Publishing the single marker

It is possible to track the single marker as a rigid body with constant orientation. Go to the config/initiate.yaml It is suggested to make a copy of the file and rename the new file. The file contains the details on what to modify.

The question might arise on how to check the position of the single marker. For that, you can log the frames of the incoming data in the terminal. To do so, enable the log_frames in the launch file.

After configuring the initiate.yaml, in the launch file, enable the pub_individual_marker. Change the name of the config file in the argument conf_file if needed and launch the file.

Replacing existing package

You can easily replace the current package with this package. In the natnet_ros.launch change the name of node to the node you currently using. For an example, If you are using the vrpn_client_node changes are following

<node pkg="natnet_ros_cpp" type="natnet_ros_cpp" name="vrpn_client_node" output="screen" >

Citation

If you use this software, please consider citing it from here

natnet_ros_cpp's People

Contributors

Stargazers

Watchers

Forkers

aarsht7 carolinepascal asaniart g-paris skymul hazemelrefaei arghyachatterjee ucsd-saslab a2s-institute hezepy prisma-avl thu-uav hparekh15 personalrobotics agresj

natnet_ros_cpp's Issues

Point cloud does not get published

Point cloud does not get published even after setting it to true

 <arg name="pub_rigid_body" default="true"/>

rostopic echo /natnet_ros/pointcloud

Does not return anything inspite of multiple markers being visible

[ INFO] [1680139028.704298891]: Parent Offset : 0.00,0.00,0.00
[ INFO] [1680139028.705371684]: Marker #0:
[ INFO] [1680139028.705410869]: Position: -0.05, 0.05, -0.00
[ INFO] [1680139028.705449994]: Required active label: 1
[ INFO] [1680139028.705853335]: Marker #1:
[ INFO] [1680139028.705868378]: Position: -0.05, -0.05, 0.00
[ INFO] [1680139028.705879130]: Required active label: 4
[ INFO] [1680139028.706236172]: Marker #2:
[ INFO] [1680139028.706251574]: Position: -0.00, -0.04, 0.01
[ INFO] [1680139028.706259770]: Required active label: 8
[ INFO] [1680139028.706572464]: Marker #3:
[ INFO] [1680139028.706583992]: Position: 0.05, -0.05, -0.00
[ INFO] [1680139028.706590555]: Required active label: 16
[ INFO] [1680139028.706927177]: Marker #4:
[ INFO] [1680139028.706942605]: Position: 0.05, 0.05, -0.00
[ INFO] [1680139028.706950237]: Required active label: 64
[ INFO] [1680139028.707282964]: Marker #5:
[ INFO] [1680139028.707298347]: Position: 0.00, 0.04, 0.00
[ INFO] [1680139028.707306386]: Required active label: 128

can't get rigid body points and pointcloud

Hey, thanks for this guide. I succeeded to install and get the publishers of my markers but I encountered a problem that my publishers of markers does not get the data.
when I do rostopic echo /..../marker1 for example it isn't print anything.

daniel@de-VM:~/auto_car_ws$ roslaunch natnet_ros_cpp natnet_ros.launch
... logging to /home/daniel/.ros/log/f3eeedc2-d89e-11ee-a9a5-cf729d479363/roslaunch-de-VM-3317.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://de-VM:45791/

SUMMARY
========

PARAMETERS
 * /conn_params/clientIP: **.**.**
 * /conn_params/globalFrame: world
 * /conn_params/multicastAddress: 239.255.42.99
 * /conn_params/serverCommandPort: 1510
 * /conn_params/serverDataPort: 1511
 * /conn_params/serverIP: 192.***.*.***
 * /conn_params/serverType: multicast
 * /natnet_ros/E: 0.04
 * /natnet_ros/E_x: 0.04
 * /natnet_ros/E_y: 0.04
 * /natnet_ros/E_z: 0.04
 * /natnet_ros/cf1/marker_config: 0
 * /natnet_ros/cf1/pose/orientation: []
 * /natnet_ros/cf1/pose/position: [-0.2455459535121...
 * /natnet_ros/individual_error: False
 * /natnet_ros/kalman: False
 * /natnet_ros/log_frames: False
 * /natnet_ros/log_internals: False
 * /natnet_ros/log_latencies: False
 * /natnet_ros/nearest_nbr: True
 * /natnet_ros/object_names: ['cf1']
 * /natnet_ros/pub_individual_marker: False
 * /natnet_ros/pub_pointcloud: True
 * /natnet_ros/pub_rigid_body: True
 * /natnet_ros/pub_rigid_body_marker: True
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    natnet_ros (natnet_ros_cpp/natnet_ros_cpp)

ROS_MASTER_URI=http://localhost:11311

process[natnet_ros-1]: started with pid [3331]
[ INFO] [1709388783.796545590]: NatNet Sample Client (NatNet ver. 4.0.0.0)
[ INFO] [1709388783.799581194]: Got server IP : 192.***.*.***
[ INFO] [1709388783.800009283]: Got client IP : **.**.**
[ INFO] [1709388783.800405660]: Got server Type : multicast
[ INFO] [1709388783.800834294]: Got server Type : 239.255.42.99
[ INFO] [1709388783.801186849]: Got server Command Port : 1510
[ INFO] [1709388783.801526712]: Got server Command Port : 1511
[ INFO] [1709388783.801837822]: Got global frame name : world
[ INFO] [1709388783.824488452]: Mocap Framerate : 120.00
[ INFO] [1709388783.826332141]: Analog Samples Per Mocap Frame : 0
[ INFO] [1709388783.826380041]: Client initialized and ready.
[ INFO] [1709388783.826387226]: [SampleClient] Requesting Data Descriptions...
[ INFO] [1709388783.828718893]: [SampleClient] Received 9 Data/Devices Descriptions:
[ INFO] [1709388783.828810906]: RigidBody found : test
[ INFO] [1709388783.833374551]: Client is connected to server and listening for data...


daniel@de-VM:~$ rostopic list
/natnet_ros/pointcloud
/natnet_ros/test/marker0/pose
/natnet_ros/test/marker1/pose
/natnet_ros/test/marker2/pose
/natnet_ros/test/marker3/pose
/natnet_ros/test/marker4/pose
/natnet_ros/test/marker5/pose
/natnet_ros/test/pose
/rosout
/rosout_agg
daniel@de-VM:~$ rostopic echo /natnet_ros/pointcloud

there is no print after the last command.
Please assist, thanks!

Unable to publish rigid body markers

Added the following to publish the individual markers.

  <!-- To publish the individual (unlabeled) markers along with the (labeled) markers of the rigid bodies -->
  <!-- keep pub_rigid_body to true to publish the markers of the rigidbody -->
  <arg name="pub_rigid_body_marker" default="true"/>

But, I get the following error:

[ INFO] [1680138254.776717634]: Software latency : 1.16 milliseconds
[ INFO] [1680138254.776734826]: System latency : 4.53 milliseconds
[ INFO] [1680138254.776765734]: Total client latency : 5.05 milliseconds (transit time +0.50 ms)
[ INFO] [1680138254.776775864]: FrameID : 359756
[ INFO] [1680138254.776796641]: Rigid Bodies [Count=1]
[ INFO] [1680138254.777233675]: Rigid Body [ID=11 Error=0.00]
[ INFO] [1680138254.777248227]: x y z qx qy qz qw
[ INFO] [1680138254.777259504]: 2.44 0.39 0.97 0.02 0.76 0.65 -0.04
[ INFO] [1680138254.777268671]: Markers [Count=0]
[ INFO] [1680138254.777291578]: x y z
[ INFO] [1680138254.777300436]: 2.49 0.39 1.01
[FATAL] [1680138254.777328574]: ASSERTION FAILED
file = /opt/ros/noetic/include/ros/publisher.h
line = 107
cond = false
message =
[FATAL] [1680138254.777358778]: Call to publish() on an invalid Publisher
[FATAL] [1680138254.777367665]:

Tracking of lost/regained individual markers

Hi,

First I wanted to say thank you for making this resource, our lab has found it very useful. I am attempting to track a few single markers which are unfortunately sometimes lost temporarily. This is rare and only brief but it halts tracking of a labeled point. I was wondering if you all had any ideas for work arounds or enhancements that would help me overcome this.

SPECIFICALLY: With the new gui, when turning on the "publish individual markers" and labeling them in the "single marker naming" tab, when the marker is lost, the data for it stops being received and is never regained despite the marker reappearing. I suspect this is because when it is regained, it does not have the previous label.

As of now, my only working solution is to instead use the pointcloud setting which allows for dynamically losing/gaining points, however, this means I have to write my own custom tracking, which works but I think is less robust.

ROS timestamp at the host or source?

Hi, I am using this node to stream rigid body poses to ROS.
Does the header.stamp in the pose msg correspond to the time at which the data was generated at the mocap optitrack system, or is it the time at which the host machine receives the streamed data?

parallels@ubuntu-linux-20-04-desktop:~/demo_ws$ catkin_make
Base path: /home/parallels/demo_ws
Source space: /home/parallels/demo_ws/src
Build space: /home/parallels/demo_ws/build
Devel space: /home/parallels/demo_ws/devel
Install space: /home/parallels/demo_ws/install
####
#### Running command: "make cmake_check_build_system" in "/home/parallels/demo_ws/build"
####
####
#### Running command: "make -j6 -l6" in "/home/parallels/demo_ws/build"
####
[ 50%] Built target Hello_pub
[ 66%] Built target hello_vscode_node
[ 50%] Built target Hello_sub
[ 50%] Built target hello
[ 75%] Linking CXX executable /home/parallels/demo_ws/devel/lib/natnet_ros_cpp/natnet_ros_cpp
/usr/bin/ld: /home/parallels/demo_ws/src/natnet_ros_cpp/deps/NatNetSDK/lib/libNatNet.so: error adding symbols: file in wrong format
collect2: error: ld returned 1 exit status
make[2]: *** [natnet_ros_cpp/CMakeFiles/natnet_ros_cpp.dir/build.make:136: /home/parallels/demo_ws/devel/lib/natnet_ros_cpp/natnet_ros_cpp] Error 1
make[1]: *** [CMakeFiles/Makefile2:1007: natnet_ros_cpp/CMakeFiles/natnet_ros_cpp.dir/all] Error 2
make: *** [Makefile:141: all] Error 2
Invoking "make -j6 -l6" failed
parallels@ubuntu-linux-20-04-desktop:~/demo_ws$

Build instructions incomplete

The build instructions are not mentioning the tf2 dependency. The NatNet SDK is mentioned, but could also be added to the build instructions to make it easier to use.

apt install -y ros-$ROS_DISTRO-tf2* wget
cd /path/to/workspace
wget https://s3.amazonaws.com/naturalpoint/software/NatNetSDKLinux/ubuntu/NatNet_SDK_4.0_ubuntu.tar
tar -xf NatNet_SDK_4.0_ubuntu.tar -C src/natnet_ros_cpp/deps/NatNetSDK/