The Ros node for computing rigid-body linear and angular velocity based on data streaming from the Motive software

Node description：

The OptiTrack node is responsible for processing the position and attitude feedback for quadrotor experiments in Flight System and Control Lab (FSC). The OptiTrack node shall be download and installed on the ground station desktop/computer. The OptiTrack node supports an arbitrary number of rigid bodies defined in the argument of the launch file. The names of the rigid bodies should be the same as the ones used in the Moitve software.

User guide:

• Intall and make sure you can run vrpn_client_ros pacakge first: http://wiki.ros.org/vrpn_client_ros . This pacakge is used to receive the data stream from the Motive software.

1. you may either download and complie the source file from https://github.com/ros-drivers/vrpn_client_ros, or sudo apt-get install ros-kinetic-vrpn-client-ros -y. Change the kinetic to the ubuntu version you are using.
2. roslaunch vrpn_client_ros sample.launch
3. use rostopic list to check whether the rigid-body information from the Motive software is published and updated.

• You may install the OptiTrack node using the following commands:

1. $ cd yourws/src/
2. $ git clone https://github.com/LonghaoQian/optitrack_broadcast
3. $ cd ../..
4. $ catkin build

• For gazebo simulations, please use emulator_for_gazebo.launch to generate simulated position and velocity data stream.

• For flight experiments, please use optitrack_broadcast_p300.launch to obtain the position and velocity measurements.

• Adjust the arguments in the pkg node based on the rigid bodies defined in Motive software to get corresponding feedbacks.

For Gazebo simulations:

Adjust the argument "objects" according to the names of the rigid bodies defined in Motive software in the following XML node: <arg name="objects" default="UAV0 UAV1 UAV2 Payload"/> The default name list is "UAV0 , UAV1, UAV2, Payload".

For flight experiments:

Make sure that the IP address of the desktop running the Motive software is correct. The IP address is specified in <arg name="server" default="192.168.2.230"/>

The IP address of the Motive desktop is the default: 192.168.2.230.

Adjust the value of args according to the names of the rigid bodies defined in Motive software in the following XML node: <node pkg = "optitrack_broadcast" name = "broadcast_1" type = "broadcast_position_velocity" output="screen" args="UAV cf1 cf2 cf3 cf4 cf5"/> The default name list is "UAV, cf1, cf2, cf3, cf4, cf5".

The data structure:

The OptiTrack node utilizes a customized Ros data structure Mocap.msg to reduce the wireless communication burden for experiments. Mocap.msg is defined as follows:

std_msgs/Header header

float32[3] position ## [m]

float32[3] velocity ## [m/s]

float32[3] angular_velocity ## [rad/s]

float32[4] quaternion ##

Copy Mocap.msg to the msg folder of your project and add this customized message type to your CMakelist.txt in oder to use the data type:

add_message_files(

DIRECTORY msg

FILES

Mocap.msg

)

The velocity estimation algorithm

The Motive software only streams the position and the attitude of the specified rigid bodies to the ground station computer, so the OptiTrack node calculates the velocity and angular velocity of the rigid bodies based on the following algorithms. We denote as the raw data streamed form the Motive software. The velocity estimation at the Nth time step is denoted as , obtained as follows:

where dt is the size of the time step between the Nth and the N-1th data feedback. The attitude of the rigid body from the Motive software is expressed in a quaternion: . Two auxiliary matrices are defined as follows:

Then we can calculate the rotation matrix between the body-fixed frame of the rigid body and the inertial frame at the Nth time step:

The angular velocity estimation is calculated by using the Lie algebra of the rotation matrix. The map is denoted as the inverse of the Lie map.

After obtaining the raw velocity estimation, we use the following sliding window filter to provide a filtered result:

The size of the window is set to 4 based on experimental testings.