RobotUnitReader.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <memory>
#include <optional>
#include <queue>
#include <string>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
 
#include "../exteroception/converters/ConverterInterface.h"
#include "../exteroception/converters/ConverterRegistry.h"
#include "RobotUnitData.h"
#include "converters/ConverterInterface.h"
#include "converters/ConverterRegistry.h"
 
namespace armarx::plugins
{
    class RobotUnitComponentPlugin;
    class DebugObserverComponentPlugin;
} // namespace armarx::plugins
 
namespace armarx
{
    using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
}
 
// TODO move this out of proprioception. it is now for proprioception and exteroception
namespace armarx::armem::server::robot_state::proprioception
{
    class RobotUnitReader : public armarx::Logging
    {
    public:
        RobotUnitReader();
 
        using Queue = armarx::armem::server::robot_state::proprioception::Queue;
 
 
        void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
                     armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
                     const std::string& robotTypeName);
 
 
        /// Reads data from `handler` and fills `dataQueue`.
        void run(float pollFrequency, Queue& dataBuffer);
 
        std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
 
 
    private:
        /// Fetch the latest timestep and clear the robot unit buffer.
        std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
 
 
    public:
        struct Properties
        {
            std::string sensorPrefix = "sens.*";
        };
 
        Properties properties;
 
        std::optional<DebugObserverHelper> debugObserver;
 
 
        RobotUnitDataStreaming::Config config;
        RobotUnitDataStreamingReceiverPtr receiver;
        RobotUnitDataStreaming::DataStreamingDescription description;
 
        ConverterRegistry proprioceptionConverterRegistry;
        exteroception::ConverterRegistry exteroceptionConverterRegistry;
        proprioception::ConverterInterface* converterProprioception = nullptr;
        exteroception::ConverterInterface* converterExteroception = nullptr;
 
        armarx::SimpleRunningTask<>::pointer_type task = nullptr;
    };
 
} // namespace armarx::armem::server::robot_state::proprioception