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