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

[zchenpds]

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.

Before submitting a Pull Request:

• I reviewed CONTRIBUTING.md
• I built my changes locally
• I tested my changes with a device
• I changed both the ROS1 and ROS2 branches

I tested changes on:

• Windows
• Linux
• ROS1
• ROS2

I measured the delay by adding ROS_INFO_STREAM("Body tracking delay: " << (ros::Time::now() - capture_time).toSec() * 1000.0 << " ms."); below Line 990

Azure_Kinect_ROS_Driver/src/k4a_ros_device.cpp

Line 990 in 87d10fa

k4abt::frame body_frame = k4abt_tracker_.pop_result();

and run roslaunch azure_kinect_ros_driver driver.launch fps:=30 depth_mode:=WFOV_2X2BINNED body_tracking_enabled:=true. The delay increases quickly from 80 ms to around 280 ms on my laptop if/when I run some light-weight tasks such as opening a new webpage in Chrome browser or scrolling up/down in vscode editor. (Laptop specs: Intel® Core™ i7-9750H CPU @ 2.60GHz × 12 , GeForce RTX 2060, 32 GB RAM)

By moving

Azure_Kinect_ROS_Driver/src/k4a_ros_device.cpp

Lines 990 to 1039 in 87d10fa

	k4abt::frame body_frame = k4abt_tracker_.pop_result();
	if (body_frame == nullptr)
	{
	ROS_ERROR_STREAM("Pop body frame result failed!");
	ros::shutdown();
	return;
	}
	else
	{
	if (body_marker_publisher_.getNumSubscribers() > 0)
	{
	// Joint marker array
	MarkerArrayPtr markerArrayPtr(new MarkerArray);
	auto num_bodies = body_frame.get_num_bodies();
	for (size_t i = 0; i < num_bodies; ++i)
	{
	k4abt_body_t body = body_frame.get_body(i);
	for (int j = 0; j < (int) K4ABT_JOINT_COUNT; ++j)
	{
	MarkerPtr markerPtr(new Marker);
	getBodyMarker(body, markerPtr, j, capture_time);
	markerArrayPtr->markers.push_back(*markerPtr);
	}
	}
	body_marker_publisher_.publish(markerArrayPtr);
	}
	if (body_index_map_publisher_.getNumSubscribers() > 0)
	{
	// Body index map
	ImagePtr body_index_map_frame(new Image);
	result = getBodyIndexMap(body_frame, body_index_map_frame);
	if (result != K4A_RESULT_SUCCEEDED)
	{
	ROS_ERROR_STREAM("Failed to get body index map");
	ros::shutdown();
	return;
	}
	else if (result == K4A_RESULT_SUCCEEDED)
	{
	// Re-sychronize the timestamps with the capture timestamp
	body_index_map_frame->header.stamp = capture_time;
	body_index_map_frame->header.frame_id =
	calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
	body_index_map_publisher_.publish(body_index_map_frame);
	}
	}
	}

to a separate thread, the delay was able to stay at around 80 ms regardless of what light-weight tasks I run.

Alternatively the delay can be measured by printTimestampDebugMessage("Body tracking", capture_time);, running roslaunch azure_kinect_ros_driver driver.launch fps:=30 depth_mode:=WFOV_2X2BINNED body_tracking_enabled:=true imu_rate_target:=3 and change log verbosity level to debug.

[ooeygui]

Thank you for the contribution!

We need to keep the ROS1 and ROS2 trees in sync, can you make an equivalent fix to the Foxy branch and test on ROS2?

[zchenpds]

@ooeygui Sure. I will do it later today.

[ooeygui]

Sorry for the delay on this, we are working on validating it.

[zchenpds]

@amelhassan Please let me know if anything needs to fixed in this PR. I will lose access to the k4a device after this week so hopefully I can amend it to your satisfaction.

[amelhassan]

Yes I'll take a look today and let you know if any changes are needed.