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

Example hello_world_all_streams.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, enabling all streams.
 *
 * Demonstrates the following:
 *
 *   - Create and interact with the SLAM system
 * (slamcore::SLAMSystemCallbackInterface::open, slamcore::SLAMSystemCallbackInterface::start,
 * slamcore::SLAMSystemCallbackInterface::close etc.)
 *   - Enable/use data streams.
 *   - Toggling of slamcore::Property(s).
 *   - Receive and display data in a loop.
 */

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

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

int main(int argc, char* argv[])
{
  // ******************************************************************
  // 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;
  std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam = slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

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

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

  // ******************************************************************
  // 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 all the streams
  // ******************************************************************
  slam->setStreamEnabled(slamcore::Stream::Pose, true);
  slam->setStreamEnabled(slamcore::Stream::Video, true);
  slam->setStreamEnabled(slamcore::Stream::IMU, true);
  slam->setStreamEnabled(slamcore::Stream::ActiveMap, true);
  slam->setStreamEnabled(slamcore::Stream::Velocity, true);
  slam->setStreamEnabled(slamcore::Stream::MetaData, true);
  slam->setStreamEnabled(slamcore::Stream::LocalPointCloud, true);

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

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

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

  slam->registerCallback<slamcore::Stream::Video>(
    [](const slamcore::MultiFrameInterface::CPtr& multiFrameObj)
    {
      std::cout << "Received: MultiFrame" << std::endl;
      std::cout << "\t" << multiFrameObj->size() << " "
                << (multiFrameObj->isKeyFrame() ? "KF" : "NON-KF") << std::endl;
      for (const auto& frame : *multiFrameObj)
      {
        const auto& img = frame.image();
        std::cout << "\t\t" << img.getWidth() << " x " << img.getHeight() << " at "
                  << std::chrono::duration_cast<std::chrono::nanoseconds>(
                       img.getHWTimestamp().time_since_epoch())
                       .count()
                  << " [ns]" << std::endl;
      }
    });

  slam->registerCallback<slamcore::Stream::IMU>(
    [](const slamcore::IMUListInterface::CPtr& imuObj)
    {
      std::cout << "Received: IMUList" << std::endl;
      for (std::size_t idx = 0; idx < imuObj->size(); ++idx)
      {
        const auto& meas = imuObj->get(idx);
        switch (meas.getSensorID().first)
        {
          case (slamcore::SensorType::Accelerometer):
          {
            std::cout << "\tTimestamp: "
                      << std::chrono::duration_cast<std::chrono::nanoseconds>(
                           meas.getHWTimestamp().time_since_epoch())
                           .count();
            std::cout << "\tAccelerometer: " << meas.getMeasurement().x() << ","
                      << meas.getMeasurement().y() << "," << meas.getMeasurement().z() << std::endl;
            break;
          }
          case (slamcore::SensorType::Gyroscope):
          {
            std::cout << "\tTimestamp: "
                      << std::chrono::duration_cast<std::chrono::nanoseconds>(
                           meas.getHWTimestamp().time_since_epoch())
                           .count();
            std::cout << "\tGyroscope: " << meas.getMeasurement().x() << ","
                      << meas.getMeasurement().y() << "," << meas.getMeasurement().z() << std::endl;
            break;
          }
          default:
            std::cerr << "Unknown IMU measurement: " << meas.getSensorID().first << std::endl;
        }
      }
    });

  slam->registerCallback<slamcore::Stream::Velocity>(
    [](const slamcore::VelocityInterface<slamcore::camera_clock>::CPtr& velObj)
    {
      std::cout << "Received: Velocities" << std::endl;
      std::cout << "\tLinear: " << velObj->getLinear().x() << "," << velObj->getLinear().y() << ","
                << velObj->getLinear().z() << std::endl;
      std::cout << "\tAngular: " << velObj->getAngular().x() << "," << velObj->getAngular().y()
                << "," << velObj->getAngular().z() << std::endl;
    });

  slam->registerCallback<slamcore::Stream::ActiveMap>(
    [](const slamcore::SparseMapInterface::CPtr& mapObj)
    {
      std::cout << "Received: SparseMap" << std::endl;
      std::cout << "\tLandmarks: " << mapObj->size() << std::endl;
    });

  slam->registerCallback<slamcore::Stream::MetaData>(
    [](const slamcore::MetaDataInterface::CPtr& metaObj)
    {
      std::cout << "Received: MetaData" << std::endl << "\tID: ";
      slamcore::MetaDataID ID = metaObj->getID();

      if (ID >= slamcore::MetaDataID::Count)
      {
        std::cout << "Unknown MetaData ID";
      }
      else
      {
        std::cout << ID;
      }

      std::cout << std::endl << "\tValue: ";

      switch (metaObj->getValueType())
      {
        case slamcore::MetaDataInterface::ValueType::Integer:
        {
          int64_t ival = 0;
          metaObj->getValue(ival);
          std::cout << ival << std::endl;
        }
        break;
        case slamcore::MetaDataInterface::ValueType::Floating:
        {
          double dval = 0.0;
          metaObj->getValue(dval);
          std::cout << dval << std::endl;
        }
        break;
        case slamcore::MetaDataInterface::ValueType::String:
        {
          std::string sval;
          metaObj->getValue(sval);
          std::cout << sval << std::endl;
        }
        break;
        default:
        {
          std::cout << "Unknown data type" << std::endl;
        }
        break;
      }
    });

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

  slam->registerCallback<slamcore::Stream::LocalPointCloud>(
    [](const slamcore::PointCloudInterface::CPtr& pointCloudObj)
    {
      std::cout << "Received: LocalPointCloud" << std::endl;
      std::cout << "\tNumber of Points: " << pointCloudObj->size() << std::endl;
    });

  // ******************************************************************
  // Start streaming
  // ******************************************************************
  slam->start();

  // ******************************************************************
  // Main receiving loop
  // ******************************************************************
  while (slam->spinOnce())
    ;

  // ******************************************************************
  // Stop SLAM
  // ******************************************************************
  slam->stop();

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

  slam.reset();

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

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

  return 0;
}