/******************************************************************************
*
* 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, save session and then load it.
*
* Demonstrates the following:
*
* - Create and interact with the SLAM system,
* - Save session,
* - Create new instance of a SLAM system,
* - Load session.
*/
#include "slamcore/errors.hpp"
#include "slamcore/slam/slam_create.hpp"
#include <ctime>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>
#include <thread>
int main(int argc, char* argv[])
{
if (argc < 2)
{
throw std::runtime_error("Need path to D435i dataset as argument!");
}
// ******************************************************************
// 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";
});
// ******************************************************************
// Create/Connect SLAM System
// ******************************************************************
slamcore::v0::SystemConfiguration sysCfg;
sysCfg.Sources.push_back(slamcore::DataSource::Dataset);
sysCfg.DatasetPath = argv[1];
std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam = slamcore::createSLAMSystem(sysCfg);
if (!slam)
{
std::cerr << "Error creating SLAM system!" << std::endl;
slamcore::slamcoreDeinit();
return -1;
}
// ******************************************************************
// Open the device
// ******************************************************************
slam->open();
// ******************************************************************
// Show our current mode!
// ******************************************************************
std::cout << "Positioning Mode: "
<< slam->getProperty<slamcore::PositioningMode>(slamcore::Property::PositioningMode)
<< std::endl;
// ******************************************************************
// 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"
<< "SLAM Version: " << slam_version << "/" << slam_build_ver << "/" << slam_build_type
<< std::endl;
// ******************************************************************
// Enable just the pose
// ******************************************************************
slam->setStreamEnabled(slamcore::Stream::Pose, true);
// *****************************************************************
// Register callbacks!
// *****************************************************************
slam->registerCallback<slamcore::Stream::ErrorCode>(
[](const slamcore::ErrorCodeInterface::CPtr& error)
{
const auto ec = error->getValue();
std::cout << "Received: ErrorCode\n"
<< "\t" << ec.message() << " / " << ec.value() << " / " << ec.category().name()
<< std::endl;
throw slamcore::slam_exception(ec);
});
slam->registerCallback<slamcore::Stream::Pose>(
[](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose)
{
std::cout << "Received: Pose\n"
<< "\t" << pose->getTranslation().x() << "," << pose->getTranslation().y() << ","
<< pose->getTranslation().z() << std::endl;
});
slam->registerCallback<slamcore::Stream::FrameSync>(
[](const slamcore::FrameSyncInterface::CPtr&)
{ std::cout << "Received: FrameSync" << std::endl; });
// ******************************************************************
// Start streaming
// ******************************************************************
std::cout << "Starting SLAM..." << std::endl;
slam->start();
// ******************************************************************
// Main receiving loop
// ******************************************************************
while (true)
{
try
{
slam->spinOnce();
}
catch (const slamcore::slam_exception& ex)
{
if (ex.code() == make_error_code(std::errc::timed_out))
{
std::cout << "Why timed out?" << std::endl;
continue;
}
if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
{
break;
}
throw;
}
}
// ******************************************************************
// Stop SLAM
// ******************************************************************
try
{
slam->stop();
}
catch (const slamcore::slam_exception& ex)
{
if (ex.code() != make_error_code(slamcore::errc::device_not_running))
{
throw;
}
}
// ******************************************************************
// Save session
// ******************************************************************
constexpr auto ses_path = "/tmp/test.session"; // where to save the session
const slamcore::IDT tid = slam->launchAsyncTask(slamcore::TaskType::SaveSession,
{{"filename", ses_path}});
auto sessionState = slamcore::TaskStatusInterface::TaskState::Idle;
while (true)
{
const slamcore::TaskStatusInterface::Ptr info =
slam->getTaskStatus(slamcore::TaskType::SaveSession, tid);
if (info)
{
sessionState = info->getState();
std::cout << std::unitbuf << "\rStatus: " << sessionState << std::flush;
if (sessionState != slamcore::TaskStatusInterface::TaskState::Progress)
{
std::cout << std::endl;
break;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
switch (sessionState)
{
case slamcore::TaskStatusInterface::TaskState::Success:
std::cout << "Saving session done!" << std::endl;
break;
case slamcore::TaskStatusInterface::TaskState::Progress:
slam->cancelAsyncTask(slamcore::TaskType::SaveSession, tid);
std::cout << "Saving session interrupted!" << std::endl;
break;
case slamcore::TaskStatusInterface::TaskState::Cancelled:
std::cout << "Saving session cancelled!" << std::endl;
break;
default:
std::cout << "Saving session error!" << std::endl;
break;
}
// ******************************************************************
// Disconnect/Close SLAM
// ******************************************************************
slam->close();
// ******************************************************************
// Destroy the old SLAM
// ******************************************************************
slam = nullptr;
// ******************************************************************
// Try playing it back with the session file
// ******************************************************************
if (sessionState == slamcore::TaskStatusInterface::TaskState::Success)
{
// ******************************************************************
// Create new SLAM system
// ******************************************************************
slam = slamcore::createSLAMSystem(sysCfg);
if (!slam)
{
std::cerr << "Error creating SLAM system!" << std::endl;
slamcore::slamcoreDeinit();
return -1;
}
// ******************************************************************
// Open the device
// ******************************************************************
slam->openWithSession(ses_path);
// ******************************************************************
// Show our current mode!
// ******************************************************************
std::cout << "Positioning Mode: "
<< slam->getProperty<slamcore::PositioningMode>(slamcore::Property::PositioningMode)
<< std::endl;
// ******************************************************************
// Enable just the pose
// ******************************************************************
slam->setStreamEnabled(slamcore::Stream::Pose, true);
// *****************************************************************
// Register callbacks!
// *****************************************************************
slam->registerCallback<slamcore::Stream::ErrorCode>(
[](const slamcore::ErrorCodeInterface::CPtr& error)
{
const auto ec = error->getValue();
throw slamcore::slam_exception(ec);
});
slam->registerCallback<slamcore::Stream::Pose>(
[](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose)
{
std::cout << "Received: Pose\n"
<< "\t" << pose->getTranslation().x() << "," << pose->getTranslation().y() << ","
<< pose->getTranslation().z() << std::endl;
});
// ******************************************************************
// Start streaming
// ******************************************************************
std::cout << "Starting SLAM..." << std::endl;
slam->start();
// ******************************************************************
// Main receiving loop
// ******************************************************************
while (true)
{
try
{
slam->spinOnce();
}
catch (const slamcore::slam_exception& ex)
{
if (ex.code() == make_error_code(std::errc::timed_out))
{
std::cout << "Why timed out?" << std::endl;
continue;
}
if (ex.code() == make_error_code(slamcore::errc::end_of_dataset))
{
break;
}
throw;
}
}
// ******************************************************************
// Stop SLAM
// ******************************************************************
try
{
slam->stop();
}
catch (const slamcore::slam_exception& ex)
{
if (ex.code() != make_error_code(slamcore::errc::device_not_running))
{
throw;
}
}
// ******************************************************************
// Disconnect/Close SLAM
// ******************************************************************
slam->close();
// ******************************************************************
// Destroy the old SLAM
// ******************************************************************
slam = nullptr;
}
// ******************************************************************
// Deinitialise Slamcore API
// ******************************************************************
slamcore::slamcoreDeinit();
std::cout << "We're Done Here." << std::endl;
return 0;
}