Optitrack on Raspberry Pi
Using Optitrack and thin NatNet client using C++
Back to ASTA motion capture
Use
The PacketClient described here is a "Direct Depacketization" code for NatNet and is not using the NatNat SDK, and thus have to be updated if the data structure used in NatNet is changed.
The Git repository is maintained, so you should keep the modifications to this file small, so that future updates are easily integrated.
Install original sample version
Optional install, you may want to use the version adapted to Asta, see next section.
Get client from https://github.com/b4be1/packet-client-linux e.g. directly to the raspberry:
git clone https://github.com/b4be1/packet-client-linux.git cd packet-client-linux mkdir build cd build cmake .. make
Filelist:
NatNetClient.py - python client requirements.txt - almost empty CMakeLists.txt - cmake instructions PacketClient.cpp - source code README.md - how to use/compile
The CMakeLists.txt is rather short (only the pthread library is needed):
cmake_minimum_required(VERSION 2.8)
project(PacketClient)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall")
add_executable(PacketClient PacketClient.cpp)
target_link_libraries(PacketClient pthread)
Install ASTA version
The Asta version is adapted to the Asta Optitrack version and includes data extraction for rigid bodies only.
This is not ROS based. If you use ROS, then you should use the ROS package instead.
Get the source code
Install prerequisites.
sudo apt install subversion cmake
Get from repository
cd svn co svn://repos.gbar.dtu.dk/jcan/asta optiAsta cd optiAsta mkdir build cd build cmake .. make
Install ASTA drone_ctrl version
This version includes an interface to drone_ctrl flight controller, Raspberry camera, Optitrack and GNSS.
Some additional packages are needed,
- see http://rsewiki.elektro.dtu.dk/index.php/Linux_tools for general tools
- see http://rsewiki.elektro.dtu.dk/index.php/Raspberry_Camera_API#Raspicam for camera API support - and optionally streaming software.
cd svn co svn://repos.gbar.dtu.dk/jcan/mobotware/drone_ctrl
The raspberry pi interface to Teensy, Optitrack and GPS is then in the directory
cd ~/drone_ctrl/trunk/drone_mission mkdir build cd build cmake .. make ./mission
It will not run a mission (out of the box), but start the interface and allow configuration using the Python GUI in
cd ~/drone_ctrl/trunk/drone_gui python3 drone_gui.py
Modify local IP
in the main.cpp file the IP of the raspberry on the wifi connection to the optitrack access point is needed.
Find the IP
ifconfig
the wlan0 should be connected and have an IP in the area 192.168.1.XX.
If not, look at the settings described below and possibly reboot the raspberry.
Modify the main.cpp code
There is a findIP(), but it works only if the Raspberry has just one IP, and there is therefore a line with a hard coded IP, as shown here:
/** (part of main.cpp)
* setup parameters and start */
bool startNatNetConnection(const char * argv0)
{ // find IP/name of server and this machine
const int MIL = 100;
char ipStr[MIL];
const char * optitrackServerInAsta = "DESKTOP-OVLVRUD.local";
// IP of this machine
findIP(ipStr, MIL);
strncpy(ipStr, "192.168.1.33", MIL);
// generate an argc, argv set
const char * argv1[4] = {argv0, optitrackServerInAsta, ipStr, "\0"};
// run NatNet connection for optitrackServer
// also starts threads for package reception
// takes 3 string parameters (app name, server IP/name, client IP)
bool started = setup(3, (char **)argv1);
return started;
}
Modify as needed and recompile the code.
It should produce an executable called optiAsta, run by:
~/optiAsta/build/optiAsta
Class description
This version includes 2 classes
The first holds position and orientation (in quartonions) in the optitrack coordinate system
class URigid
{
public:
int ID;
float pos[3]; // position in m in the Opritrack coordinates
float rotq[4]; // orientation - quartonions - I think
float posErr;
bool valid; // true, if visible
void printRigid(); // print on console
};
and a class to hold all rigid bodies
class UOptitrack
{
protected:
// positions
static const int MAX_MARKERS = 30;
URigid * markers[MAX_MARKERS] = {nullptr};
int markersCnt = 0;
double timestamp;
public:
void print();
URigid * findMarker(int id);
public:
// major NatNet unpack, modified version of the one in PacketClient.cpp
void unpack(char * pData);
};
One instance of UOptitrack is created in the main.cpp file. Use the 'findMarker(int ID)' to get a pointer to a URigid with the position of that rigid body.
The function 'unpack(...)' is called in a thread every time there is a new data frame from the optitrack system.
Raspberry setup
On the Raspberry Pi set the wireless network to access the dedicated Optitrack network
Modify the /etc/wpa_supplicant/wpa_supplicant.conf by adding the network groups. The 5Ghz SSID has in this case higher priority (higher number).
ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
ctrl_interface_group=0
update_config=1
country=DK
network={
ssid="asta_optitrack"
psk="asta2020"
priority=1
}
network={
ssid="asta_optitrack_5G"
psk="asta2020"
priority=2
}
Restart the network by reboot or just restart the network:
sudo /etc/init.d/networking restart
Optitrack setup
In
- EDIT mode (bottom of window left)
In 'View' and 'Data streaming pane'
- select 'local interface' to the IP address of the server (192.168.1.129) and
- deselect other streaming services (VRPN and Trackd) if not used by others
In 'View' and 'Assets Pane'
- check the relevant rigid bodies
Then
- go live (bottom of window left)
and assuming there is at least one rigid body defined and visible, streaming should commence.
Modify the original source
The NatNet version should be changed - it is hard-coded in source file.
cd ~/packet-client-linux nano PacketClient.cpp
A bit down change the NatNetVersion to 3.1 (to fit Motive version 2.2)
// Versioning
int NatNetVersion[4] = {3, 1, 0, 0};
int ServerVersion[4] = {0, 0, 0, 0};
The multicast address is "239.255.42.99" and should not be changed.
Compile the client
Compile
cd ~/packet-client-linux/build make
Run the client
cd ~/packet-client-linux/build ./PacketClient DESKTOP-OVLVRUD.local 192.168.1.136
where DESKTOP-OVLVRUD is the hostname of the Optitrack server (in ASTA) and 192.168.1.136 is the IP of the raspberry pi in this case.
Data in client
The client should then print package data on the console. The frame data packet looks like this:
Begin Packet ------- Message ID : 7 Byte count : 220 Frame # : 285290 Marker Set Count : 0 Rigid Body Count : 1 ID : 20 pos: [0.73,0.57,-0.15] ori: [-0.01,-0.00,1.00,0.02] Mean marker error: 0.00 Tracking Valid: True Skeleton Count : 0 Labeled Marker Count : 4 ID : [MarkerID: 1] [ModelID: 20] pos : [0.72,0.50,-0.15] size: [0.01] err: [0.00] ID : [MarkerID: 2] [ModelID: 20] pos : [0.78,0.53,-0.14] size: [0.01] err: [0.00] ID : [MarkerID: 3] [ModelID: 20] pos : [0.74,0.62,-0.15] size: [0.01] err: [0.00] ID : [MarkerID: 4] [ModelID: 20] pos : [0.66,0.64,-0.15] size: [0.01] err: [0.00] Timestamp : 2377.417 Mid-exposure timestamp : 12335057189261 Camera data received timestamp : 12335057223494 Transmit timestamp : 12335057235053 End Packet
The data is thus available in the client and should be integrated into the intended application (by further source code editing).
