Example wheel_odometry.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 with wheel odometry fusion.
 *
 * Demonstrates the following:
 *
 *   - Create and interact with the SLAM system with WO support.
 *   - Feed wheel odometry data.
 *   - Receive and display data in a loop.
 *
 * @note This example will not work, i.e. it generates random WO data that doesn't match
 * visual-inertial sensors.
 * In the actual implementation please feed the data from the mobile platform driver.
 */

#include "slamcore/sensor_sources/wheel_odometry.hpp"

#include "slamcore/errors.hpp"
#include "slamcore/objects/pose_list.hpp"
#include "slamcore/sensor_source.hpp"
#include "slamcore/slam/slam_create.hpp"
#include "slamcore/types/wheel_odometry_sample.hpp"

#include <atomic>
#include <chrono>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <memory>
#include <random>
#include <stdexcept>
#include <system_error>
#include <thread>

class PoseListImpl : public slamcore::PoseListInterface<slamcore::odometry_clock>
{
public:
  PoseListImpl() { }
  virtual ~PoseListImpl() { }

  virtual std::size_t size() const { return m_vector.size(); }

  void append(value_type&& el) { m_vector.push_back(el); }

private:
  virtual const value_type& getPoseImpl(const std::size_t idx) const { return m_vector.at(idx); }

  std::vector<slamcore::WheelOdometrySample> m_vector;
};

