Example save_load_session.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 set up and run a SLAM system, save session and then load it.
 *
 * Demonstrates the following:
 *
 *   - Create and interact with the SLAM system,
 *   - Save session,
 *   - Create new instance of a SLAM system,
 *   - Load session.
 */

#include <slamcore/slamcore.hpp>

#include <ctime>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>
#include <thread>

int main(int argc, char* argv[])
try
{
  if (argc < 2)
  {
    throw std::runtime_error("Need path to D435i dataset as argument!");
  }

  // ******************************************************************
  // Initialise SLAMcore API
  // ******************************************************************
  slamcore::slamcoreInit(
    slamcore::LogSeverity::Info, [](const slamcore::LogMessageInterface& message) {
      const time_t time = std::chrono::system_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/Connect SLAM System
  // ******************************************************************
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.Source = slamcore::DataSource::Dataset;
  sysCfg.DatasetPath = argv[1];

  std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam =
    slamcore::createSLAMSystem(sysCfg);

  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

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

  // ******************************************************************
  // Show our current mode!
  // ******************************************************************
  std::cout << "Positioning Mode: "
            << slam->getProperty<slamcore::PositioningMode>(
                 slamcore::Property::PositioningMode)
            << std::endl;

  // ******************************************************************
  // Print versions
  // ******************************************************************
  const std::string slam_version =
    slam->getProperty<std::string>(slamcore::Property::FirmwareVersion);
  const std::string slam_build_ver =
    slam->getProperty<std::string>(slamcore::Property::FirmwareBuildVersion);
  const std::string slam_build_type =
    slam->getProperty<std::string>(slamcore::Property::FirmwareBuildType);

  std::cout << "Client Version: " << slamcore::getVersion() << "/"
            << slamcore::getBuildVersion() << "/" << slamcore::getBuildType() << std::endl;
  std::cout << "SLAM Version: " << slam_version << "/" << slam_build_ver << "/"
            << slam_build_type << std::endl;

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

  // *****************************************************************
  // Register callbacks!
  // *****************************************************************

  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [](const slamcore::ErrorCodeInterface::CPtr& error) {
      const auto ec = error->getValue();
      std::cout << "Received: ErrorCode" << std::endl;
      std::cout << "\t" << ec.message() << " / " << ec.value() << " / "
                << ec.category().name() << std::endl;
      throw slamcore::slam_exception(ec);
    });

  slam->registerCallback<slamcore::Stream::Pose>(
    [](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose) {
      std::cout << "Received: Pose" << std::endl;
      std::cout << "\t" << pose->getTranslation().x() << "," << pose->getTranslation().y()
                << "," << pose->getTranslation().z() << std::endl;
    });

  slam->registerCallback<slamcore::Stream::FrameSync>(
    [](const slamcore::FrameSyncInterface::CPtr&) {
      std::cout << "Received: FrameSync" << std::endl;
    });

  // ******************************************************************
  // Start streaming
  // ******************************************************************
  std::cout << "Starting SLAM..." << std::endl;
  slam->start();

  // ******************************************************************
  // Main receiving loop
  // ******************************************************************
  while (true)
  {
    try
    {
      slam->spinOnce();
    }
    catch (const slamcore::slam_exception& ex)
    {
      if (ex.code() == make_error_code(std::errc::timed_out))
      {
        std::cout << "Why timed out?" << std::endl;
        continue;
      }
      else if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
      {
        break;
      }
      else
      {
        throw;
      }
    }
  }

  // ******************************************************************
  // Stop SLAM
  // ******************************************************************
  try
  {
    slam->stop();
  }
  catch (const slamcore::slam_exception& ex)
  {
    if (ex.code() != make_error_code(slamcore::errc::device_not_running))
    {
      throw;
    }
  }

  // ******************************************************************
  // Save session
  // ******************************************************************
  const std::string ses_path = "/tmp/test.session";  // where to save the session
  const slamcore::IDT tid =
    slam->launchAsyncTask(slamcore::TaskType::SaveSession, {{"filename", ses_path}});
  slamcore::TaskStatusInterface::TaskState sessionState =
    slamcore::TaskStatusInterface::TaskState::Idle;

  while (true)
  {
    const slamcore::TaskStatusInterface::Ptr info =
      slam->getTaskStatus(slamcore::TaskType::SaveSession, tid);
    if (info)
    {
      sessionState = info->getState();

      std::cout << std::unitbuf << "\rStatus: " << sessionState << std::flush;

      if (sessionState != slamcore::TaskStatusInterface::TaskState::Progress)
      {
        std::cout << std::endl;
        break;
      }
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  switch (sessionState)
  {
  case slamcore::TaskStatusInterface::TaskState::Success:
    std::cout << "Saving session done!" << std::endl;
    break;
  case slamcore::TaskStatusInterface::TaskState::Progress:
    slam->cancelAsyncTask(slamcore::TaskType::SaveSession, tid);
    std::cout << "Saving session interrupted!" << std::endl;
    break;
  case slamcore::TaskStatusInterface::TaskState::Cancelled:
    std::cout << "Saving session cancelled!" << std::endl;
    break;
  default:
    std::cout << "Saving session error!" << std::endl;
    break;
  }

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

  // ******************************************************************
  // Destroy the old SLAM
  // ******************************************************************
  slam = nullptr;

  // ******************************************************************
  // Try playing it back with the session file
  // ******************************************************************
  if (sessionState == slamcore::TaskStatusInterface::TaskState::Success)
  {
    // ******************************************************************
    // Create new SLAM system
    // ******************************************************************
    slam = slamcore::createSLAMSystem(sysCfg);
    if (!slam)
    {
      std::cerr << "Error creating SLAM system!" << std::endl;
      slamcore::slamcoreDeinit();
      return -1;
    }

    // ******************************************************************
    // Open the device
    // ******************************************************************
    slam->openWithSession(ses_path.c_str());

    // ******************************************************************
    // Show our current mode!
    // ******************************************************************
    std::cout << "Positioning Mode: "
              << slam->getProperty<slamcore::PositioningMode>(
                   slamcore::Property::PositioningMode)
              << std::endl;

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

    // *****************************************************************
    // Register callbacks!
    // *****************************************************************

    slam->registerCallback<slamcore::Stream::ErrorCode>(
      [](const slamcore::ErrorCodeInterface::CPtr& error) {
        const auto ec = error->getValue();
        throw slamcore::slam_exception(ec);
      });

    slam->registerCallback<slamcore::Stream::Pose>(
      [](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose) {
        std::cout << "Received: Pose" << std::endl;
        std::cout << "\t" << pose->getTranslation().x() << "," << pose->getTranslation().y()
                  << "," << pose->getTranslation().z() << std::endl;
      });

    // ******************************************************************
    // Start streaming
    // ******************************************************************
    std::cout << "Starting SLAM..." << std::endl;
    slam->start();

    // ******************************************************************
    // Main receiving loop
    // ******************************************************************
    while (true)
    {
      try
      {
        slam->spinOnce();
      }
      catch (const slamcore::slam_exception& ex)
      {
        if (ex.code() == make_error_code(std::errc::timed_out))
        {
          std::cout << "Why timed out?" << std::endl;
          continue;
        }
        else if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
        {
          break;
        }
        else
        {
          throw;
        }
      }
    }

    // ******************************************************************
    // Stop SLAM
    // ******************************************************************
    try
    {
      slam->stop();
    }
    catch (const slamcore::slam_exception& ex)
    {
      if (ex.code() != make_error_code(slamcore::errc::device_not_running))
      {
        throw;
      }
    }

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

    // ******************************************************************
    // Destroy the old SLAM
    // ******************************************************************
    slam = nullptr;
  }

  // ******************************************************************
  // Deinitialise SLAMcore API
  // ******************************************************************
  slamcore::slamcoreDeinit();

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

  return 0;
}
catch (const slamcore::slam_exception& ex)
{
  std::cerr << "system_error exception! " << ex.what() << " / " << ex.code().message()
            << " / " << ex.code().value() << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}
catch (const std::exception& ex)
{
  std::cerr << "Uncaught std::exception! " << ex.what() << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}
catch (...)
{
  std::cerr << "Uncaught unknown exception!" << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}