Example reset.cpp

/******************************************************************************
 *
 * Slamcore Confidential
 * ---------------------
 *
 * Slamcore Limited
 * All Rights Reserved.
 * (C) Copyright 2021
 *
 * 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 showcase how to "reset" the SLAM system
 *
 * If the user wishes to reset the SLAM system to start processing a data source
 * cleanly, the common practice is to delete the current system and create a new
 * one.
 *
 * Bear in mind that it is a restriction that only one SLAM system can exist at
 * any given time. The internal SLAM object will be deleted when the unique
 * pointer returned by `createSLAMSystem()` stops managing it, and this can be
 * achieved by calling `reset` or assigning it to `nullptr`. Subsequently, a new
 * SLAM system could be created.
 */

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

#include <atomic>
#include <csignal>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>

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

namespace
{
/**
 * Logging callback.
 *
 * @param message Message received by the logging system.
 */
void logInfo(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";
}

/**
 * Callback executed when Pose data is received.
 *
 * @param poseObj Pointer to the received Pose.
 */
void handlePose(const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& poseObj)
{
  std::cout << "Position: " << poseObj->getTranslation().x() << ", "
            << poseObj->getTranslation().y() << ", " << poseObj->getTranslation().z() << '\n';
  std::cout << "Rotation: " << poseObj->getRotation().x() << ", " << poseObj->getRotation().y()
            << ", " << poseObj->getTranslation().z() << ", " << poseObj->getRotation().w() << '\n';
}

/**
 * Callback executed when an Error is received.
 *
 * @param errorObj Pointer to the received error.
 */
void handleError(const slamcore::ErrorCodeInterface::CPtr& errorObj)
{
  const std::error_code rc = errorObj->getValue();
  if (rc == make_error_code(slamcore::errc::end_of_data))
  {
    s_finished.store(true);
  }
}

/**
 * Convenience function to encapsulate the SLAM system's setting up and
 * processing.
 *
 * @param slam Interface to the SLAM system.
 */
void process(const std::unique_ptr<slamcore::SLAMSystemCallbackInterface>& slam)
{
  // Open the device
  slam->open();

  // Enable the Pose stream
  slam->setStreamEnabled(slamcore::Stream::Pose, true);

  // Register callbacks
  slam->registerCallback<slamcore::Stream::ErrorCode>(::handleError);
  slam->registerCallback<slamcore::Stream::Pose>(::handlePose);

  // Start streaming
  slam->start();

  // Main receiving loop
  // You can either let it finish or press Ctrl-C to interrupt it.
  while (slam->spin() && !s_finished)
  {
    ;
  }

  // Stop SLAM
  std::cout << "Stopping..." << '\n';
  slam->stop();

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

} // namespace

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

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

  // 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];
  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 SLAM..." << '\n';

  process(slam);

  // ******************************************************************
  // Reset the unique_ptr so the internal SLAM object is deleted
  // ******************************************************************
  slam.reset();

  // Create/Connect new SLAM System
  slam = slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

  s_finished.store(false);

  std::cout << "Starting SLAM again ..." << '\n';

  process(slam);

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

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