class WheelOdometryProvider : public slamcore::SensorSourceInterface,
                              public slamcore::WheelOdometrySensorInterface
{
public:
  WheelOdometryProvider() : m_isopen(false), m_isrunning(false) { }

  virtual ~WheelOdometryProvider() { }

  /**
   * Returns the unique serial identifier associated with this sensor source.
   *
   * @note Must be one of the device(s) identifiers agreed upon with Slamcore in design phase.
   */
  virtual std::string getSerial() const { return "1234"; }

  /**
   * Lists all the sensors this source is able to provide.
   */
  virtual std::vector<slamcore::SensorIDT> listSensors() const
  {
    std::vector<slamcore::SensorIDT> ret;
    ret.push_back(slamcore::SensorIDT(slamcore::SensorType::Odometry, 0));
    return ret;
  }

  /**
   * Request to open the sensor source.
   *
   * Slamcore might register data callbacks after calling this method.
   *
   * Slamcore might call this method multiple times. Implementers
   * can return `slamcore::errc::device_already_open` in that case.
   */
  virtual std::error_code open()
  {
    if (m_isopen)
    {
      return make_error_code(slamcore::errc::device_already_open);
    }

    m_isopen = true;

    return std::error_code();
  }

  /**
   * Request to close the sensor source.
   *
   * Slamcore will not register data callbacks after calling this method.
   *
   * Slamcore might call this method multiple times. Implementers
   * can return `slamcore::errc::device_not_open` in that case.
   */
  virtual std::error_code close()
  {
    if (!m_isopen)
    {
      return make_error_code(slamcore::errc::device_not_open);
    }

    if (m_isrunning)
    {
      stop();
    }

    m_isopen = false;

    return std::error_code();
  }

  /**
   * Request to start streaming data.
   *
   * Slamcore will not register data callbacks after calling this method.
   *
   * Implementers might start invoking registered data callbacks
   * after Slamcore calls this method.
   *
   * Slamcore might call this method multiple times. Implementers
   * can return `slamcore::errc::device_already_running` in that case.
   */
  virtual std::error_code start()
  {
    if (!m_isopen)
    {
      return make_error_code(slamcore::errc::device_not_open);
    }

    if (m_isrunning)
    {
      return make_error_code(slamcore::errc::device_already_running);
    }

    m_isrunning = true;

    m_publisher = std::thread(&WheelOdometryProvider::publishing_thread, this);

    return std::error_code();
  }

  /**
   * Request to stop streaming data.
   *
   * Slamcore might de-register data callbacks after calling this method.
   *
   * Implementers must not invoke registered data callbacks
   * after Slamcore calls this method.
   *
   * Slamcore might call this method multiple times. Implementers
   * can return `slamcore::errc::device_not_running` in that case.
   */
  virtual std::error_code stop()
  {
    if (!m_isopen)
    {
      return make_error_code(slamcore::errc::device_not_open);
    }

    if (!m_isrunning)
    {
      return make_error_code(slamcore::errc::device_not_running);
    }

    m_isrunning = false;
    m_publisher.join();

    return std::error_code();
  }

  /**
   * Returns true if the device is open.
   */
  virtual bool isOpen() const { return m_isopen; }

  /**
   * Returns true if the device is running.
   *
   * This method must be re-entrant. Slamcore will call this
   * method from within the registered data callbacks.
   */
  virtual bool isRunning() const { return m_isrunning; }

  /**
   * @name Registers a callback to invoke when wheel odometry data becomes available.
   * Implementers can invoke the registered callbacks in any thread.
   */
  virtual std::error_code registerCallback(CallbackT cb)
  {
    if (m_isrunning)
    {
      return make_error_code(slamcore::errc::device_already_running);
    }

    m_callback = cb;

    return std::error_code();
  }

private:
  void publishing_thread()
  {
    slamcore::IDT sample_counter = 0;

    // random number generator for the sake of the example
    std::uniform_real_distribution<double> dist_m(-10.0, 10.0);
    std::uniform_real_distribution<double> dist_angle(-M_PI, M_PI);
    std::default_random_engine re;

    while (m_isrunning)
    {
      std::shared_ptr<PoseListImpl> sample_list(new PoseListImpl());
      slamcore::WheelOdometrySample sample;

      // translation
      const slamcore::Vector<double, 3> translation{{dist_m(re), dist_m(re), 0.0}}; // X, Y, 0

      // rotation
      const double theta = dist_angle(re); // rotation around the Z axis

      // convert to quaternion
      const double cz = std::cos(theta * 0.5);
      const double sz = std::sin(theta * 0.5);
      slamcore::Vector<double, 4> rotation{{0.0, 0.0, sz, cz}}; // X, Y, Z, W

      // fill the measurement
      sample.setValue(translation, rotation);

      // fill the metadata
      sample.setID(sample_counter++);
      sample.setSourceAcquisitionTimestamp(slamcore::host_clock::now());
      // when this sample was captured

      sample_list->append(std::move(sample));

      // feed
      if (m_callback)
      {
        m_callback(sample_list);
      }

      // to mimick some sampling period from the robot
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
  }

  bool m_isopen;
  bool m_isrunning;
  CallbackT m_callback;
  std::thread m_publisher;
};

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

  // ******************************************************************
  // Our wheel odometry plugin
  // ******************************************************************
  std::shared_ptr<WheelOdometryProvider> odom(new WheelOdometryProvider());

  slamcore::registerExternalSensor("wheel_odometry", odom);

  // ******************************************************************
  // Create/Connect SLAM System
  // ******************************************************************
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.ConfigFilePath = ""; /// @note Please pass the path your wheel odometry calibration
                              /// file here!

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

  // ******************************************************************
  // 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() << '\n';
  std::cout << "SLAM Version: " << slam_version << "/" << slam_build_ver << "/" << slam_build_type
            << '\n';

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

  // ******************************************************************
  // Create the MobileRobot subsystem
  // ******************************************************************
  if (!slam->getProperty<bool>(slamcore::Property::FeatureKinematicsEnabled))
  {
    std::cerr << "This system doesn't support wheel odometry data!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

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

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

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

  // ******************************************************************
  // Main receiving loop
  // ******************************************************************
  while (slam->spin())
  {
    ;
  }

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

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

  slam.reset();

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

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

  return 0;
}
catch (const slamcore::slam_exception& ex)
{
  std::cerr << "system_error exception! " << ex.what() << " / " << ex.code().message() << " / "
            << ex.code().value() << '\n';
  slamcore::slamcoreDeinit();
  return -1;
}
catch (const std::exception& ex)
{
  std::cerr << "Uncaught std::exception! " << ex.what() << '\n';
  slamcore::slamcoreDeinit();
  return -1;
}
catch (...)
{
  std::cerr << "Uncaught unknown exception!" << '\n';
  slamcore::slamcoreDeinit();
  return -1;
}