Component.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <algorithm>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <ios>
#include <memory>
#include <mutex>
#include <optional>
#include <sstream>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <Ice/Current.h>
 
#include <nlohmann/json.hpp>
#include <nlohmann/json_fwd.hpp>
 
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 
#include "ArmarXCore/core/PackagePath.h"
#include "ArmarXCore/core/services/tasks/TaskUtil.h"
#include "ArmarXCore/core/time/Clock.h"
#include "ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h"
#include "ArmarXCore/libraries/logging/FrequencyReporter.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/interface/observers/Timestamp.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/interface/core/GeometryBase.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
#include <RobotAPI/libraries/armem_vision/types.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include "armarx/localization_and_mapping/cartographer_adapter/ArVizDrawer.h"
#include "armarx/localization_and_mapping/cartographer_adapter/ArVizDrawerMapBuilder.h"
#include <armarx/localization_and_mapping/cartographer_adapter/CartographerAdapter.h>
#include <armarx/localization_and_mapping/cartographer_adapter/types.h>
#include <armarx/localization_and_mapping/cartographer_adapter/utils.h>
#include <armarx/localization_and_mapping/components/cartographer_mapping_and_localization/LocalizationRemoteGui.h>
#include <armarx/localization_and_mapping/components/cartographer_mapping_and_localization/MappingRemoteGui.h>
#include <armarx/localization_and_mapping/self_localization/SelfLocalization.h>
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/algorithms/types.h>
#include <armarx/navigation/conversions/eigen.h>
 
namespace fs = std::filesystem;
 
namespace
{
    inline Eigen::Isometry3f
    toM(Eigen::Isometry3f pose)
    {
        pose.translation() /= 1000;
        return pose;
    }
 
    inline Eigen::Isometry3f
    toMM(Eigen::Isometry3f pose)
    {
        pose.translation() *= 1000;
        return pose;
    }
 
} // namespace
 
namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping
{
    // forward declarations for file-local helpers
    std::string toString(Component::Mode mode);
    std::string toString(Component::ConfigProfile profile);
    void dumpSubMapsToDisk(const cartographer_adapter::SubMapDataVector& submapData);
 
    std::string
    toString(Component::Mode mode)
    {
        std::string modeStr;
 
        switch (mode)
        {
            case Component::Mode::Localization:
                modeStr = "localization";
                break;
            case Component::Mode::Mapping:
                modeStr = "mapping";
                break;
            default:
                break;
        }
 
        return modeStr;
    }
 
    std::string
    toString(Component::ConfigProfile profile)
    {
        std::string modeStr;
 
        switch (profile)
        {
            case Component::ConfigProfile::Default:
                modeStr = "Default";
                break;
            case Component::ConfigProfile::RobotSpecific:
                modeStr = "RobotSpecific";
                break;
            default:
                break;
        }
 
        return modeStr;
    }
 
    Component::Component()
    {
        addPlugin(heartbeatPlugin);
        // addPlugin(occupancyGridWriterPlugin);
        addPlugin(costmapWriterPlugin);
        addPlugin(robotReaderPlugin);
 
        setTag(getName());
    }
 
    Component::~Component() = default;
 
    std::string
    Component::getDefaultName() const
    {
        return GetDefaultName();
    }
 
    std::string
    Component::GetDefaultName()
    {
        return "CartographerMappingAndLocalization";
    }
 
    armarx::PropertyDefinitionsPtr
    Component::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            self_localization::SelfLocalization::createPropertyDefinitions();
 
        // topic publisher
        def->topic(debugObserver);
 
        // topic subscriber
        // def->topic<LaserScannerUnitListener>(
        //    "LaserScans", "laser_scanner_topic", "Name of the laser scanner topic.");
 
        //        def->topic<PlatformUnitListener>(
        //            "PlatformState",
        //            "platform_state",
        //            "Name of the platform + state to use. This property is used to listen to "
        //            "the platform topic");
 
        // def->topic<OdometryListener>();
 
        def->component(remoteGui,
                       "RemoteGuiProvider",
                       "remote_gui_provider",
                       "Name of the remote gui provider");
 
 
        // mapPath package path
        def->optional(properties.mapPath.package,
                      "map_path.package",
                      "Path where the map will be created (within a unique subdirectory)");
        def->optional(properties.mapPath.path,
                      "map_path.path",
                      "Path where the map will be created (within a unique subdirectory)");
 
        def->optional(properties.mapStorageSubDir,
                      "map_mapStorageSubDir",
                      "Only relevant in mapping mode. Path relative to `properties.mapPath` where "
                      "the generated map should be stored.");
 
        // mapToLoad package path
        def->optional(properties.mapToLoad.package,
                      "map_to_load.package",
                      "Name of a map, e.g. 2021-02-15-17-17.carto. Load this map from "
                      "within the 'map_path'");
        def->optional(properties.mapToLoad.path,
                      "map_to_load.path",
                      "Name of a map, e.g. 2021-02-15-17-17.carto. Load this map from "
                      "within the 'map_path'");
 
        // cartographerConfigPath package path
        def->optional(properties.cartographerConfigPath.package,
                      "config_path.package",
                      "Path to the lua config files");
        def->optional(properties.cartographerConfigPath.path,
                      "config_path.path",
                      "Path to the lua config files");
 
        def->optional(properties.mode,
                      "mode",
                      "Either run the component in mapping or localization optimized mode")
            .map(toString(Mode::Localization), Mode::Localization)
            .map(toString(Mode::Mapping), Mode::Mapping);
 
        def->optional(properties.profile,
                      "profile",
                      "Either default or robot specific. Will load config from directory based on "
                      "this parameter.")
            .map(toString(ConfigProfile::Default), ConfigProfile::Default)
            .map(toString(ConfigProfile::RobotSpecific), ConfigProfile::RobotSpecific);
 
        def->optional(properties.enableVisualization,
                      "enable_arviz",
                      "If enabled, ArViz layers are created.");
 
        def->optional(properties.enableMappingVisualization,
                      "enable_arviz_mapping",
                      "If enabled, ArViz layers are created.");
 
        def->optional(adapterConfig.useOdometry, "use_odometry");
        def->optional(adapterConfig.useLaserScanner, "use_laserscanner");
        def->optional(adapterConfig.useImu, "use_imu");
        def->optional(adapterConfig.frequency,
                      "frequency",
                      "The rate (in Hz) at which the cartographer processes data");
        def->optional(
            adapterConfig.enableDebugTiming,
            "enableCartographerAdapterTiming",
            "If true, the cartographer adapter will publish timing information to the debug topic");
        def->optional(
            adapterConfig.useOdometryBasedLaserCorrection, "useOdometryBasedLaserCorrection", "");
        def->optional(adapterConfig.global_localization_min_score, "global_localization_min_score");
        def->optional(adapterConfig.min_score, "min_score");
 
        def->optional(properties.laserScanners,
                      "laser_scanners",
                      "Set of laser scanners to use. Comma separated.");
 
        def->optional(
            properties.useNativeOdometryTimestamps,
            "useNativeOdometryTimestamps",
            "if disabled, will assume that odometry is always available (this is a bit hacky)");
 
        def->optional(properties.occupancyGridMemoryUpdatePeriodMs,
                      "occupancyGridMemoryUpdatePeriodMs",
                      "The frequency to store the occupancy grid in the memory.");
        def->optional(properties.odomQueryPeriodMs,
                      "odomQueryPeriodMs",
                      "The polling frequency to retrieve odometry data from the memory.");
 
        def->optional(properties.obstacleRadius, "obstacleRadius");
        def->optional(properties.occupiedThreshold, "occupiedThreshold");
 
 
        return def;
    }
 
    void
    Component::setupMappingAdapter()
    {
        ARMARX_TRACE;
 
        if (mappingAdapter != nullptr)
        {
            ARMARX_INFO << "Mapping adapter already intialized. Skipping setup.";
            return;
        }
 
        const std::string modeSubfolder = toString(properties.mode);
        const std::string profileSubfolder = [&]() -> std::string
        {
            std::string dir;
            switch (properties.profile)
            {
                case ConfigProfile::Default:
                    ARMARX_INFO << "Using `default` profile";
                    dir = "default";
                    break;
                case ConfigProfile::RobotSpecific:
                    ARMARX_INFO << "Using robot specific profile: `" << getRobotName() << "`.";
                    dir = getRobotName();
                    break;
                default:
                    break;
            }
 
            ARMARX_CHECK_NOT_EMPTY(dir);
            return dir;
        }();
 
        const fs::path mapPath = armarx::PackagePath(properties.mapPath).toSystemPath();
 
        ARMARX_INFO << "Map will be stored in " << mapPath.string();
        fs::create_directories(mapPath);
 
        const auto configPath =
            armarx::PackagePath(properties.cartographerConfigPath).toSystemPath() / modeSubfolder /
            profileSubfolder;
        ARMARX_IMPORTANT << "Using config files from " << configPath;
        ARMARX_CHECK(std::filesystem::exists(configPath))
            << "The config path `" << configPath
            << "` does not exist. If you use a robot-specific config profile (see property "
               "`profile`) then this folder must exist.";
 
 
        switch (properties.mode)
        {
            case Mode::Mapping:
            {
                // this is the interface to cartographer
                mappingAdapter =
                    std::make_unique<cartographer_adapter::CartographerAdapter>(mapPath,
                                                                                configPath,
                                                                                *this,
                                                                                adapterConfig,
                                                                                std::nullopt,
                                                                                std::nullopt,
                                                                                debugObserver);
                break;
            }
            case Mode::Localization:
            {
                // load the map in localization mode
                const fs::path mapToLoad = armarx::PackagePath(properties.mapToLoad).toSystemPath();
 
                ARMARX_INFO << "Trying to load map from " << mapToLoad;
 
                ARMARX_CHECK(fs::is_regular_file(mapToLoad))
                    << "In localization mode, a valid map has to be provided.";
 
 
                std::optional<Eigen::Isometry3f> world_T_robot_prior = globalPose(); // [mm]
                if (not world_T_robot_prior.has_value())
                {
                    world_T_robot_prior = globalPoseFromLongTermMemory(); // [mm]
                }
 
                const auto map_T_robot_prior = [&]() -> std::optional<Eigen::Isometry3f> // [m] !!
                {
                    if (not world_T_robot_prior.has_value())
                    {
                        return std::nullopt;
                    }
 
                    return toM(world_T_map.inverse() * world_T_robot_prior.value());
                }();
 
                // this is the interface to cartographer
                mappingAdapter =
                    std::make_unique<cartographer_adapter::CartographerAdapter>(mapPath,
                                                                                configPath,
                                                                                *this,
                                                                                adapterConfig,
                                                                                mapToLoad,
                                                                                map_T_robot_prior,
                                                                                debugObserver);
 
                break;
            }
            default:
                throw std::invalid_argument("Unknown mode");
        }
 
        // hook up the input queues
        laserMessageQueue.connect(&cartographer_adapter::CartographerAdapter::processSensorValues,
                                  mappingAdapter.get());
        odomMessageQueue.connect(&cartographer_adapter::CartographerAdapter::processOdometryPose,
                                 mappingAdapter.get());
 
        auto sensorPose = [this](const std::string& sensorFrame) -> Eigen::Isometry3f
        {
            Eigen::Isometry3f robot_T_sensor = Eigen::Isometry3f::Identity();
            robot_T_sensor.matrix() = getRobot()->getRobotNode(sensorFrame)->getPoseInRootFrame();
            robot_T_sensor.translation() /= 1000.; // to [m]
 
            ARMARX_DEBUG << "Sensor pose for sensor " << sensorFrame << " is " << '\n'
                         << robot_T_sensor.matrix();
 
            return robot_T_sensor;
        };
 
        // set sensor poses
        const auto sensorIds = mappingAdapter->laserSensorIds();
        std::for_each(sensorIds.begin(),
                      sensorIds.end(),
                      [&](const std::string& sensorId)
                      { mappingAdapter->setSensorPose(sensorId, sensorPose(sensorId)); });
    }
 
    std::optional<Eigen::Isometry3f>
    Component::globalPose() const
    {
        if (not map_T_robot.has_value())
        {
            return std::nullopt;
        }
 
        return world_T_map * map_T_robot.value();
    }
 
    // Lifecycle management methods
 
    void
    Component::onInitComponent()
    {
        const auto laserScanners = simox::alg::split(properties.laserScanners, ",");
        ARMARX_INFO << "Using laser scanners: " << laserScanners;
        adapterConfig.laserScanners.insert(laserScanners.begin(), laserScanners.end());
 
        SelfLocalization::onInitComponent();
    }
 
    Eigen::Isometry3f
    Component::worldToMapTransform() const
    {
        ARMARX_TRACE;
 
        if (properties.mode == Mode::Mapping)
        {
            ARMARX_IMPORTANT << "Mapping mode. World to map transform set to identity";
            return Eigen::Isometry3f::Identity();
        }
 
        // store data alongside the map file
        const fs::path mapPath = armarx::PackagePath(properties.mapPath).toSystemPath();
 
        fs::path jsonFilename = armarx::PackagePath(properties.mapToLoad).toSystemPath();
        jsonFilename.replace_extension("json");
        const fs::path jsonLoadPath = mapPath / jsonFilename;
 
        ARMARX_CHECK(fs::is_regular_file(jsonLoadPath))
            << "In mapping mode, the json registration file " << jsonLoadPath << " has to exist!";
 
        ARMARX_IMPORTANT << "Loading registration result: " << jsonLoadPath.string();
 
        if (not fs::is_regular_file(jsonLoadPath))
        {
            return Eigen::Isometry3f::Identity();
        }
 
        std::ifstream ifs(jsonLoadPath.string(), std::ios::in);
        const auto j = nlohmann::json::parse(ifs);
 
        ARMARX_CHECK(ifs.is_open());
        ARMARX_CHECK(not ifs.fail());
 
        Eigen::Isometry3f world_T_map = Eigen::Isometry3f::Identity();
        world_T_map.translation().x() = j.at("x");
        world_T_map.translation().y() = j.at("y");
        world_T_map.linear() =
            Eigen::AngleAxisf(j.at("yaw"), Eigen::Vector3f::UnitZ()).toRotationMatrix();
 
        ARMARX_IMPORTANT << "Loading registration result is " << world_T_map.matrix();
 
        return world_T_map;
    }
 
    void
    Component::onConnectComponent()
    {
 
        // const bool reconnect = mappingAdapter != nullptr;
        // if (reconnect)
        // {
        //     onReconnectComponent();
        //     return;
        // }
 
        ARMARX_INFO << "SelfLocalization::onConnectComponent";
        ARMARX_TRACE;
        SelfLocalization::onConnectComponent();
 
        slamResultReporter.emplace(debugObserver, "CartographerMappingAndLocalization_slam_result");
        laserDataReporter.emplace(debugObserver, "CartographerMappingAndLocalization_laser_data");
        odomDataReporter.emplace(debugObserver, "CartographerMappingAndLocalization_odom_data");
 
        world_T_map = worldToMapTransform();
 
        ARMARX_TRACE;
        setupMappingAdapter();
        ARMARX_CHECK_NOT_NULL(mappingAdapter);
 
        // Only in the mapping mode, allow the user to create a map.
        // Although this would be possible in localization mode as well, this might
        // be confusing and likely results in suboptimal maps. It's therefore prefered to record
        // the data using the TopicRecorder such that you can create the map afterwards.
        switch (properties.mode)
        {
            case Mode::Mapping:
 
            {
                const MappingRemoteGui::Params mappingRemoteGuiParams{
                    .remoteGuiTabName = getName(),
                    .mapStorageDir = armarx::PackagePath(properties.mapPath.package,
                                                         properties.mapPath.path + "/" +
                                                             properties.mapStorageSubDir)};
 
                mappingRemoteGui =
                    std::make_unique<MappingRemoteGui>(remoteGui, *this, mappingRemoteGuiParams);
                break;
            }
            case Mode::Localization:
                localizationRemoteGui =
                    std::make_unique<LocalizationRemoteGui>(remoteGui, *this, world_T_map);
                break;
            default:
                break;
        }
 
 
        if (properties.enableVisualization)
        {
            arvizDrawer =
                std::make_unique<cartographer_adapter::ArVizDrawer>(getArvizClient(), world_T_map);
 
            // we use the map visu also to visualize the recorded / known map
            arVizDrawerMapBuilder = std::make_unique<cartographer_adapter::ArVizDrawerMapBuilder>(
                getArvizClient(), mappingAdapter->getMapBuilder(), world_T_map);
 
            arVizDrawerMapBuilder->drawOnce();
 
            if (properties.enableMappingVisualization and properties.mode == Mode::Mapping)
            {
                arVizDrawerMapBuilder->startMappingVisu();
            }
        }
 
 
        const auto storeOccupancyGridAndCostmapInMemory = [&]()
        {
            ARMARX_INFO << "Obtaining grids";
            const auto grids =
                armarx::localization_and_mapping::cartographer_adapter::utils::occupancyGrids(
                    mappingAdapter->getMapBuilder(), 0);
 
            if (grids.empty())
            {
                ARMARX_INFO << "No grids.";
                return;
            }
 
            armem::vision::OccupancyGrid grid = grids.front();
            grid.pose.translation() *= 1'000; // [m] to [mm]
            grid.resolution *= 1'000; // [m] to [mm]
 
            ARMARX_INFO << VAROUT(world_T_map.matrix());
 
            ARMARX_CHECK_EQUAL(grid.frame, MapFrame) << "Assuming map frame";
            grid.pose = world_T_map * grid.pose; // map to global frame
            grid.frame = GlobalFrame;
 
            ARMARX_INFO << "Storing occupancy grid in memory";
            // TODO latest timestamp
            // occupancyGridWriterPlugin->get().store(
            //     grid, MapFrame, getName(), IceUtil::Time::now().toMicroSeconds());
 
            // grid as costmap
 
            OccupancyGridHelper::Params helperParams{
                .freespaceThreshold = 0.45F, .occupiedThreshold = properties.occupiedThreshold};
 
            OccupancyGridHelper helper(grid, helperParams);
 
            const std::size_t nKnownCells = helper.knownCells().cast<int>().sum();
            const std::size_t nFreeCells = helper.freespace().cast<int>().sum();
            const std::size_t nOccupied = helper.obstacles().cast<int>().sum();
 
            ARMARX_INFO << VAROUT(nKnownCells) << VAROUT(nFreeCells) << VAROUT(nOccupied);
 
            // freespace as 0:occupied, 1:free
            // const OccupancyGridHelper::BinaryArray freespace = helper.freespace();
 
            // alternative: treat everything that is not an obstacle as free
            OccupancyGridHelper::BinaryArray freespace = helper.obstacles() == false;
            freespace = helper.knownCells() and freespace;
 
            ARMARX_INFO << VAROUT(freespace.cast<int>().sum());
 
            const Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> freespaceCvFormat =
                freespace.cast<std::uint8_t>() * 255;
 
            cv::Mat1b freespaceMat;
            cv::eigen2cv(freespaceCvFormat, freespaceMat);
 
            cv::Mat1f distanceMat(freespaceMat.size());
            cv::distanceTransform(freespaceMat, distanceMat, cv::DIST_L2, cv::DIST_MASK_PRECISE);
 
            // The distance is in pixel units, we need to convert it to metric units
            distanceMat *= grid.resolution;
 
            const navigation::algorithms::Costmap::Parameters params{
                .binaryGrid = false, .cellSize = grid.resolution, .sceneBoundsMargin = 0};
 
            const navigation::algorithms::SceneBounds sceneBounds{
                .min = Eigen::Vector2f::Zero(),
                // FIXME check rows / cols order
                .max = Eigen::Vector2f{grid.grid.rows(), grid.grid.cols()} * grid.resolution};
 
            navigation::algorithms::Costmap::Grid cGrid;
            cv::cv2eigen(distanceMat, cGrid);
 
            // store without considering robot radius
            {
                const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> reducedFreespace =
                    cGrid.array() > 0;
 
                const navigation::algorithms::Costmap::Mask mask = reducedFreespace.matrix();
 
                navigation::algorithms::Costmap costmap(
                    cGrid, params, sceneBounds, mask, navigation::conv::to2D(grid.pose));
 
                ARMARX_INFO << VAROUT(grid.pose.matrix());
 
                ARMARX_INFO << "Storing costmap in memory";
                costmapWriterPlugin->get().store(
                    costmap, "distance_to_obstacles_raw", getName(), Clock::Now());
            }
 
            // store with considering robot radius
            {
                // we substract the obstacle radius which also has an influence on the available freespace
                cGrid.array() -= properties.obstacleRadius;
 
                const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> reducedFreespace =
                    cGrid.array() > 0;
 
                const navigation::algorithms::Costmap::Mask mask = reducedFreespace.matrix();
 
                navigation::algorithms::Costmap costmap(
                    cGrid, params, sceneBounds, mask, navigation::conv::to2D(grid.pose));
 
                ARMARX_INFO << VAROUT(grid.pose.matrix());
 
                ARMARX_INFO << "Storing costmap in memory";
                costmapWriterPlugin->get().store(
                    costmap, "distance_to_obstacles", getName(), Clock::Now());
            }
        };
 
        // store once on startup
        storeOccupancyGridAndCostmapInMemory();
 
        // occupancyGridTask = new SimplePeriodicTask<>(storeOccupancyGridAndCostmapInMemory,
        //  properties.occupancyGridMemoryUpdatePeriodMs);
        // occupancyGridTask->start();
 
        laserMessageQueue.enable();
        odomMessageQueue.enable();
 
        // wait a bit such that all threads are initialized
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
 
        receivingData.store(true);
        ARMARX_INFO << "Will now process incoming messages.";
 
 
        // ARMARX_INFO << "Enabling robot unit streaming";
        // robotUnitReader = std::make_unique<components::cartographer_localization_and_mapping::RobotUnitReader>(*robotUnit, odomMessageQueue, *odomDataReporter);
 
        if (odometryQueryTask)
        {
            odometryQueryTask->stop();
            odometryQueryTask = nullptr;
        }
 
        odometryQueryTask = new SimplePeriodicTask<>(
            [&]()
            {
                const auto timestamp = armarx::Clock::Now();
 
                ARMARX_TRACE;
                if (not receivingData.load())
                {
                    ARMARX_INFO << deactivateSpam(1) << "Receiving data is deactivated";
                    return;
                }
 
                ARMARX_CHECK_NOT_NULL(robotReaderPlugin);
                if (auto odomPose =
                        robotReaderPlugin->get().queryOdometryPose(getRobotName(), timestamp))
                {
 
                    if (properties.useNativeOdometryTimestamps)
                    {
                        // did we retrieve the same data as last time?
                        if (lastTimestamp >= odomPose->header.timestamp.toMicroSecondsSinceEpoch())
                        {
                            return;
                        }
 
                        lastTimestamp = odomPose->header.timestamp.toMicroSecondsSinceEpoch();
                    }
                    else
                    {
                        lastTimestamp = timestamp.toMicroSecondsSinceEpoch();
                    }
 
 
                    cartographer_adapter::CartographerAdapter::OdomData odomData{
                        .position = odomPose->transform.translation() / 1000, // mm -> m
                        .orientation = Eigen::Quaternionf(odomPose->transform.linear()),
                        .timestamp = lastTimestamp};
 
                    // ARMARX_INFO << deactivateSpam(1) << "received odom data from memory";
                    odomMessageQueue.push(odomData);
 
                    if (odomDataReporter)
                    {
                        odomDataReporter->add(odomData.timestamp);
                    }
                }
                else
                {
                    ARMARX_WARNING << deactivateSpam(10) << "Failed to query odom pose";
                }
            },
            properties.odomQueryPeriodMs);
 
        odometryQueryTask->start();
 
        heartbeatPlugin->signUp(armarx::Duration::MilliSeconds(500),
                                armarx::Duration::MilliSeconds(1000),
                                {"Localization"},
                                "CartographerMappingAndLocalization");
 
        ARMARX_DEBUG << "onConnectComponent completed";
    }
 
    void
    Component::onReconnectComponent()
    {
        ARMARX_INFO << "CartographerMappingAndLocalization::reconnecting ...";
        SelfLocalization::onReconnectComponent();
 
        if (localizationRemoteGui)
        {
            localizationRemoteGui->enable();
        }
 
        if (mappingRemoteGui)
        {
            mappingRemoteGui->enable();
        }
 
        slamResultReporter.emplace(debugObserver, "slam_result");
        laserDataReporter.emplace(debugObserver, "laser_data");
        odomDataReporter.emplace(debugObserver, "odom_data");
 
 
        if (arvizDrawer)
        {
            ARMARX_TRACE;
            arvizDrawer->setWorldToMapTransform(world_T_map);
        }
 
        laserMessageQueue.enable();
        odomMessageQueue.enable();
 
        receivingData.store(true);
        ARMARX_INFO << "Will now process incoming messages.";
        ARMARX_DEBUG << "onReconnectComponent completed";
    }
 
    void
    Component::onDisconnectComponent()
    {
        ARMARX_TRACE;
 
        ARMARX_WARNING << "Disconnecting ...";
 
        receivingData.store(false);
 
        if (localizationRemoteGui)
        {
            localizationRemoteGui->disable();
        }
 
        if (mappingRemoteGui)
        {
            mappingRemoteGui->disable();
        }
 
        {
            if (odometryQueryTask and odometryQueryTask->isRunning())
            {
                odometryQueryTask->stop();
            }
            odometryQueryTask = nullptr;
        }
 
        laserMessageQueue.clear();
        odomMessageQueue.clear();
 
        laserMessageQueue.waitUntilProcessed();
        odomMessageQueue.waitUntilProcessed();
 
 
        slamResultReporter.reset();
        laserDataReporter.reset();
        odomDataReporter.reset();
 
        SelfLocalization::onDisconnectComponent();
    }
 
    void
    Component::onExitComponent()
    {
        SelfLocalization::onExitComponent();
 
        ARMARX_DEBUG << "Stopping task";
 
        // this causes remote guis and arviz layers to be cleaned up
        if (mappingRemoteGui)
        {
            mappingRemoteGui->shutdown();
        }
 
        if (localizationRemoteGui)
        {
            localizationRemoteGui->shutdown();
        }
 
        if (arvizDrawer)
        {
            arvizDrawer.reset();
        }
    }
 
    void
    Component::onLocalSlamData(const cartographer_adapter::LocalSlamData& slamData)
    {
        // publish localization
        ARMARX_DEBUG << "Received SLAM result";
 
        map_T_robot = toMM(slamData.global_pose());
        publishSelfLocalization(
            {.pose = map_T_robot.value(),
             .timestamp = armarx::DateTime(armarx::Duration::MicroSeconds(slamData.timestamp))});
 
 
        heartbeatPlugin->heartbeat();
 
        // publish visualization stuff
        if (arvizDrawer != nullptr && throttlerArviz.check(slamData.timestamp))
        {
            ARMARX_DEBUG << "arvizDrawer->onLocalSlamData";
            arvizDrawer->onLocalSlamData(slamData);
        }
 
        if (slamResultReporter)
        {
            slamResultReporter->add(slamData.timestamp);
        }
    }
 
    void
    dumpSubMapsToDisk(const cartographer_adapter::SubMapDataVector& submapData)
    {
        static int i{0};
 
        for (const auto& sd : submapData)
        {
            cv::Mat2b img;
            cv::flip(sd.submap, img, 1); // horizontally
 
            std::vector<cv::Mat> channels(img.channels());
            cv::split(img, channels);
 
            // save the submap (occupancy grid) ...
            std::stringstream ss;
            ss << "/tmp/cartographer_mapping/submap_";
            ss << std::setw(10) << std::setfill('0') << ++i;
            ss << ".png";
            std::string s = ss.str();
 
            cv::imwrite(s, channels.at(0));
 
            // ... and its mask
            std::stringstream ssm;
            ssm << "/tmp/cartographer_mapping/submap_mask_";
            ssm << std::setw(10) << std::setfill('0') << i;
            ssm << ".png";
            std::string sm = ssm.str();
 
            cv::imwrite(sm, channels.at(1));
        }
    }
 
    void
    Component::onLaserSensorData(const cartographer_adapter::LaserScannerData& laserData)
    {
        if (arvizDrawer != nullptr)
        {
            arvizDrawer->onLaserSensorData(laserData);
        }
    }
 
    void
    Component::onCreateMapButtonClicked(const RemoteGuiCallee::ButtonClickContext& ctx)
    {
        ARMARX_TRACE;
 
        ARMARX_CHECK_NOT_NULL(mappingRemoteGui);
        ARMARX_CHECK_NOT_NULL(mappingAdapter);
 
        // check if sensor data has been received
        if (not mappingAdapter->hasReceivedSensorData())
        {
            ARMARX_WARNING << "No sensor data received yet. Will not create map.";
            return;
        }
 
        ARMARX_INFO << "Map button clicked which triggers map creation.";
 
        // ensure that only one map can be created at a time
        // mappingRemoteGui->disable();
 
        receivingData.store(false);
 
        ARMARX_INFO << "Waiting until all data is processed.";
        laserMessageQueue.waitUntilProcessed();
        odomMessageQueue.waitUntilProcessed();
 
        ARMARX_INFO << "Creating map.";
        mappingAdapter->createMap(ctx.mapStorageDir);
        //        mappingRemoteGui->enable();
    }
 
    void
    Component::reportSensorValues(const ::std::string&,
                                  const ::std::string& name,
                                  const ::armarx::LaserScan& scan,
                                  const ::armarx::TimestampBasePtr& timestamp,
                                  const ::Ice::Current&)
    {
        ARMARX_TRACE;
 
        ARMARX_VERBOSE << name << " points " << scan.size();
 
        const armarx::DateTime timestampReference =
            armarx::Duration::MilliSeconds(timestamp->timestamp);
        const armarx::DateTime timestampArrival = armarx::Clock::Now();
 
        const armarx::Duration timediff = timestampArrival - timestampReference;
        if (timediff.toMilliSeconds() > 100)
        {
            ARMARX_WARNING << "There is a significant delay (receiving laserscanner data) of "
                           << timediff.toMilliSeconds() << "ms from sensor " << name;
        }
 
        std::lock_guard g{inputMtx};
 
        if (not receivingData.load())
        {
            return;
        }
 
        // check if this is a laser scanner not activated.
        if (adapterConfig.laserScanners.count(name) == 0)
        {
            return;
        }
 
        laserMessageQueue.push(
            cartographer_adapter::LaserScannerMessage{name, scan, timestamp->timestamp});
        ARMARX_DEBUG << "Inserted laser scanner data into laserMessageQueue";
 
        if (laserDataReporter)
        {
            laserDataReporter->add(timestamp->timestamp);
        }
 
        if (properties.mode == Mode::Mapping)
        {
            // if (throttlerLaserScansMemoryWriter.check(timestamp->timestamp) and
            //     laserScansMemoryWriter != nullptr)
            // {
            //     laserScansMemoryWriter->storeSensorData(
            //         scan, name, getAgentName(), timestamp->timestamp);
            // }
        }
    }
 
    namespace util
    {
        armem::robot_state::localization::Transform toTransform(const TransformStamped& transform);
 
        armem::robot_state::localization::Transform
        toTransform(const TransformStamped& transform)
        {
            return {.header = {.parentFrame = transform.header.parentFrame,
                               .frame = transform.header.frame,
                               .agent = transform.header.agent,
                               .timestamp = armarx::Duration::MicroSeconds(
                                   transform.header.timestampInMicroSeconds)},
                    .transform = Eigen::Isometry3f(transform.transform)};
        }
 
    } // namespace util
 
    //    void CartographerMappingAndLocalization::reportOdometryPose(
    //        const TransformStamped& odometryPose, const Ice::Current&)
    //    {
    //        std::lock_guard g{inputMtx};
 
    //        ARMARX_TRACE;
    //        if (not receivingData.load())
    //        {
    //            return;
    //        }
 
    //        Eigen::Isometry3f odomPose(odometryPose.transform);
 
    //        constexpr float mmToM = 1 / 1000.;
 
    //        CartographerAdapter::OdomData odomData;
    //        odomData.position = odomPose.translation() * mmToM;
    //        odomData.orientation = Eigen::Quaternionf(odomPose.linear());
    //        odomData.timestamp = odometryPose.header.timestampInMicroSeconds;
 
    //        ARMARX_TRACE;
    //        odomMessageQueue.push(odomData);
    //        ARMARX_DEBUG << "Inserted odom data into odomMessageQueue";
 
    //        Eigen::Isometry3f odomVisuPose(odometryPose.transform);
    //        odomVisuPose.translation() /= 1000.;
 
    //        if (arvizDrawer)
    //        {
    //            arvizDrawer->onOdomPose(odomVisuPose);
    //        }
 
    //        if (odomDataReporter)
    //        {
    //            odomDataReporter->add(odomData.timestamp);
    //        }
 
    //        // TODO(fabian.reister): this should be published by robot state component
    //        // getTransformWriter().commitTransform(util::toTransform(odometryPose));
    //    }
 
    void
    Component::onWorldToMapTransformUpdate(const Eigen::Isometry3f& world_T_map)
    {
        if (not receivingData.load())
        {
            ARMARX_INFO << "onWorldToMapTransformUpdate() requested but component is deactivated. "
                           "Will not process request.";
            return;
        }
 
        this->world_T_map = world_T_map;
        updateWorldToMapTransform(world_T_map);
 
        // Persist to the JSON file next to the .carto map file
        namespace fs = std::filesystem;
        fs::path jsonPath = armarx::PackagePath(properties.mapToLoad).toSystemPath();
        jsonPath.replace_extension("json");
 
        ARMARX_INFO << "Writing updated world-to-map registration to " << jsonPath;
 
        const float yaw = simox::math::mat3f_to_rpy(world_T_map.linear()).z();
 
        nlohmann::json j;
        j["x"] = world_T_map.translation().x();
        j["y"] = world_T_map.translation().y();
        j["yaw"] = yaw;
 
        std::ofstream ofs(jsonPath);
        ofs << j.dump(4);
 
        if (arvizDrawer)
        {
            arvizDrawer->setWorldToMapTransform(this->world_T_map);
        }
 
        if (arVizDrawerMapBuilder)
        {
            arVizDrawerMapBuilder = std::make_unique<cartographer_adapter::ArVizDrawerMapBuilder>(
                getArvizClient(), mappingAdapter->getMapBuilder(), this->world_T_map);
            arVizDrawerMapBuilder->drawOnce();
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
 
} // namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping