void K4AROSDevice::imuPublisherThread()

in src/k4a_ros_device.cpp [1321:1434]
94 lines of code
24 McCabe index (conditional complexity)

void K4AROSDevice::imuPublisherThread()
{
  ros::Rate loop_rate(300);

  k4a_result_t result;
  k4a_imu_sample_t sample;

  // For IMU throttling
  unsigned int count = 0;
  unsigned int target_count = params_.imu_rate_target ? IMU_MAX_RATE / params_.imu_rate_target : IMU_MAX_RATE;
  std::vector<k4a_imu_sample_t> accumulated_samples;
  accumulated_samples.reserve(target_count);
  bool throttling = target_count > 1;

  while (running_ && ros::ok() && !ros::isShuttingDown())
  {
    if (k4a_device_)
    {
      // IMU messages are delivered in batches at 300 Hz. Drain the queue of IMU messages by
      // constantly reading until we get a timeout
      bool read = false;
      do
      {
        read = k4a_device_.get_imu_sample(&sample, std::chrono::milliseconds(0));

        if (read)
        {
          if (throttling)
          {
            accumulated_samples.push_back(sample);
            count++;
          }

          if (count % target_count == 0)
          {
            ImuPtr imu_msg(new Imu);

            if (throttling)
            {
              k4a_imu_sample_t mean_sample_float = computeMeanIMUSample(accumulated_samples);
              result = getImuFrame(mean_sample_float, imu_msg);
              accumulated_samples.clear();
              count = 0;
            }
            else
            {
              result = getImuFrame(sample, imu_msg);
            }

            ROS_ASSERT_MSG(result == K4A_RESULT_SUCCEEDED, "Failed to get IMU frame");

            if (std::abs(imu_msg->angular_velocity.x) > DBL_EPSILON ||
                std::abs(imu_msg->angular_velocity.y) > DBL_EPSILON ||
                std::abs(imu_msg->angular_velocity.z) > DBL_EPSILON){
              imu_orientation_publisher_.publish(imu_msg);
            }
          }
        }

      } while (read);
    }
    else if (k4a_playback_handle_)
    {
      // publish imu messages as long as the imu timestamp is less than the last capture timestamp to catch up to the
      // cameras compare signed with unsigned shouldn't cause a problem because timestamps should always be positive
      while (last_imu_time_usec_ <= last_capture_time_usec_ && !imu_stream_end_of_file_)
      {
        std::lock_guard<std::mutex> guard(k4a_playback_handle_mutex_);
        if (!k4a_playback_handle_.get_next_imu_sample(&sample))
        {
          imu_stream_end_of_file_ = true;
        }
        else
        {
          if (throttling)
          {
            accumulated_samples.push_back(sample);
            count++;
          }

          if (count % target_count == 0)
          {
            ImuPtr imu_msg(new Imu);

            if (throttling)
            {
              k4a_imu_sample_t mean_sample_float = computeMeanIMUSample(accumulated_samples);
              result = getImuFrame(mean_sample_float, imu_msg);
              accumulated_samples.clear();
              count = 0;
            }
            else
            {
              result = getImuFrame(sample, imu_msg);
            }

            ROS_ASSERT_MSG(result == K4A_RESULT_SUCCEEDED, "Failed to get IMU frame");

            if (std::abs(imu_msg->angular_velocity.x) > DBL_EPSILON ||
                std::abs(imu_msg->angular_velocity.y) > DBL_EPSILON ||
                std::abs(imu_msg->angular_velocity.z) > DBL_EPSILON){
              imu_orientation_publisher_.publish(imu_msg);
            }

            last_imu_time_usec_ = sample.acc_timestamp_usec;
          }
        }
      }
    }

    ros::spinOnce();
    loop_rate.sleep();
  }
}