Use a separate thread to pop k4abt_tracker result. Fix #109.

microsoft / Azure_Kinect_ROS_Driver

A ROS sensor driver for the Azure Kinect Developer Kit.

MIT License

304 stars 226 forks source link

Use a separate thread to pop k4abt_tracker result. Fix #109. #162

Closed zchenpds closed 3 years ago

zchenpds commented 4 years ago

Fixes #109

Description of the changes:

Create a separate thread dedicated to popping the result from k4abt_tracker_ and publishing body_tracking_data following the suggestion from this post:

the tracker will process faster if you keep queuing the frame without waiting for previous frame to be finished. Because then the pipeline can process two frames at different stages at the same time.

Required before submitting a Pull Request:

[x] I reviewed CONTRIBUTING.md
[x] I built my changes locally (this link is for ROS 1, I did not build my changes for ROS2)
[x] I tested my changes with a device
[x] I changed both the ROS1 and ROS2 branches

I tested changes on:

[ ] Windows
[x] Linux
[ ] ROS1
[x] ROS2

Tested in ROS2 with rviz subscribing to /body_tracking_data and observed significant decrease in latency after the changes.

amelhassan commented 3 years ago

This looks good to me, it was able to be built and run on Ubuntu 20.04, thanks for your contribution.

zchenpds commented 3 years ago

Sorry I was focused on the PR for Melodic branch. This PR for the foxy branch is not up-to-date with regard to the queue overflow bug that might be triggered when the k4a sampling frequency is higher than can be handled by the body tracking SDK. Could you please apply the following fix to the foxy branch? https://github.com/microsoft/Azure_Kinect_ROS_Driver/pull/161/commits/647387a906084411f5b662345b5e8fe2d60e930f

amelhassan commented 3 years ago

I was just going to ask about that, yes I can make that addition. Also I noticed you added a mutex to the printTimestampDebugMessage() function, should the same change be made to the foxy-devel branch?

zchenpds commented 3 years ago

I was just going to ask about that, yes I can make that addition.

Thank you for making the addition!

Also I noticed you added a mutex to the printTimestampDebugMessage() function, should the same change be made to the foxy-devel branch?

Yes I believe so. But I really regret writing function printTimestampDebugMessage() in the first place. I was not aware that the command line utility rostopic echo already had an --offset option which displays "time in messages as offset from current time (e.g. to calculate lag/latency)". So this function is probably redundant and should be deprecated. I doubt any one except me really uses it so please feel free to delete it.

amelhassan commented 3 years ago

Ok, I will remove the function printTimestampDebugMessage() from both PRs since it is redundant.

zchenpds commented 3 years ago

Ok, I will remove the function printTimestampDebugMessage() from both PRs since it is redundant.

Cool. Note that the extra time it takes for the messages to reach rostopic echo --offset would affect the delay measurement, but presumably not by a lot.