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

#include <atomic>
#include <fstream>
#include <iomanip>
#include <thread>

void logInfo(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";
}

int main(int argc, char* argv[])
{
  if (argc < 3)
  {
    std::cout << "Usage: " << argv[0] << " <dataset-path> <output-file> [<config-file>]" << '\n';
    return -1;
  }

  // Initialise Slamcore API
  slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::logInfo);

  // Create/Connect SLAM System
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.Sources.push_back(slamcore::DataSource::Dataset);
  sysCfg.DatasetPath = argv[1];
  if (argc > 3)
  {
    sysCfg.ConfigFilePath = argv[3];
  }

  // Enable map generation
  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;

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

  // Catch end of dataset
  std::atomic<bool> finished{false};
  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [&finished](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const std::error_code rc = errorObj->getValue();
      if (rc == make_error_code(slamcore::errc::end_of_data))
      {
        finished.store(true);
      }
    });

  // Start streaming
  slam->start();

  // Main receiving loop
  while (slam->spin() && !finished)
  {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
  }

  // Stop SLAM
  slam->stop();

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

  // Get map
  heightMappingSubSystem->fetch();
  const auto map = heightMappingSubSystem->get();

  // Write map to file
  std::ofstream file;
  file.open(argv[2]);
  for (int i = map->getHeight() - 1; i >= 0; --i)
  {
    slamcore::CellCoordinates cellCoordinates;
    cellCoordinates.x = 0;
    cellCoordinates.y = i;
    uint8_t occupancyProbability = slamcore::mapGetOccupancyProbability(*map, cellCoordinates);
    file << unsigned(occupancyProbability);
    for (size_t j = 1; j < map->getWidth(); ++j)
    {
      cellCoordinates.x = j;
      cellCoordinates.y = i;
      occupancyProbability = slamcore::mapGetOccupancyProbability(*map, cellCoordinates);
      file << " " << unsigned(occupancyProbability);
    }
    file << "\n";
  }
  file.close();

  std::cout << "Map written to " << argv[2] << '\n';

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

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

  std::cout << "We're Done Here." << '\n';
}