You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.
Example high_rate_pose.cpp
/******************************************************************************
*
* Slamcore Confidential
* ---------------------
*
* Slamcore Limited
* All Rights Reserved.
* (C) Copyright 2022
*
* 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 high-rate pose.
*/
#include "slamcore/errors.hpp"
#include "slamcore/slam/slam_create.hpp"
#include <atomic>
#include <chrono>
#include <csignal>
#include <iomanip>
#include <iostream>
#include <stdexcept>
std::atomic_flag notInterrupted = ATOMIC_FLAG_INIT;
void interruptHandler(int)
{
notInterrupted.clear();
}
namespace
{
void logInfo(const slamcore::LogMessageInterface& message)
{
const time_t time = slamcore::host_clock::to_time_t(message.getTimestamp());
struct tm tm;
(void)localtime_r(&time, &tm);
std::cerr << "[" << message.getSeverity() << " " << std::put_time(&tm, "%FT%T%z") << "] "
<< message.getMessage() << "\n";
}
} // namespace
int main(int argc, char* argv[])
{
(void)notInterrupted.test_and_set();
(void)signal(SIGINT, interruptHandler);
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " <dataset-path> [<config-file>]" << std::endl;
return -1;
}
std::atomic<bool> finished{false};
// 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];
sysCfg.DatasetTimeScale = 1.0;
if (argc > 2)
{
sysCfg.ConfigFilePath = argv[2];
}
std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam = slamcore::createSLAMSystem(sysCfg);
if (!slam)
{
std::cerr << "Error creating SLAM system!" << std::endl;
slamcore::slamcoreDeinit();
return -1;
}
std::cerr << "Starting SLAM..." << std::endl;
// Write CSV header
std::cout << "process_timestamp [ns], hw_timestamp [ns], acq_timestamp [ns], t_x [m], "
"t_y [m], t_z [m], q_w, q_x, q_y, q_z"
<< std::endl;
// Open the device
slam->open();
// Set low-latency high-rate mode
slam->setProperty(slamcore::Property::LowLatencyMode, true);
// Enable streams of interest
slam->setStreamEnabled(slamcore::Stream::Pose, true);
// Catch end of dataset
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);
}
});
// Write pose and status to output
const auto poseCallback = [](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& pose) -> void
{
const auto nowTime = std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::system_clock::now().time_since_epoch())
.count();
const auto timestamp = pose->getHWTimestamp().time_since_epoch().count();
const auto acqTimestamp = pose->getSourceAcquisitionTimestamp().time_since_epoch().count();
const auto& translation = pose->getTranslation();
const auto& rotation = pose->getRotation();
std::cout << std::fixed << std::setprecision(6) << nowTime << ", " << timestamp << ", "
<< acqTimestamp << ", " << translation.x() << ", " << translation.y() << ", "
<< translation.z() << ", " << rotation.w() << ", " << rotation.x() << ", "
<< rotation.y() << ", " << rotation.z() << std::endl;
};
slam->registerCallback<slamcore::Stream::Pose>(poseCallback);
// Start streaming
slam->start();
// Main receiving loop
bool normal = true;
while (true)
{
if (!slam->spinOnce())
{
std::cerr << "Unexpected condition, spinOnce returned false" << std::endl;
normal = false;
break;
}
if (!notInterrupted.test_and_set())
{
std::cerr << "Interrupted" << std::endl;
normal = false;
break;
}
if (finished)
{
std::cerr << "Finished dataset, exiting normally" << std::endl;
break;
}
}
// Stop SLAM
slam->stop();
// Disconnect/Close SLAM
slam->close();
// Deinitialise Slamcore API
slamcore::slamcoreDeinit();
return normal ? 0 : -1;
}