-
Notifications
You must be signed in to change notification settings - Fork 21
Position Tracking
Motion capture system set up in Humberto Gonzalez's dynamics lab -- consists of 16 infrared cameras from Optitrack, which can track fluorescent marker positions within the lab space. Interfaces with 'Motive' software, where rigid body tracking and recording configurations can be modified.
In this Pi Car project, Motive is used to obtain position measurements of the car at any given time in the lab. Frames of data are then streamed to a distributive server program (running on the same PC), which broadcasts the data to any listening Raspberry Pis. This is a central (MoCap PC) -> peripheral (Pi) setup performed over a WiFi connection.
PC-side:
- Windows 7 Professional OS
- Motive 1.6
- NatNet SDK 2.8.0
- Microsoft Visual Studio 2010 (C++10)
Raspberry Pi:
- Raspbian Jessie OS
Both:
- Python 2.7.11 (or 2.7.12)
- Twisted-Python 16.6
The cameras are linked to a desktop PC in the lab, running on Windows 7 Professional. To run the actual software application that interfaces with the cameras, a valid Optitrack USB hardware key must be plugged in prior to opening.
Once the cameras are online and a new project is opened, several configurations must be performed to set up a data streaming session. It is assumed that you have the following tools available: a marker wand, a calibration square, and individual infrared marker bearings.
Please refer to the Optitrack documentation wiki for a guide to working in Motive. The following key actions are most relevant:
- Camera calibration: for initial setup
- Rigid body tracking: for appending infrared markers to the Pi Car and tracking them as one, cohesive 'rigid body'
- Data streaming: for enabling position frame streaming on the Motive server
Note that you do not have to start 'recording' at any point in running Motive. Instead, by enabling Optitrack Engine data streaming (under View > Data Streaming), UDP packets containing MoCap frames are automatically sent over a multicast connection.
When running a data stream, ensure that:
(1) The multicast IP address is 239.255.42.99. This IP is what is used in the extraction program further down the pipeline.
(2) You have defined at least one rigid body in Motive, as that is what the extraction program selectively filters for.
- With future Pi Cars featuring distinctive LED matrices, the next step is to define a standard method of identifying, naming, and configuring each car within the software.
The C++ extraction client makes use of the company-provided NatNet API to read in MoCap frames from Motive, whose cameras run with Windows drivers. You can find the application within the Visual Studio project under "PiCar\ Program/MoCap/Motive/Samples/MotiveClient2/MotiveClient2.cpp". Note the includes and DLLs in the Motive base directory, as they are needed to compile and run the program.
To begin, open 'SampleClient.vcxproj' with Visual Studio; this will load the C++ file and external dependencies into the project view.
Then ensure that the NatNet library is linked to your C++ project, which can be done statically or dynamically. In our case, we chose to use 'NatNetLibStatic.lib' (x32 version), located in "MoCap_PC/Motive/lib".
- Within Visual Studio's Solution Explorer pane, right click on the 'MotiveClient2' project directory and select 'Properties'.
- Under Configuration Properties > Linker > Input, add 'NatNetLibStatic.lib' as a value within the 'Additional Dependencies' field.
Should you wish to use the x64 version, please refer to the x64 subfolder within the 'lib' directory. Other extensive options for setting up this application can be found in the 'NatNetUsersGuide.pdf', located in 'MoCap_PC/Motive'.
To compile and build the program, use "Build > Build Solution" or right click on the MotiveClient2 project to do the same thing. With default settings, any successfully built executables will be placed in the /bin folder of the same file directory. Should this not be the case, or should you wish to change this, again look into the project's Configuration Properties, within the 'Output Directory' field of the General settings.
Concretely, the extraction client performs the following functions:
- Declares, instantiates a NatNetClient object that handles the connection to the Motive streaming server. Callback functions for interpreting packet data are appended to this object.
- Sets up an 'export server' using Microsoft Winsock. Application waits until the server distribution program (discussed later) connects via localhost. **
- Receives Motive's FrameOfMocapData structs contained within UDP packets, and extracts relevant data (xyz coordinates, quaternions) for rigid bodies only. This data is then stored in a smaller, custom payload struct. **
- When payload struct is filled (37 bytes) with position and timestamp, Winsock 'send' function is called to immediately pass payload into server distribution program. The message is sent as a netstring.**
- Actively listens for user-supplied keyboard interrupts (e.g. quit).
This program was based upon the SampleClient program contained within the NatNet API download (note that the only available online version is 2.10, whereas we used 2.8 in our implementation). The bullet points ending in '**' denote significant modifications made to the original sample. Consult the API PDF within the Motive directory for details on custom objects and callbacks.
Handling multiple Pi Cars
Over the course of fall 2016, we had access to at most one Pi Car prototype, so the C++ extraction app was not designed for multiple cars. To modify it accordingly, there are several steps you can take:
-
When reading in the FrameOfMocapData struct, assign an ID tag to each unique rigid body. Provided that each PiCar has its own LED matrix identifier, you can establish a naming convention from Motive when defining each rigid body in the 3D lab space.
-
In the 'payload_t' struct definition, add an additional field for the ID tag.
-
In the 'message_t' wrapper struct definition, allow for multiple rigid body payloads to be added.
-
In the 'DataHandler' callback function, append separate rigid body payloads to a message_t struct. (Preferably, we want to avoid calling 'send' multiple times per frame -- generating excessive TCP traffic)
Named as 'twisted_server7.py', within the MoCap_PC folder.
The Twisted Server program receives all packet frames from the Motive side and passes them off to a client program running on the Raspberry Pi. Its main purpose is to serve as an intermediary for the sake of both compatibility and efficiency. The Twisted framework is a event-based networking API that can asynchronously process multiple connections and their respective data flows.
Its functionality in this project is as follows:
- Connect to the 'export server' via a local area TCP connection.
- Upon reception of the message_t payload, a custom-defined InputProtocol reads in the netstring, and unpacks the individual bytes as a quality check.
- Listen for any TCP connections via WiFi with its OutputProtocol classes.
- With at least one server-client connection, and with a message_t payload unpacked, sends payload directly to the Raspberry Pi client(s) in tuple form.
- (Optional) Writes positions received and time interval benchmarks to output files.
Handling multiple Pi Cars
- Like above, the Twisted Server will have to adjust the format of its messages received and sent for unique identifiers attributed to each Pi Car.
- In the 'connectionMade' callback function for the OutputProtocol, some initial messages should be exchanged between the program and the Raspberry Pi to help establish an ID for that particular car. How this is handled is up to suggestion. e.g. predetermining a name/ID for a car based on its LED matrix configuration, in both the Motive software and the Raspberry Pi client.
- In addition, changing the OutputProtocolFactory's client list into a dictionary, in order to accommodate mappings from client -> ID and other connection-specific information.
Data Formatting
- Currently, the payload passed over to the Raspberry Pi client is done so with the LineReceiver protocol class. This sends each frame as a string, which may introduce some overhead when converting for actual application usage. Using netstrings or some other protocol for direct byte interpretation may be faster.
'twisted_client.py' within the PiCar Program directory. Note that this program is not to be used in the full implementation, because it has been incorporated into the aggregate scheduler program (discussed in a separate page). twisted_client.py should be used when solely testing the MoCap PC -> Raspberry Pi connection and data handling.
Functionality:
- Connect to a listening Twisted Server program on the MoCap PC side.
- Receive position frames, and convert them from strings into tuples
- Replaces 'buffered' frame with most recently arrived one
- Using Twisted's LoopingCall, samples from the buffer every 20ms (can be adjusted)
- Output results to file, with time intervals between frames
-
On the MoCap PC, plug in the USB hardware key and start up Motive
-
Once the cameras are online, identify the Pi Car(s) within the software as a rigid body
-
In the data streaming pane in Motive, check off 'Enable data streaming', ensuring that the multicast IP address matches up with the C++ Extraction client
-
Open up two Command Prompts. Use one to run the MotiveClient2.exe program, and the other to run twisted_server7.py
-
Once data is passing through the server distribution program, open and run twisted_client.py on the Raspberry Pi
- C++ extraction application built off sample from Optitrack's NatNet SDK
- Code, wiki page written by Jeffrey Gu