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

Author: zchenpds

include/azure_kinect_ros_driver/k4a_ros_device.h

@@ -86,6 +86,9 @@ class K4AROSDevice
                                    sensor_msgs::PointCloud2Ptr& point_cloud);
 
   void framePublisherThread();
+#if defined(K4A_BODY_TRACKING)
+  void bodyPublisherThread();
+#endif
   void imuPublisherThread();
 
   // Gets a timestap from one of the captures images
@@ -155,6 +158,8 @@ class K4AROSDevice
 #if defined(K4A_BODY_TRACKING)
   // Body tracker
   k4abt::tracker k4abt_tracker_;
+  std::atomic_int16_t k4abt_tracker_queue_size_;
+  std::thread body_publisher_thread_;
 #endif
 
   std::chrono::nanoseconds device_to_realtime_offset_{0};

src/k4a_ros_device.cpp

@@ -38,6 +38,7 @@ K4AROSDevice::K4AROSDevice(const NodeHandle& n, const NodeHandle& p)
 // clang-format off
 #if defined(K4A_BODY_TRACKING)
     k4abt_tracker_(nullptr),
+    k4abt_tracker_queue_size_(0),
 #endif
     // clang-format on
     node_(n),
@@ -252,6 +253,13 @@ K4AROSDevice::~K4AROSDevice()
   // Start tearing down the publisher threads
   running_ = false;
 
+#if defined(K4A_BODY_TRACKING)
+  // Join the publisher thread
+  ROS_INFO("Joining body publisher thread");
+  body_publisher_thread_.join();
+  ROS_INFO("Body publisher thread joined");
+#endif
+
   // Join the publisher thread
   ROS_INFO("Joining camera publisher thread");
   frame_publisher_thread_.join();
@@ -328,6 +336,9 @@ k4a_result_t K4AROSDevice::startCameras()
 
   // Start the thread that will poll the cameras and publish frames
   frame_publisher_thread_ = thread(&K4AROSDevice::framePublisherThread, this);
+#if defined(K4A_BODY_TRACKING)
+  body_publisher_thread_ = thread(&K4AROSDevice::bodyPublisherThread, this);
+#endif
 
   return K4A_RESULT_SUCCEEDED;
 }
@@ -977,8 +988,6 @@ void K4AROSDevice::framePublisherThread()
         if (params_.body_tracking_enabled &&
             (body_marker_publisher_.getNumSubscribers() > 0 || body_index_map_publisher_.getNumSubscribers() > 0))
         {
-          capture_time = timestampToROS(capture.get_depth_image().get_device_timestamp());
-
           if (!k4abt_tracker_.enqueue_capture(capture))
           {
             ROS_ERROR("Error! Add capture to tracker process queue failed!");
@@ -987,56 +996,7 @@ void K4AROSDevice::framePublisherThread()
           }
           else
           {
-            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);
-                }
-              }
-            }
+            ++k4abt_tracker_queue_size_;
           }
         }
 #endif
@@ -1183,6 +1143,75 @@ void K4AROSDevice::framePublisherThread()
   }
 }
 
+#if defined(K4A_BODY_TRACKING)
+void K4AROSDevice::bodyPublisherThread()
+{
+  while (running_ && ros::ok() && !ros::isShuttingDown())
+  {
+    if (k4abt_tracker_queue_size_ > 0)
+    {
+      k4abt::frame body_frame = k4abt_tracker_.pop_result();
+      --k4abt_tracker_queue_size_;
+      auto capture_time = timestampToROS(body_frame.get_device_timestamp());
+
+      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);
+          auto 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);
+          }
+        }
+      }
+    }
+    else 
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds{20});
+    }
+  }
+}
+#endif
+
 k4a_imu_sample_t K4AROSDevice::computeMeanIMUSample(const std::vector<k4a_imu_sample_t>& samples)
 {
   // Compute mean sample