k4a_result_t K4AROSDevice::startCameras()

in src/k4a_ros_device.cpp [349:404]

• 37 lines of code
• 6 McCabe index (conditional complexity)

k4a_result_t K4AROSDevice::startCameras()
{
  k4a_device_configuration_t k4a_configuration = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;

  if (k4a_device_)
  {
    k4a_result_t result = params_.GetDeviceConfig(&k4a_configuration);
    if (result != K4A_RESULT_SUCCEEDED)
    {
      ROS_ERROR("Failed to generate a device configuration. Not starting camera!");
      return result;
    }

    // Now that we have a proposed camera configuration, we can
    // initialize the class which will take care of device calibration information
    calibration_data_.initialize(k4a_device_, k4a_configuration.depth_mode, k4a_configuration.color_resolution,
                                 params_);
  }
  else if (k4a_playback_handle_)
  {
    // initialize the class which will take care of device calibration information from the playback_handle
    calibration_data_.initialize(k4a_playback_handle_, params_);
  }

#if defined(K4A_BODY_TRACKING)
  // When calibration is initialized the body tracker can be created with the device calibration
  if (params_.body_tracking_enabled)
  {
    k4abt_tracker_ = k4abt::tracker::create(calibration_data_.k4a_calibration_);
    k4abt_tracker_.set_temporal_smoothing(params_.body_tracking_smoothing_factor);
  }
#endif

  if (k4a_device_)
  {
    ROS_INFO_STREAM("STARTING CAMERAS");
    k4a_device_.start_cameras(&k4a_configuration);
  }

  // Cannot assume the device timestamp begins increasing upon starting the cameras.
  // If we set the time base here, depending on the machine performance, the new timestamp
  // would lag the value of ros::Time::now() by at least 0.5 secs which is much larger than
  // the real transmission delay as can be observed using the rqt_plot tool.
  // start_time_ = ros::Time::now();

  // Prevent the worker thread from exiting immediately
  running_ = true;

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