Optitrack on Raspberry Pi

From Rsewiki
(Difference between revisions)
Jump to: navigation, search
(Created page with "Using optitrack and thin NatNet client ==Install== Get client from https://github.com/b4be1/packet-client-linux e.g. from the raspberry: git clone https://github.com/b4be1...")
 
(→‎Install ASTA drone_ctrl version)
 
(46 intermediate revisions by one user not shown)
Line 1: Line 1:
−
Using optitrack and thin NatNet client
+
Using Optitrack and thin NatNet client using C++
  
−
==Install==
+
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
 
Get client from https://github.com/b4be1/packet-client-linux
−
e.g. from the raspberry:
+
e.g. directly to the raspberry:
  
 
  git clone https://github.com/b4be1/packet-client-linux.git
 
  git clone https://github.com/b4be1/packet-client-linux.git
Line 13: Line 23:
 
  make
 
  make
  
−
== setup ==
+
Filelist:
 +
NatNetClient.py    - python client
 +
requirements.txt  - almost empty
 +
CMakeLists.txt    - cmake instructions
 +
PacketClient.cpp  - source code
 +
README.md          - how to use/compile
  
−
On the raspberry pi set the wireless network to access the dedicated optitrack network
+
The CMakeLists.txt is rather short (only the pthread library is needed):
  
−
Modify the /etc/wpa_supplicant/wpa_supplicant.conf by adding the network groups, maybe the 5Ghz SSID should have highest priority.
+
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=DIR=/var/run/wpa_supplicant GROUP=netdev
Line 23: Line 178:
 
  update_config=1
 
  update_config=1
 
  country=DK
 
  country=DK
−
 
+
 
  network={
 
  network={
 
         ssid="asta_optitrack"
 
         ssid="asta_optitrack"
Line 29: Line 184:
 
         priority=1
 
         priority=1
 
  }
 
  }
−
 
+
 
  network={
 
  network={
 
         ssid="asta_optitrack_5G"
 
         ssid="asta_optitrack_5G"
Line 36: Line 191:
 
  }
 
  }
  
−
Restart the network by reboot or just restart the network
+
Restart the network by reboot or just restart the network:
  
 
  sudo /etc/init.d/networking restart
 
  sudo /etc/init.d/networking restart
  
−
== modify source ==
+
== Optitrack setup ==
  
−
The NatNet version should be changed - hard-coded in file.
+
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
 
   cd ~/packet-client-linux
Line 52: Line 223:
 
  int NatNetVersion[4] = {3, 1, 0, 0};
 
  int NatNetVersion[4] = {3, 1, 0, 0};
 
  int ServerVersion[4] = {0, 0, 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 the client ===
  
 
Compile
 
Compile
Line 60: Line 233:
 
  make
 
  make
  
−
==== Run the client ====
+
=== Run the client ===
  
 
  cd ~/packet-client-linux/build
 
  cd ~/packet-client-linux/build
 
  ./PacketClient DESKTOP-OVLVRUD.local 192.168.1.136
 
  ./PacketClient DESKTOP-OVLVRUD.local 192.168.1.136
  
−
where DESKTOP-OVLVRUD is the hostname of the optitrack server and 192.168.1.136 is the IP of the raspberry pi in this case.
+
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).

Latest revision as of 14:09, 7 July 2021

Using Optitrack and thin NatNet client using C++

Back to ASTA motion capture

Contents

[edit] 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.

[edit] 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)

[edit] 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

[edit] 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, 

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

[edit] 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

[edit] 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.

[edit] 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

[edit] 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.

[edit] 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.

[edit] Compile the client

Compile 

cd ~/packet-client-linux/build
make

[edit] 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.

[edit] 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).

Retrieved from "http://rsewiki.elektro.dtu.dk/index.php?title=Optitrack_on_Raspberry_Pi&oldid=5519"
Personal tools
Namespaces

Variants
Actions
Navigation
Toolbox