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::SLAMSystemPollingInterface::open, slamcore::SLAMSystemPollingInterface::start,
 * slamcore::SLAMSystemPollingInterface::close etc.)
 *   - Enable/use data streams.
 *   - Toggling of slamcore::Property(s).
 *   - Receive and display data in a loop.
 */

#include <slamcore/slamcore.hpp>

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

int main(int argc, char* argv[])
try
{
  // ******************************************************************
  // 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;
  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& img : *multiFrameObj)
      {
        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: " << static_cast<int>(imuObj->type())
                    << 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;
}
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;
}