diff --git a/mola_kernel/include/mola_kernel/interfaces/FrontEndBase.h b/mola_kernel/include/mola_kernel/interfaces/FrontEndBase.h index b420a854..6a2e8117 100644 --- a/mola_kernel/include/mola_kernel/interfaces/FrontEndBase.h +++ b/mola_kernel/include/mola_kernel/interfaces/FrontEndBase.h @@ -31,7 +31,7 @@ namespace mola * - Use your own logic to enqueue a task into a worker thread pool (preferred). * * \ingroup mola_kernel_grp */ -class FrontEndBase : public ExecutableBase, RawDataConsumer +class FrontEndBase : public ExecutableBase, public RawDataConsumer { DEFINE_VIRTUAL_MRPT_OBJECT(FrontEndBase) diff --git a/mola_kernel/include/mola_kernel/interfaces/LocalizationSourceBase.h b/mola_kernel/include/mola_kernel/interfaces/LocalizationSourceBase.h new file mode 100644 index 00000000..e2eccf8e --- /dev/null +++ b/mola_kernel/include/mola_kernel/interfaces/LocalizationSourceBase.h @@ -0,0 +1,92 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file LocalizationSourceBase.h + * @brief Virtual interface for SLAM/odometry methods publishing poses + * @author Jose Luis Blanco Claraco + * @date Mar 13, 2024 + */ +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace mola +{ +/** Virtual interface for SLAM/odometry methods publishing poses. + * + * Publishers must call advertiseUpdatedLocalization(), subscribers + * must call subscribeToLocalizationUpdates() providing a callback that returns + * as fast as possible. + * + * \ingroup mola_kernel_grp */ +class LocalizationSourceBase +{ + public: + LocalizationSourceBase() = default; + virtual ~LocalizationSourceBase() = default; + + struct LocalizationUpdate + { + LocalizationUpdate() = default; + + /** The timestamp associated to the new KeyFrame localization. */ + mrpt::Clock::time_point timestamp; + + /** Vehicle/robot pose is given wrt this frame of reference */ + std::string reference_frame = "map"; + + /** The source of the localization (e.g. "slam", "lidar_odometry", + * "wheel_odometry", etc.) */ + std::string method = "slam"; + + mrpt::math::TPose3D pose; + std::optional cov; + }; + + using localization_updates_callback_t = + std::function; + + void subscribeToLocalizationUpdates( + const localization_updates_callback_t& callback) + { + auto lck = mrpt::lockHelper(locUpdSubsMtx_); + locUpdSubs_.push_back(callback); + } + + protected: + 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 locUpdSubs_; + std::mutex locUpdSubsMtx_; +}; + +} // namespace mola diff --git a/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h b/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h new file mode 100644 index 00000000..7d883378 --- /dev/null +++ b/mola_kernel/include/mola_kernel/interfaces/MapSourceBase.h @@ -0,0 +1,84 @@ +/* ------------------------------------------------------------------------- + * A Modular Optimization framework for Localization and mApping (MOLA) + * Copyright (C) 2018-2024 Jose Luis Blanco, University of Almeria + * See LICENSE for license information. + * ------------------------------------------------------------------------- */ +/** + * @file MapSourceBase.h + * @brief Virtual interface for SLAM/odometry methods publishing a map + * @author Jose Luis Blanco Claraco + * @date Mar 13, 2024 + */ +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace mola +{ +/** Virtual interface for SLAM/odometry methods publishing a map + * + * \ingroup mola_kernel_grp */ +class MapSourceBase +{ + public: + MapSourceBase() = default; + virtual ~MapSourceBase() = default; + + struct MapUpdate + { + MapUpdate() = default; + + /** The timestamp associated to the new map information. */ + mrpt::Clock::time_point timestamp; + + /** Frame of reference for this map. */ + std::string reference_frame = "map"; + + /** The source of the localization (e.g. "slam", "lidar_odometry", + * "wheel_odometry", etc.) */ + std::string method = "slam"; + + /** Map layer/submap name */ + std::string map_name = "local_map"; + + mrpt::maps::CMetricMap::Ptr map; + }; + + using map_updates_callback_t = std::function; + + void subscribeToMapUpdates(const map_updates_callback_t& callback) + { + auto lck = mrpt::lockHelper(mapUpdSubsMtx_); + mapUpdSubs_.push_back(callback); + } + + protected: + void advertiseUpdatedMap(const MapUpdate& l) + { + auto lck = mrpt::lockHelper(mapUpdSubsMtx_); + for (const auto& callback : mapUpdSubs_) + { + try + { + callback(l); + } + catch (const std::exception& e) + { + std::cerr << "[MapSourceBase] Exception in callback: " + << e.what(); + } + } + } + + private: + std::vector mapUpdSubs_; + std::mutex mapUpdSubsMtx_; +}; + +} // namespace mola