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/slamcore.hpp>

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

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;
  sysCfg.EnableWheelOdometry = true; /// @note Remember to enable this option!
  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!" << 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 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!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

  if(!slam->isSubsystemSupported(slamcore::SubsystemType::MobileRobot))
  {
    std::cerr << "This system doesn't support wheel odometry data!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::shared_ptr<slamcore::MobileRobotSubsystemInterface> robot_feed =
    slam->getSubsystem<slamcore::MobileRobotSubsystemInterface>();

  // *****************************************************************
  // 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;
    });

  // ******************************************************************
  // Create thread for feeding the wheel odometry data
  // ******************************************************************
  std::atomic<bool> is_running(true);

  std::thread odom_provider([&is_running,&robot_feed]()
  {
    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(is_running)
    {
      std::shared_ptr<slamcore::WheelOdometrySample>
        sample(new slamcore::WheelOdometrySample());

      // translation
      const slamcore::Vector 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 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(std::chrono::system_clock::now());
      // when this sample was captured

      // feed
      robot_feed->feedOdometry(sample);

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

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

  // ******************************************************************
  // Main receiving loop
  // ******************************************************************
  while (slam->spinOnce());

  // ******************************************************************
  // Stop the wheel odometry feeding thread
  // ******************************************************************
  is_running = false;
  odom_provider.join();

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

  // ******************************************************************
  // Release the subsystem
  // ******************************************************************
  robot_feed.reset();

  // ******************************************************************
  // 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;
}