You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.
Example reset.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 showcase how to "reset" the SLAM system
*
* If the user wishes to reset the SLAM system to start processing a data source
* cleanly, the common practice is to delete the current system and create a new
* one.
*
* Bear in mind that it is a restriction that only one SLAM system can exist at
* any given time. The internal SLAM object will be deleted when the unique
* pointer returned by `createSLAMSystem()` stops managing it, and this can be
* achieved by calling `reset` or assigning it to `nullptr`. Subsequently, a new
* SLAM system could be created.
*/
#include <slamcore/slamcore.hpp>
#include <signal.h>
#include <atomic>
#include <ctime>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>
// Flag variable indicating processing completed or stopped.
static std::atomic<bool> s_finished{false};
// Callback to be executed on SIGINT.
static void handleSignal(int)
{
s_finished.store(true);
}
namespace
{
/**
* Logging callback.
*
* @param message Message received by the logging system.
*/
void logInfo(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";
}
/**
* Callback executed when Pose data is received.
*
* @param poseObj Pointer to the received Pose.
*/
void handlePose(const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& poseObj)
{
std::cout << "Position: " << poseObj->getTranslation().x() << ", "
<< poseObj->getTranslation().y() << ", " << poseObj->getTranslation().z()
<< std::endl;
std::cout << "Rotation: " << poseObj->getRotation().x() << ", "
<< poseObj->getRotation().y() << ", " << poseObj->getTranslation().z() << ", "
<< poseObj->getRotation().w() << std::endl;
}
/**
* Callback executed when an Error is received.
*
* @param errorObj Pointer to the received error.
*/
void handleError(const slamcore::ErrorCodeInterface::CPtr& errorObj)
{
const std::error_code rc = errorObj->getValue();
if (rc == make_error_code(slamcore::errc::end_of_data))
{
s_finished.store(true);
}
}
/**
* Convenience function to encapsulate the SLAM system's setting up and
* processing.
*
* @param slam Interface to the SLAM system.
*/
void process(const std::unique_ptr<slamcore::SLAMSystemCallbackInterface>& slam)
{
// Open the device
slam->open();
// Enable the Pose stream
slam->setStreamEnabled(slamcore::Stream::Pose, true);
// Register callbacks
slam->registerCallback<slamcore::Stream::ErrorCode>(::handleError);
slam->registerCallback<slamcore::Stream::Pose>(::handlePose);
// Start streaming
slam->start();
// Main receiving loop
// You can either let it finish or press Ctrl-C to interrupt it.
while (slam->spinOnce() && !s_finished)
;
// Stop SLAM
std::cout << "Stopping..." << std::endl;
slam->stop();
// Disconnect/Close SLAM
slam->close();
}
} // namespace
int main(int argc, char* argv[])
try
{
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " <dataset-path>" << std::endl;
return -1;
}
// Handle Ctrl-C
signal(SIGINT, handleSignal);
// Initialise SLAMcore API
slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::logInfo);
// Create/Connect SLAM System
slamcore::v0::SystemConfiguration sysCfg;
sysCfg.Source = 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;
}
std::cout << "Starting SLAM..." << std::endl;
process(slam);
// ******************************************************************
// Reset the unique_ptr so the internal SLAM object is deleted
// ******************************************************************
slam.reset();
// Create/Connect new SLAM System
slam = slamcore::createSLAMSystem(sysCfg);
if (!slam)
{
std::cerr << "Error creating SLAM system!" << std::endl;
slamcore::slamcoreDeinit();
return -1;
}
s_finished.store(false);
std::cout << "Starting SLAM again ..." << std::endl;
process(slam);
// Deinitialise SLAMcore API
slamcore::slamcoreDeinit();
std::cout << "We're Done Here." << std::endl;
}
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;
}