Merge branch 'melodic' into patch-1

amelhassan · web-flow · commit f80aca04eea7 · 2021-10-30T18:02:47.000-04:00
diff --git a/include/azure_kinect_ros_driver/k4a_ros_device.h b/include/azure_kinect_ros_driver/k4a_ros_device.h
@@ -82,6 +82,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
@@ -157,6 +160,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};
diff --git a/src/k4a_ros_device.cpp b/src/k4a_ros_device.cpp
@@ -56,6 +56,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),
@@ -312,6 +313,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();
@@ -388,6 +396,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;
 }
@@ -596,7 +607,6 @@ k4a_result_t K4AROSDevice::getRgbPointCloudInDepthFrame(const k4a::capture& capt
 
   point_cloud->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
   point_cloud->header.stamp = timestampToROS(k4a_depth_frame.get_device_timestamp());
-  printTimestampDebugMessage("RGB point cloud", point_cloud->header.stamp);
 
   return fillColorPointCloud(calibration_data_.point_cloud_image_, calibration_data_.transformed_rgb_image_,
                              point_cloud);
@@ -629,7 +639,6 @@ k4a_result_t K4AROSDevice::getRgbPointCloudInRgbFrame(const k4a::capture& captur
 
   point_cloud->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_;
   point_cloud->header.stamp = timestampToROS(k4a_depth_frame.get_device_timestamp());
-  printTimestampDebugMessage("RGB point cloud", point_cloud->header.stamp);
 
   return fillColorPointCloud(calibration_data_.point_cloud_image_, k4a_bgra_frame, point_cloud);
 }
@@ -646,7 +655,6 @@ k4a_result_t K4AROSDevice::getPointCloud(const k4a::capture& capture, sensor_msg
 
   point_cloud->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
   point_cloud->header.stamp = timestampToROS(k4a_depth_frame.get_device_timestamp());
-  printTimestampDebugMessage("Point cloud", point_cloud->header.stamp);
 
   // Tranform depth image to point cloud
   calibration_data_.k4a_transformation_.depth_image_to_point_cloud(k4a_depth_frame, K4A_CALIBRATION_TYPE_DEPTH,
@@ -758,7 +766,6 @@ k4a_result_t K4AROSDevice::getImuFrame(const k4a_imu_sample_t& sample, sensor_ms
 {
   imu_msg->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.imu_frame_;
   imu_msg->header.stamp = timestampToROS(sample.acc_timestamp_usec);
-  printTimestampDebugMessage("IMU", imu_msg->header.stamp);
 
   // The correct convention in ROS is to publish the raw sensor data, in the
   // sensor coordinate frame. Do that here.
@@ -989,7 +996,6 @@ void K4AROSDevice::framePublisherThread()
         else if (result == K4A_RESULT_SUCCEEDED)
         {
           capture_time = timestampToROS(capture.get_ir_image().get_device_timestamp());
-          printTimestampDebugMessage("IR image", capture_time);
 
           // Re-sychronize the timestamps with the capture timestamp
           ir_raw_camera_info.header.stamp = capture_time;
@@ -1021,7 +1027,6 @@ void K4AROSDevice::framePublisherThread()
           else if (result == K4A_RESULT_SUCCEEDED)
           {
             capture_time = timestampToROS(capture.get_depth_image().get_device_timestamp());
-            printTimestampDebugMessage("Depth image", capture_time);
 
             // Re-sychronize the timestamps with the capture timestamp
             depth_raw_camera_info.header.stamp = capture_time;
@@ -1053,7 +1058,6 @@ void K4AROSDevice::framePublisherThread()
           else if (result == K4A_RESULT_SUCCEEDED)
           {
             capture_time = timestampToROS(capture.get_depth_image().get_device_timestamp());
-            printTimestampDebugMessage("Depth image", capture_time);
 
             depth_rect_frame->header.stamp = capture_time;
             depth_rect_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_;
@@ -1067,11 +1071,9 @@ void K4AROSDevice::framePublisherThread()
 
 #if defined(K4A_BODY_TRACKING)
         // Publish body markers when body tracking is enabled and a depth image is available
-        if (params_.body_tracking_enabled &&
+        if (params_.body_tracking_enabled &&  k4abt_tracker_queue_size_ < 3 &&
             (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!");
@@ -1080,56 +1082,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
@@ -1155,7 +1108,6 @@ void K4AROSDevice::framePublisherThread()
           }
 
           capture_time = timestampToROS(capture.get_color_image().get_device_timestamp());
-          printTimestampDebugMessage("Color image", capture_time);
 
           rgb_jpeg_frame->header.stamp = capture_time;
           rgb_jpeg_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_;
@@ -1181,7 +1133,6 @@ void K4AROSDevice::framePublisherThread()
           }
 
           capture_time = timestampToROS(capture.get_color_image().get_device_timestamp());
-          printTimestampDebugMessage("Color image", capture_time);
 
           rgb_raw_frame->header.stamp = capture_time;
           rgb_raw_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.rgb_camera_frame_;
@@ -1209,7 +1160,6 @@ void K4AROSDevice::framePublisherThread()
           }
 
           capture_time = timestampToROS(capture.get_color_image().get_device_timestamp());
-          printTimestampDebugMessage("Color image", capture_time);
 
           rgb_rect_frame->header.stamp = capture_time;
           rgb_rect_frame->header.frame_id = calibration_data_.tf_prefix_ + calibration_data_.depth_camera_frame_;
@@ -1276,6 +1226,76 @@ 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_;
+
+      if (body_frame == nullptr)
+      {
+        ROS_ERROR_STREAM("Pop body frame result failed!");
+        ros::shutdown();
+        return;
+      }
+      else
+      {
+        auto capture_time = timestampToROS(body_frame.get_device_timestamp());
+
+        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
@@ -1503,33 +1523,3 @@ void K4AROSDevice::updateTimestampOffset(const std::chrono::microseconds& k4a_de
                                      std::floor(alpha * (device_to_realtime - device_to_realtime_offset_).count())));
   }
 }
-
-void printTimestampDebugMessage(const std::string& name, const ros::Time& timestamp)
-{
-  ros::Duration lag = ros::Time::now() - timestamp;
-  static std::map<const std::string, std::pair<ros::Duration, ros::Duration>> map_min_max;
-  auto it = map_min_max.find(name);
-  if (it == map_min_max.end())
-  {
-    map_min_max[name] = std::make_pair(lag, lag);
-    it = map_min_max.find(name);
-  }
-  else
-  {
-    auto& min_lag = it->second.first;
-    auto& max_lag = it->second.second;
-    if (lag < min_lag)
-    {
-      min_lag = lag;
-    }
-    if (lag > max_lag)
-    {
-      max_lag = lag;
-    }
-  }
-
-  ROS_DEBUG_STREAM(name << " timestamp lags ros::Time::now() by\n"
-                        << std::setw(23) << lag.toSec() * 1000.0 << " ms. "
-                        << "The lag ranges from " << it->second.first.toSec() * 1000.0 << "ms"
-                        << " to " << it->second.second.toSec() * 1000.0 << "ms.");
-}
