Example ascii_mapping.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 show how to use mapping subsystem and helpers.
 */

#include <slamcore/slamcore.hpp>

#include <iomanip>
#include <iostream>

#include <sys/ioctl.h>

void printMap(const slamcore::Map2DInterface& map,
              const slamcore::PoseInterface<slamcore::camera_clock>& pose)
{
  if(!map.hasChannel(slamcore::MapChannelType::OccupancyProbability))
  {
    return;
  }

  constexpr char unknown     = '.';
  constexpr char outOfBounds = '*';
  constexpr char occupied    = '#';
  constexpr char unoccupied  = ' ';
  constexpr char currentPose = 'x';

  const slamcore::VectorInterface& poseTranslation = pose.getTranslation();
  const slamcore::MapChannel2DInterface& occupancyChannel =
    map.getChannel(slamcore::MapChannelType::OccupancyProbability);

  winsize w;
  ioctl(fileno(stdout), TIOCGWINSZ, &w);
  const int localMapHeight = (w.ws_row / 2) - 1;
  const int localMapWidth = w.ws_col / 4;

  for(int ii = 0; ii <= w.ws_row; ++ii)
  {
    std::cout << "\n";
  }

  // display the map in world aligned coordinates with x right, y up
  for(int jj = localMapHeight; jj >= -localMapHeight; --jj)
  {
    for(int ii = -localMapWidth; ii < localMapWidth; ++ii)
    {

      if(ii == 0 && jj == 0)
      {
        std::cout << currentPose << " ";
        continue;
      }

      constexpr double MapResolutionDisplay = 0.1; // m
      const slamcore::Vector mapPoint(poseTranslation.x() + MapResolutionDisplay * ii,
                                      poseTranslation.y() + MapResolutionDisplay * jj);
      if(slamcore::pointWithinMap(map, mapPoint))
      {
        const slamcore::CellCoordinates cellCoordinates = mapPointToCell(map, mapPoint);
        const uint8_t occupancyProbability =
          mapGetOccupancyProbability(map, cellCoordinates);
        if(occupancyProbability == occupancyChannel.getValueMin())
        {
          std::cout << unoccupied << " ";
        }
        else if(occupancyProbability == occupancyChannel.getValueMax())
        {
          std::cout << occupied << " ";
        }
        else
        {
          std::cout << unknown << " ";
        }
      }
      else
      {
        std::cout << outOfBounds << " ";
      }
    }
    std::cout << "\n";
  }

  std::cout << "Key: " << "occupied='" << occupied << "', unoccupied='" << unoccupied
            << "', unknown='" << unknown << "', out of bounds='" << outOfBounds
            << "', current pose='" << currentPose << "'" << std::endl;
};

int main(int argc, char* argv[])
{
  // ******************************************************************
  // 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());
      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;

  // As we are not loading a session file, generate a new map
  sysCfg.Source = slamcore::DataSource::RealSense;
  sysCfg.GenerateMap = true;

  // Optional additional configuration inputs
  // sysCfg.DatasetPath = "/path/to/dataset/";
  // sysCfg.ConfigFilePath = "/path/to/config/file";
  // sysCfg.LoadSessionFilePath = "/path/to/session/file";

  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();

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

  // ******************************************************************
  // Register Pose and Error Callback
  // ******************************************************************
  slamcore::PoseInterface<slamcore::camera_clock>::CPtr pose;
  slam->registerCallback<slamcore::Stream::Pose>(
    [&pose](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& poseObj)
    {
      pose = poseObj;
    });

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

  bool frameSync = false;
  slam->registerCallback<slamcore::Stream::FrameSync>(
    [&frameSync](const slamcore::FrameSyncInterface::CPtr& sync)
    {
      frameSync = true;
    });

  const std::shared_ptr<slamcore::HeightMappingSubsystemInterface> heightMappingSubSystem =
    slam->getSubsystem<slamcore::HeightMappingSubsystemInterface>();

  slam->start();

  std::cout << "Waiting for the first map..." << std::endl;

  constexpr auto refreshRate = 2.0; // Hz
  const auto refreshPeriod = std::chrono::seconds(1) / refreshRate;
  auto nextTime = std::chrono::steady_clock::now() + refreshPeriod;

  while (slam->spinOnce())
  {
    const auto now = std::chrono::steady_clock::now();
    if(frameSync && now >= nextTime)
    {
      frameSync = false;
      heightMappingSubSystem->fetch();

      const auto map = heightMappingSubSystem->get();
      printMap(*map, *pose);
      nextTime = now + refreshPeriod;
    }

  }
}