Motive Tracker 2 Compatibility issues

[xepost]

Hi

Just wanted to ask if anyone is working on the ros driver for Motive Tracker 2. I have called the Optitrack support and they mentioned that they did some updates that might break compatibility.

Compatibility Notes Important: Static linking is no longer supported. Projects that utilized NatNetStaticLib.lib will need to be updated to link dynamically instead. SDK security update: Starting from 3.0, the NatNet library will strictly work with Motive only. Other server applications, including Arena and Tracking Tools, are no longer compatible. Deprecated Connection Type Enum for clients. Clients now just need to invoke NatNetClient::Connect. Removed fLatency metrics from the frame of mocap data. Removed marker positions and corresponding property values from sRigidBodyData. They can now be obtained from sRigidBodyDescription.

Please let me know if anyone else is working on this. Bests,

[yamokosk]

We are currently trying to get this driver working with Motive 2/NatNet 3.0. The furthest we have gotten is on the new-and-old-support-updated branch. As seen from the console output below, it gets the version correctly and even correctly unpacks data from a single rigid body in our capture volume. However it crashes shortly after before exiting MoCapDataFormat::parse.

$ roslaunch mocap_optitrack mocap.launch                                                
... logging to /home/yamokosk/.ros/log/23d2904c-94d6-11e8-b797-000c29d08d71/roslaunch-sleepy-40707.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://sleepy:39097/

SUMMARY
========

PARAMETERS
 * /mocap_node/optitrack_config/multicast_address: 224.0.0.1
 * /mocap_node/rigid_bodies/1/child_frame_id: Robot_1/base_link
 * /mocap_node/rigid_bodies/1/parent_frame_id: world
 * /mocap_node/rigid_bodies/1/pose2d: Robot_1/ground_pose
 * /mocap_node/rigid_bodies/1/pose: Robot_1/pose
 * /mocap_node/rigid_bodies/1/use_new_coordinates: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.13

NODES
  /
    mocap_node (mocap_optitrack/mocap_node)

auto-starting new master
process[master]: started with pid [40743]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 23d2904c-94d6-11e8-b797-000c29d08d71
process[rosout-1]: started with pid [40756]
started core service [/rosout]
process[mocap_node-2]: started with pid [40759]
[ INFO] [1533050808.436903323]: Creating socket...
[ INFO] [1533050808.436983537]: Setting socket options...
[ INFO] [1533050808.437017193]: Local address: 0.0.0.0:9000
[ INFO] [1533050808.437038621]: Binding socket to local address...
[ INFO] [1533050808.437085145]: Joining multicast group 224.0.0.1...
[ INFO] [1533050808.437126437]: Enabling non-blocking I/O
[ INFO] [1533050808.437187424]: Start processMocapData
[DEBUG] [1533050808.442302368]:  260 bytes received from 10.10.15.149:9000
[DEBUG] [1533050808.450439904]:  234 bytes received from 10.10.15.149:9000
[DEBUG] [1533050808.451157655]:  283 bytes received from 10.10.15.149:1510
[DEBUG] [1533050808.451198067]: Header : 1, 1
[DEBUG] [1533050808.451212344]: nData : 279
[ INFO] [1533050808.451233803]: NATNet Version : 60.22.46.0
[ INFO] [1533050808.451245584]: Server Version : 3.0.0.0
[DEBUG] [1533050808.451274440]:  283 bytes received from 10.10.15.149:1510
[DEBUG] [1533050808.451309243]: Header : 1, 1
[DEBUG] [1533050808.451323401]: nData : 279
[DEBUG] [1533050808.451346861]:  283 bytes received from 10.10.15.149:1510
[DEBUG] [1533050808.451367293]: Header : 1, 1
[DEBUG] [1533050808.451374029]: nData : 279
[DEBUG] [1533050808.451650656]:  283 bytes received from 10.10.15.149:1510
[DEBUG] [1533050808.451660911]: Header : 1, 1
[DEBUG] [1533050808.451666476]: nData : 279
[DEBUG] [1533050808.458958873]:  260 bytes received from 10.10.15.149:9000
[DEBUG] [1533050808.459031790]: Frame number: 251740
[DEBUG] [1533050808.459060691]: Number of marker sets: 0
[DEBUG] [1533050808.459081920]: Number of markers not in sets: 3
[DEBUG] [1533050808.459102440]: Number of rigid bodies: 1
[DEBUG] [1533050808.459122622]: Rigid body ID: 1
[DEBUG] [1533050808.459143157]: Number of rigid body markers: 959312896
[DEBUG] [1533050808.459156702]: pos: [-0.04,0.02,0.15], ori: [-0.00,-0.07,0.00,-1.00]
terminate called after throwing an instance of 'std::bad_alloc'
  what():  std::bad_alloc
