Example high_rate_pose.cpp

/******************************************************************************
 *
 * Slamcore Confidential
 * ---------------------
 *
 * Slamcore Limited
 * All Rights Reserved.
 * (C) Copyright 2022
 *
 * NOTICE:
 *
 * All information contained herein is, and remains the property of Slamcore
 * Limited and its suppliers, if any. The intellectual and technical concepts
 * contained herein are proprietary to Slamcore Limited and its suppliers and
 * may be covered by patents in process, and are protected by trade secret or
 * copyright law. Dissemination of this information or reproduction of this
 * material is strictly forbidden unless prior written permission is obtained
 * from Slamcore Limited.
 *
 ******************************************************************************/

/**
 * @file
 * @ingroup slamcore_sdk_examples
 * @brief API example to show how to use high-rate pose.
 */

#include "slamcore/errors.hpp"
#include "slamcore/slam/slam_create.hpp"

#include <atomic>
#include <chrono>
#include <csignal>
#include <iomanip>
#include <iostream>
#include <stdexcept>

std::atomic_flag notInterrupted = ATOMIC_FLAG_INIT;

void interruptHandler(int)
{
  notInterrupted.clear();
}

namespace
{
void logInfo(const slamcore::LogMessageInterface& message)
{
  const time_t time = slamcore::host_clock::to_time_t(message.getTimestamp());
  struct tm tm;
  (void)localtime_r(&time, &tm);

  std::cerr << "[" << message.getSeverity() << " " << std::put_time(&tm, "%FT%T%z") << "] "
            << message.getMessage() << "\n";
}
} // namespace

int main(int argc, char* argv[])
{
  (void)notInterrupted.test_and_set();
  (void)signal(SIGINT, interruptHandler);

  if (argc < 2)
  {
    std::cout << "Usage: " << argv[0] << " <dataset-path> [<config-file>]" << std::endl;
    return -1;
  }

  std::atomic<bool> finished{false};

  // Initialise Slamcore API
  slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::logInfo);

  // Create/Connect SLAM System
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.Sources.push_back(slamcore::DataSource::Dataset);
  sysCfg.DatasetPath = argv[1];
  sysCfg.DatasetTimeScale = 1.0;
  if (argc > 2)
  {
    sysCfg.ConfigFilePath = argv[2];
  }
  std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam = slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::cerr << "Starting SLAM..." << std::endl;

  // Write CSV header
  std::cout << "process_timestamp [ns], hw_timestamp [ns], acq_timestamp [ns], t_x [m], "
               "t_y [m], t_z [m], q_w, q_x, q_y, q_z"
            << std::endl;

  // Open the device
  slam->open();

  // Set low-latency high-rate mode
  slam->setProperty(slamcore::Property::LowLatencyMode, true);

  // Enable streams of interest
  slam->setStreamEnabled(slamcore::Stream::Pose, true);

  // Catch end of dataset
  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [&finished](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const std::error_code rc = errorObj->getValue();
      if (rc == make_error_code(slamcore::errc::end_of_data))
      {
        finished.store(true);
      }
    });

  // Write pose and status to output
  const auto poseCallback = [](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose) -> void
  {
    const auto nowTime = std::chrono::duration_cast<std::chrono::nanoseconds>(
                           std::chrono::system_clock::now().time_since_epoch())
                           .count();
    const auto timestamp = pose->getHWTimestamp().time_since_epoch().count();
    const auto acqTimestamp = pose->getSourceAcquisitionTimestamp().time_since_epoch().count();
    const auto& translation = pose->getTranslation();
    const auto& rotation = pose->getRotation();
    std::cout << std::fixed << std::setprecision(6) << nowTime << ", " << timestamp << ", "
              << acqTimestamp << ", " << translation.x() << ", " << translation.y() << ", "
              << translation.z() << ", " << rotation.w() << ", " << rotation.x() << ", "
              << rotation.y() << ", " << rotation.z() << std::endl;
  };
  slam->registerCallback<slamcore::Stream::Pose>(poseCallback);

  // Start streaming
  slam->start();

  // Main receiving loop
  bool normal = true;
  while (true)
  {
    if (!slam->spinOnce())
    {
      std::cerr << "Unexpected condition, spinOnce returned false" << std::endl;
      normal = false;
      break;
    }
    if (!notInterrupted.test_and_set())
    {
      std::cerr << "Interrupted" << std::endl;
      normal = false;
      break;
    }
    if (finished)
    {
      std::cerr << "Finished dataset, exiting normally" << std::endl;
      break;
    }
  }

  // Stop SLAM
  slam->stop();

  // Disconnect/Close SLAM
  slam->close();

  // Deinitialise Slamcore API
  slamcore::slamcoreDeinit();

  return normal ? 0 : -1;
}