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

#include <sys/ioctl.h>

#include <iomanip>
#include <iostream>

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::Vector<double, 3>& 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::DynamicVector 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
            << "'" << '\n';
};

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());
                           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.Sources.push_back(slamcore::DataSource::AutoDetect);
  sysCfg.GenerateMap = true;
  // We need depth to generate the map and, on ARM, it's disabled by default.
  sysCfg.SensorEnableMap[slamcore::SensorIDT(slamcore::SensorType::Depth, 0)] = 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!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::cout << "Starting SLAM..." << '\n';

  // ******************************************************************
  // 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" << '\n';
      std::cout << "\t" << errorCode.message() << " / " << errorCode.value() << " / "
                << errorCode.category().name() << '\n';
    });

  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..." << '\n';

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

  while (slam->spin())
  {
    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;
    }
  }

  slamcore::slamcoreDeinit();

  return 0;
}