You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.
Example hello_world_all_streams.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, enabling all streams.
*
* Demonstrates the following:
*
* - Create and interact with the SLAM system
* (slamcore::SLAMSystemPollingInterface::open, slamcore::SLAMSystemPollingInterface::start,
* slamcore::SLAMSystemPollingInterface::close etc.)
* - Enable/use data streams.
* - Toggling of slamcore::Property(s).
* - Receive and display data in a loop.
*/
#include <slamcore/slamcore.hpp>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <system_error>
#include <thread>
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;
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 all the streams
// ******************************************************************
slam->setStreamEnabled(slamcore::Stream::Pose, true);
slam->setStreamEnabled(slamcore::Stream::Video, true);
slam->setStreamEnabled(slamcore::Stream::IMU, true);
slam->setStreamEnabled(slamcore::Stream::ActiveMap, true);
slam->setStreamEnabled(slamcore::Stream::Velocity, true);
slam->setStreamEnabled(slamcore::Stream::MetaData, true);
slam->setStreamEnabled(slamcore::Stream::LocalPointCloud, true);
// *****************************************************************
// 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;
});
slam->registerCallback<slamcore::Stream::Video>(
[](const slamcore::MultiFrameInterface::CPtr& multiFrameObj) {
std::cout << "Received: MultiFrame" << std::endl;
std::cout << "\t" << multiFrameObj->size() << " "
<< (multiFrameObj->isKeyFrame() ? "KF" : "NON-KF") << std::endl;
for (const auto& img : *multiFrameObj)
{
std::cout << "\t\t" << img.getWidth() << " x " << img.getHeight() << " at "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
img.getHWTimestamp().time_since_epoch())
.count()
<< " [ns]" << std::endl;
}
});
slam->registerCallback<slamcore::Stream::IMU>(
[](const slamcore::IMUListInterface::CPtr& imuObj) {
std::cout << "Received: IMUList" << std::endl;
for (std::size_t idx = 0; idx < imuObj->size(); ++idx)
{
const auto& meas = imuObj->get(idx);
switch (meas.getSensorID().first)
{
case (slamcore::SensorType::Accelerometer):
{
std::cout << "\tTimestamp: "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
meas.getHWTimestamp().time_since_epoch())
.count();
std::cout << "\tAccelerometer: " << meas.getMeasurement().x() << ","
<< meas.getMeasurement().y() << "," << meas.getMeasurement().z()
<< std::endl;
break;
}
case (slamcore::SensorType::Gyroscope):
{
std::cout << "\tTimestamp: "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
meas.getHWTimestamp().time_since_epoch())
.count();
std::cout << "\tGyroscope: " << meas.getMeasurement().x() << ","
<< meas.getMeasurement().y() << "," << meas.getMeasurement().z()
<< std::endl;
break;
}
default:
std::cerr << "Unknown IMU measurement: " << static_cast<int>(imuObj->type())
<< std::endl;
}
}
});
slam->registerCallback<slamcore::Stream::Velocity>(
[](const slamcore::VelocityInterface<slamcore::camera_clock>::CPtr& velObj) {
std::cout << "Received: Velocities" << std::endl;
std::cout << "\tLinear: " << velObj->getLinear().x() << "," << velObj->getLinear().y()
<< "," << velObj->getLinear().z() << std::endl;
std::cout << "\tAngular: " << velObj->getAngular().x() << ","
<< velObj->getAngular().y() << "," << velObj->getAngular().z() << std::endl;
});
slam->registerCallback<slamcore::Stream::ActiveMap>(
[](const slamcore::SparseMapInterface::CPtr& mapObj) {
std::cout << "Received: SparseMap" << std::endl;
std::cout << "\tLandmarks: " << mapObj->size() << std::endl;
});
slam->registerCallback<slamcore::Stream::MetaData>(
[](const slamcore::MetaDataInterface::CPtr& metaObj) {
std::cout << "Received: MetaData" << std::endl << "\tID: ";
slamcore::MetaDataID ID = metaObj->getID();
if (ID >= slamcore::MetaDataID::Count)
{
std::cout << "Unknown MetaData ID";
}
else
{
std::cout << ID;
}
std::cout << std::endl << "\tValue: ";
switch (metaObj->getValueType())
{
case slamcore::MetaDataInterface::ValueType::Integer:
{
int64_t ival = 0;
metaObj->getValue(ival);
std::cout << ival << std::endl;
}
break;
case slamcore::MetaDataInterface::ValueType::Floating:
{
double dval = 0.0;
metaObj->getValue(dval);
std::cout << dval << std::endl;
}
break;
case slamcore::MetaDataInterface::ValueType::String:
{
std::string sval;
metaObj->getValue(sval);
std::cout << sval << std::endl;
}
break;
default:
{
std::cout << "Unknown data type" << std::endl;
}
break;
}
});
slam->registerCallback<slamcore::Stream::FrameSync>(
[](const slamcore::FrameSyncInterface::CPtr&) {
std::cout << "Received: FrameSync" << std::endl;
});
slam->registerCallback<slamcore::Stream::LocalPointCloud>(
[](const slamcore::PointCloudInterface::CPtr& pointCloudObj) {
std::cout << "Received: LocalPointCloud" << std::endl;
std::cout << "\tNumber of Points: " << pointCloudObj->size() << std::endl;
});
// ******************************************************************
// Start streaming
// ******************************************************************
slam->start();
// ******************************************************************
// Main receiving loop
// ******************************************************************
while (slam->spinOnce())
;
// ******************************************************************
// Stop SLAM
// ******************************************************************
slam->stop();
// ******************************************************************
// 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;
}