Example record_dataset.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 set up and run a pass-through SLAM system to record datasets
 */

#include "slamcore/slam/slam_create.hpp"

#include <atomic>
#include <csignal>
#include <ctime>
#include <iomanip>
#include <thread>

// Flag variable indicating processing completed or stopped.
static std::atomic<bool> s_finished{false};

// Callback to be executed on SIGINT.
static void handleSignal(int)
{
  s_finished.store(true);
}

int main(int argc, char* argv[])
{
  if (argc < 2)
  {
    std::cout << "Usage: " << argv[0] << " <dataset-output-path>" << '\n';
    return -1;
  }

  // Handle Ctrl-C
  signal(SIGINT, handleSignal);

  // Initialise Slamcore API
  slamcore::slamcoreInit(slamcore::LogSeverity::Info,
                         [](const slamcore::LogMessageInterface& message)
                         {
                           const time_t time = slamcore::host_clock::to_time_t(message.getTimestamp());
                           struct tm tm;
                           localtime_r(&time, &tm);

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

  // Create SLAM System
  slamcore::v0::SystemConfiguration sysCfg;

  // Disable SLAM - record dataset only
  sysCfg.DisableSLAM = true;

  // Set the destination dataset directory
  sysCfg.DatasetWritePath = std::string(argv[1]);

  std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam = slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::cout << "Starting dataset recording..." << '\n';

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

  // Register the error callback
  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const std::error_code ec = errorObj->getValue();
      std::cerr << "Error: " << ec.message() << " / " << ec.value() << '\n';
      s_finished.store(true);
    });

  // Start streaming
  slam->start();

  // Record until error or CTRL+C
  while (!s_finished)
  {
    // Spin to consume errors (if any), but not indefinitely, you might not get any
    slam->spin(std::chrono::milliseconds(60));
  }

  // Stop SLAM
  slam->stop();

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

  slam.reset();

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

  std::cout << "We're Done Here." << '\n';

  return 0;
}