Program Listing for File LocalizationSourceBase.h
↰ Return to documentation for file (include/mola_kernel/interfaces/LocalizationSourceBase.h
)
/* -------------------------------------------------------------------------
* A Modular Optimization framework for Localization and mApping (MOLA)
* Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria
* See LICENSE for license information.
* ------------------------------------------------------------------------- */
#pragma once
#include <mrpt/core/Clock.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/TPose3D.h>
#include <functional>
#include <iostream>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
namespace mola
{
class LocalizationSourceBase
{
public:
LocalizationSourceBase() = default;
virtual ~LocalizationSourceBase() = default;
struct LocalizationUpdate
{
LocalizationUpdate() = default;
mrpt::Clock::time_point timestamp;
std::string reference_frame = "map";
std::string method = "slam";
mrpt::math::TPose3D pose;
std::optional<mrpt::math::CMatrixDouble66> cov;
};
using localization_updates_callback_t =
std::function<void(const LocalizationUpdate&)>;
void subscribeToLocalizationUpdates(
const localization_updates_callback_t& callback)
{
auto lck = mrpt::lockHelper(locUpdSubsMtx_);
locUpdSubs_.push_back(callback);
}
protected:
bool anyUpdateLocalizationSubscriber()
{
auto lck = mrpt::lockHelper(locUpdSubsMtx_);
return !locUpdSubs_.empty();
}
void advertiseUpdatedLocalization(const LocalizationUpdate& l)
{
auto lck = mrpt::lockHelper(locUpdSubsMtx_);
for (const auto& callback : locUpdSubs_)
{
try
{
callback(l);
}
catch (const std::exception& e)
{
std::cerr << "[LocalizationSourceBase] Exception in callback: "
<< e.what();
}
}
}
private:
std::vector<localization_updates_callback_t> locUpdSubs_;
std::mutex locUpdSubsMtx_;
};
} // namespace mola