void K4AROSDevice::bodyPublisherThread()

in src/k4a_ros_device.cpp [1230:1296]
59 lines of code
12 McCabe index (conditional complexity)

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 });
    }
  }
}