Tracking ground vehicles indoors can be tricky business. Unlike a tiny quadrotor that is easily thwarted by a net, a runaway Husky can go through such barriers with ease. That’s why reliable indoor positioning is important for us at Clearpath Robotics.

Why OptiTrack?

Given that we don’t need extreme precision or capture rate, our indoor positioning hardware of choice is a basic OptiTrack camera system. Naturally, we wished to use this system along with ROS so the object tracking could be used to enable autonomous control of our robots. To this end, we have been maintaining and updating the vintage mocap_optitrack ROS package created by the AIS group at the University of Bonn.

Optitrack with Husky

The first thing we added is support for the latest NatNet protocol (2.2) as well as tracking of OptiTrack trackables instead of skeletons. Using trackables allows the node to work with Tracking Tools instead of Arena, which is more appropriate for tracking of rigid bodies like robots.

What are the challenges and limitations?

The next hurdle was using the system with multiple robots. Although simply publishing the position of every trackable in Tracking Tools was an option, we opted to allow for configurability of each tracked object. The new mocap_optitrack allows custom topic names for the published Pose and Pose2D as well as a custom frame-id for published transforms. You can also opt to not publish some of this data for a given trackable – a useful feature when trying to conserve bandwidth on wireless networks.

One unfortunate limitation of the NatNet protocol is what happens when a trackable leaves the coverage area of the cameras. NatNet data continues for the object continues to be streamed but the sent position is fixed at the origin. Given that a Husky is more than capable of causing damage if it leaves the test area the new mocap_optitrack node detects this condition and stops sending location data for trackables that have gone out of range. This effectively interrupts the control loop and stops the robot.

Improvements for success

With the improvements we made we are now able to track multiple robots from an OptiTrack camera system at rates of 100Hz or more. Best of all, if the robot leaves the test area, we are confident that it will stop on its own!

To see the code for yourself, click here.