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-2025 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;

  struct LocalizationUpdate
  {
    LocalizationUpdate() = default;

    mrpt::Clock::time_point timestamp;

    std::string reference_frame = "map";

    std::string child_frame = "base_link";

    std::string method = "slam";

    mrpt::math::TPose3D                        pose;
    std::optional<mrpt::math::CMatrixDouble66> cov;

    double quality = 1.0;
  };

  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