================================================================================REQUIRED process [mocap_node-2] has died!
process has died [pid 40759, exit code -6, cmd /home/yamokosk/ws/mocap/devel/lib/mocap_optitrack/mocap_node __name:=mocap_node __log:=/home/yamokosk/.ros/log/23d2904c-94d6-11e8-b797-000c29d08d71/mocap_node-2.log].
log file: /home/yamokosk/.ros/log/23d2904c-94d6-11e8-b797-000c29d08d71/mocap_node-2*.log
Initiating shutdown!
================================================================================
[mocap_node-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

The problem is that it is incorrectly parsing the number of markers in the rigid body. It should be 3 but somehow thinks it is 959312896. So it then crashes on line 226 with a bad alloc.

My question to the primary maintainers.. what do you use for your reference source as it pertains to the NatNet packet structure? Does Optitrack have a document somewhere with the packet description?

[yamokosk]

I've answered my own question with some Googling. Their reference raw packet deserialization is in some example code that comes with their SDK. We'll go through and find the differences between the ROS driver and reference deserialization.

[yamokosk]

Ok, looks like the problem is in parsing the marker data for rigid bodies. From the sample code in the SDK:

        // Loop through rigidbodies
        int nRigidBodies = 0;
        memcpy(&nRigidBodies, ptr, 4); ptr += 4;
        printf("Rigid Body Count : %d\n", nRigidBodies);
        for (int j=0; j < nRigidBodies; j++)
        {
            // Rigid body position and orientation 
            int ID = 0; memcpy(&ID, ptr, 4); ptr += 4;
            float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4;
            float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4;
            float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4;
            float qx = 0; memcpy(&qx, ptr, 4); ptr += 4;
            float qy = 0; memcpy(&qy, ptr, 4); ptr += 4;
            float qz = 0; memcpy(&qz, ptr, 4); ptr += 4;
            float qw = 0; memcpy(&qw, ptr, 4); ptr += 4;
            printf("ID : %d\n", ID);
            printf("pos: [%3.2f,%3.2f,%3.2f]\n", x,y,z);
            printf("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx,qy,qz,qw);

            // NatNet version 2.0 and later
            if(major >= 2)
            {
                // Mean marker error
                float fError = 0.0f; memcpy(&fError, ptr, 4); ptr += 4;
                printf("Mean marker error: %3.2f\n", fError);
            }

            // NatNet version 2.6 and later
            if( ((major == 2)&&(minor >= 6)) || (major > 2) || (major == 0) ) 
            {
                // params
                short params = 0; memcpy(&params, ptr, 2); ptr += 2;
                bool bTrackingValid = params & 0x01; // 0x01 : rigid body was successfully tracked in this frame
            }

        } // Go to next rigid body

And the ROS driver:

  read_and_seek(model.numRigidBodies);
  ROS_DEBUG("Number of rigid bodies: %d", model.numRigidBodies);

  model.rigidBodies = new RigidBody[model.numRigidBodies];
  for (int m = 0; m < model.numRigidBodies; m++)
  {
    // read id, position and orientation of each rigid body
    read_and_seek(model.rigidBodies[m].ID);
    read_and_seek(model.rigidBodies[m].pose);

    // get number of markers per rigid body
    read_and_seek(model.rigidBodies[m].NumberOfMarkers);

    ROS_DEBUG("Rigid body ID: %d", model.rigidBodies[m].ID);
    ROS_DEBUG("Number of rigid body markers: %d", model.rigidBodies[m].NumberOfMarkers);
    ROS_DEBUG("pos: [%3.2f,%3.2f,%3.2f], ori: [%3.2f,%3.2f,%3.2f,%3.2f]",
             model.rigidBodies[m].pose.position.x,
             model.rigidBodies[m].pose.position.y,
             model.rigidBodies[m].pose.position.z,
             model.rigidBodies[m].pose.orientation.x,
             model.rigidBodies[m].pose.orientation.y,
             model.rigidBodies[m].pose.orientation.z,
             model.rigidBodies[m].pose.orientation.w);

    if (model.rigidBodies[m].NumberOfMarkers > 0)
    {
      model.rigidBodies[m].marker = new Marker [model.rigidBodies[m].NumberOfMarkers];

      size_t byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(Marker);
      memcpy(model.rigidBodies[m].marker, packet, byte_count);
      seek(byte_count);

      // skip marker IDs
      byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(int);
      seek(byte_count);

      // skip marker sizes
      byte_count = model.rigidBodies[m].NumberOfMarkers * sizeof(float);
      seek(byte_count);
    }

    // skip mean marker error
    seek(sizeof(float));

    // 2.6 or later.
    if (NatNetVersion > Version("2.6"))
    {
      seek(sizeof(short));
    }

  }

Does anyone know the history of this.. if marker data for rigid bodies used to be in the mocap frame, I'd expect to see some if-statements in the sample code but there are none. In fact it looks like rigidbody marker data might be treated as "labeled markers" (https://forums.naturalpoint.com/viewtopic.php?f=59&t=14648), which in the PacketClient example aren't deserialized until after skeletons.

[xepost]

Hi @yamokosk ,

Did you had any progress on this ? I was away for a while so not able to start working on the code yet. Thanks a lot for your previous comments.

[tonybaltovski]

Closing due to inactivity. Please feel free to re-open if issue persists.

[Berayksl]

When I launch the mocap node it gets stuck at [mocap_node-1] [INFO] [1717540789.899215214] [mocap_node]: Enabling non-blocking I/O What might be the issue?