You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.

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/errors.hpp"
#include "slamcore/slam/slam_create.hpp"

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

int main(int argc, char* argv[])
{
  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 = 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/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!" << 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() << "\n"
            << "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\n"
                << "\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\n"
                << "\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;
      }
      if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
      {
        break;
      }
      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
  // ******************************************************************
  constexpr auto ses_path = "/tmp/test.session"; // where to save the session
  const slamcore::IDT tid = slam->launchAsyncTask(slamcore::TaskType::SaveSession,
                                                  {{"filename", ses_path}});
  auto 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);

    // ******************************************************************
    // 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\n"
                  << "\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;
        }
        if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
        {
          break;
        }
        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;
}