ProprioceptionStressTest.h

Go to the documentation of this file.

#pragma once
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/armem/client/plugins.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/RobotWriter.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/constants.h>
 
#include <atomic>
#include <chrono>
#include <random>
#include <vector>
 
namespace armarx::armem::client
{
    namespace arondto = ::armarx::armem::arondto;
 
    /**
     * @brief Debug component for stress-testing the Proprioception segment
     *        of RobotStateMemory with high-frequency read and write operations.
     *
     * This component generates mock joint data and performs configurable
     * read/write operations to test memory performance and concurrency.
     */
    class ProprioceptionStressTest :
        virtual public armarx::Component,
        virtual public armarx::armem::client::ComponentPluginUser
    {
    public:
        ProprioceptionStressTest();
        ~ProprioceptionStressTest() override;
 
        std::string getDefaultName() const override;
 
    protected:
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
        void onInitComponent() override;
        void onConnectComponent() override;
        void onDisconnectComponent() override;
        void onExitComponent() override;
 
    private:
        /// Main write loop - generates and writes mock proprioception data
        void writeLoop();
 
        /// Main read loop - reads proprioception data and tracks statistics
        void readLoop();
 
        /// Generate complete mock proprioception data (joints + extra fields)
        arondto::Proprioception generateMockProprioceptionData();
 
        /// Store proprioception data with extra fields directly to memory
        bool storeProprioceptionWithExtras(const arondto::Proprioception& proprioception,
                                           const std::string& robotTypeName,
                                           const std::string& robotName,
                                           const armem::Time& timestamp);
 
        /// Print statistics about read/write operations
        void printStatistics();
 
        struct Properties
        {
            /// Name of the robot to test (e.g., "Armar6", "Armar7")
            std::string robotName = "Armar6";
 
            /// Number of mock joints to generate
            int numJoints = 30;
 
            /// Frequency of write operations in Hz (0 = disabled)
            float writeFrequencyHz = 100.0f;
 
            /// Frequency of read operations in Hz (0 = disabled)
            float readFrequencyHz = 100.0f;
 
            /// Number of parallel read threads (for testing shared mutexes)
            int numParallelReads = 1;
 
            /// Number of parallel write threads (for testing concurrent writes)
            int numParallelWrites = 1;
 
            /// Number of extra float values to write per snapshot
            int numExtraFloats = 10;
 
            /// Number of extra long values to write per snapshot
            int numExtraLongs = 10;
 
            /// How often to print statistics (in seconds)
            float statisticsIntervalSec = 1.0f;
 
            /// Enable verbose logging of each operation
            bool verboseLogging = false;
        };
 
        Properties properties;
 
        /// Robot state reader and writer
        armarx::armem::robot_state::RobotReader robotReader;
        armarx::armem::robot_state::RobotWriter robotWriter;
 
        /// Direct memory writer for custom proprioception data
        armarx::armem::client::Writer memoryWriter;
 
        /// Running tasks for read and write loops
        std::vector<armarx::SimpleRunningTask<>::pointer_type> writeTasks;
        std::vector<armarx::SimpleRunningTask<>::pointer_type> readTasks;
        armarx::SimpleRunningTask<>::pointer_type statsTask;
 
        /// Random number generator for mock data
        std::mt19937 rng;
        std::uniform_real_distribution<float> jointAngleDist;
 
        /// Statistics tracking (atomic for thread safety)
        std::atomic<uint64_t> writeCount{0};
        std::atomic<uint64_t> readCount{0};
        std::atomic<uint64_t> readSuccessCount{0};
        std::atomic<uint64_t> readFailureCount{0};
        std::atomic<uint64_t> writeErrorCount{0};
 
        std::chrono::steady_clock::time_point startTime;
    };
 
} // namespace armarx::armem::client