ProprioceptionStressTest.cpp

Go to the documentation of this file.

#include "ProprioceptionStressTest.h"
 
#include <iomanip>
#include <ArmarXCore/core/time/Metronome.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::armem::client::ProprioceptionStressTest,
                                     "ProprioceptionStressTest");
 
namespace armarx::armem::client
{
    ProprioceptionStressTest::ProprioceptionStressTest() :
        rng(std::random_device{}()),
        jointAngleDist(-M_PI, M_PI)
    {
    }
 
    ProprioceptionStressTest::~ProprioceptionStressTest() = default;
 
    std::string
    ProprioceptionStressTest::getDefaultName() const
    {
        return "ProprioceptionStressTest";
    }
 
    armarx::PropertyDefinitionsPtr
    ProprioceptionStressTest::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(properties.robotName,
                       "p.robotName",
                       "The name of the robot to test with (e.g., 'Armar6', 'Armar7')");
 
        defs->optional(properties.numJoints,
                       "p.numJoints",
                       "Number of mock joints to generate for testing");
 
        defs->optional(properties.writeFrequencyHz,
                       "p.writeFrequencyHz",
                       "Frequency of write operations in Hz (0 = disabled)");
 
        defs->optional(properties.readFrequencyHz,
                       "p.readFrequencyHz",
                       "Frequency of read operations in Hz (0 = disabled)");
 
        defs->optional(properties.numParallelReads,
                       "p.numParallelReads",
                       "Number of parallel read threads (for testing shared mutexes)");
 
        defs->optional(properties.numParallelWrites,
                       "p.numParallelWrites",
                       "Number of parallel write threads (for testing concurrent writes)");
 
        defs->optional(properties.numExtraFloats,
                       "p.numExtraFloats",
                       "Number of extra float values to write per snapshot");
 
        defs->optional(properties.numExtraLongs,
                       "p.numExtraLongs",
                       "Number of extra long values to write per snapshot");
 
        defs->optional(properties.statisticsIntervalSec,
                       "p.statisticsIntervalSec",
                       "How often to print statistics (in seconds)");
 
        defs->optional(properties.verboseLogging,
                       "p.verboseLogging",
                       "Enable verbose logging of each read/write operation");
 
        return defs;
    }
 
    void
    ProprioceptionStressTest::onInitComponent()
    {
        ARMARX_INFO << "Initializing ProprioceptionStressTest component";
        ARMARX_INFO << "  Robot: " << properties.robotName;
        ARMARX_INFO << "  Number of joints: " << properties.numJoints;
        ARMARX_INFO << "  Write frequency: " << properties.writeFrequencyHz << " Hz"
                    << " (x" << properties.numParallelWrites << " threads)";
        ARMARX_INFO << "  Read frequency: " << properties.readFrequencyHz << " Hz"
                    << " (x" << properties.numParallelReads << " threads)";
    }
 
    void
    ProprioceptionStressTest::onConnectComponent()
    {
        ARMARX_INFO << "Connecting to RobotStateMemory...";
 
        // Connect reader and writer to the memory system
        auto& mns = memoryNameSystem();
        robotReader.connect(mns);
        robotWriter.connect(mns);
 
        // Initialize memory writer for direct proprioception commits
        using namespace armem::robot_state;
        memoryWriter = mns.useWriter(constants::memoryName);
 
        ARMARX_INFO << "Connected to RobotStateMemory";
 
        // Record start time for statistics
        startTime = std::chrono::steady_clock::now();
 
        // Start write tasks if enabled
        if (properties.writeFrequencyHz > 0 && properties.numParallelWrites > 0)
        {
            ARMARX_INFO << "Starting " << properties.numParallelWrites << " write task(s) at "
                        << properties.writeFrequencyHz << " Hz each";
 
            for (int i = 0; i < properties.numParallelWrites; ++i)
            {
                auto task = new SimpleRunningTask<>([this]() {
                    this->writeLoop();
                });
                task->start();
                writeTasks.push_back(task);
            }
        }
        else
        {
            ARMARX_INFO << "Write operations disabled (frequency = 0 or numParallelWrites = 0)";
        }
 
        // Start read tasks if enabled
        if (properties.readFrequencyHz > 0 && properties.numParallelReads > 0)
        {
            ARMARX_INFO << "Starting " << properties.numParallelReads << " read task(s) at "
                        << properties.readFrequencyHz << " Hz each";
 
            for (int i = 0; i < properties.numParallelReads; ++i)
            {
                auto task = new SimpleRunningTask<>([this]() {
                    this->readLoop();
                });
                task->start();
                readTasks.push_back(task);
            }
        }
        else
        {
            ARMARX_INFO << "Read operations disabled (frequency = 0 or numParallelReads = 0)";
        }
 
        // Start statistics task
        if (properties.statisticsIntervalSec > 0)
        {
            ARMARX_INFO << "Starting statistics task (interval: "
                        << properties.statisticsIntervalSec << " seconds)";
            statsTask = new SimpleRunningTask<>([this]() {
                Metronome metronome(
                    Frequency::Hertz(1.0f / properties.statisticsIntervalSec));
                while (statsTask && !statsTask->isStopped())
                {
                    printStatistics();
                    metronome.waitForNextTick();
                }
            });
            statsTask->start();
        }
 
        ARMARX_INFO << "All tasks started successfully";
    }
 
    void
    ProprioceptionStressTest::onDisconnectComponent()
    {
        ARMARX_INFO << "Disconnecting ProprioceptionStressTest...";
 
        // Stop all write tasks
        for (auto& task : writeTasks)
        {
            if (task)
            {
                task->stop();
            }
        }
        writeTasks.clear();
 
        // Stop all read tasks
        for (auto& task : readTasks)
        {
            if (task)
            {
                task->stop();
            }
        }
        readTasks.clear();
 
        // Stop statistics task
        if (statsTask)
        {
            statsTask->stop();
            statsTask = nullptr;
        }
 
        // Print final statistics
        printStatistics();
 
        ARMARX_INFO << "ProprioceptionStressTest disconnected";
    }
 
    void
    ProprioceptionStressTest::onExitComponent()
    {
        ARMARX_INFO << "Exiting ProprioceptionStressTest component";
    }
 
    void
    ProprioceptionStressTest::writeLoop()
    {
        Metronome metronome(Frequency::Hertz(properties.writeFrequencyHz));
 
        while (!writeTasks.empty())
        {
            try
            {
                // Generate complete mock proprioception data (joints + extras)
                auto proprioception = generateMockProprioceptionData();
 
                // Write to memory
                const armarx::armem::Time timestamp = armarx::armem::Time::Now();
                bool success = storeProprioceptionWithExtras(
                    proprioception, properties.robotName, properties.robotName, timestamp);
 
                if (success)
                {
                    writeCount++;
 
                    if (properties.verboseLogging)
                    {
                        ARMARX_DEBUG << "Write #" << writeCount.load()
                                     << ": Stored proprioception data with "
                                     << proprioception.joints.position.size() << " joints, "
                                     << proprioception.extraFloats.size() << " extraFloats, "
                                     << proprioception.extraLongs.size() << " extraLongs at "
                                     << timestamp.toMicroSecondsSinceEpoch() << " us";
                    }
                }
                else
                {
                    writeErrorCount++;
                    ARMARX_WARNING << "Failed to write proprioception data (write #"
                                   << (writeCount.load() + writeErrorCount.load()) << ")";
                }
            }
            catch (const std::exception& e)
            {
                writeErrorCount++;
                ARMARX_ERROR << "Exception during write operation: " << e.what();
            }
 
            metronome.waitForNextTick();
        }
    }
 
    void
    ProprioceptionStressTest::readLoop()
    {
        Metronome metronome(Frequency::Hertz(properties.readFrequencyHz));
 
        while (!readTasks.empty())
        {
            try
            {
                // Query proprioception data
                const armarx::armem::Time timestamp = armarx::armem::Time::Now();
                auto proprioception = robotReader.queryProprioception(properties.robotName, timestamp);
 
                readCount++;
 
                if (proprioception.has_value())
                {
                    readSuccessCount++;
 
                    if (properties.verboseLogging)
                    {
                        const auto& joints = proprioception->joints.position;
                        ARMARX_DEBUG << "Read #" << readCount.load()
                                     << ": Retrieved proprioception data with " << joints.size()
                                     << " joints at " << timestamp.toMicroSecondsSinceEpoch() << " us";
                    }
                }
                else
                {
                    readFailureCount++;
 
                    if (properties.verboseLogging)
                    {
                        ARMARX_DEBUG << "Read #" << readCount.load()
                                     << ": No proprioception data available at "
                                     << timestamp.toMicroSecondsSinceEpoch() << " us";
                    }
                }
            }
            catch (const std::exception& e)
            {
                readFailureCount++;
                ARMARX_ERROR << "Exception during read operation: " << e.what();
            }
 
            metronome.waitForNextTick();
        }
    }
 
    arondto::Proprioception
    ProprioceptionStressTest::generateMockProprioceptionData()
    {
        arondto::Proprioception proprioception;
 
        // Generate mock joint positions with random values
        for (int i = 0; i < properties.numJoints; ++i)
        {
            std::string jointName = "Joint_" + std::to_string(i);
            float angle = jointAngleDist(rng);
            proprioception.joints.position[jointName] = angle;
        }
 
        // Generate extra float values
        for (int i = 0; i < properties.numExtraFloats; ++i)
        {
            std::string key = "extraFloat_" + std::to_string(i);
            float value = jointAngleDist(rng);
            proprioception.extraFloats[key] = value;
        }
 
        // Generate extra long values
        for (int i = 0; i < properties.numExtraLongs; ++i)
        {
            std::string key = "extraLong_" + std::to_string(i);
            long value = static_cast<long>(jointAngleDist(rng) * 1000);
            proprioception.extraLongs[key] = value;
        }
 
        return proprioception;
    }
 
    bool
    ProprioceptionStressTest::storeProprioceptionWithExtras(
        const arondto::Proprioception& proprioception,
        const std::string& robotTypeName,
        const std::string& robotName,
        const armem::Time& timestamp)
    {
        using namespace armem::robot_state;
 
        // Create memory ID for proprioception segment
        const auto providerId = armem::MemoryID(
            constants::memoryName, constants::proprioceptionCoreSegment, robotTypeName);
        const auto entityID = providerId.withEntityName(robotName).withTimestamp(timestamp);
 
        // Create entity update
        armem::EntityUpdate update;
        update.entityID = entityID;
        update.instancesData = {proprioception.toAron()};
        update.referencedTime = timestamp;
 
        // Commit to memory
        armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
        if (!updateResult.success)
        {
            ARMARX_ERROR << "Failed to commit proprioception data: " << updateResult.errorMessage;
            return false;
        }
 
        return true;
    }
 
    void
    ProprioceptionStressTest::printStatistics()
    {
        const auto now = std::chrono::steady_clock::now();
        const auto elapsedSec =
            std::chrono::duration_cast<std::chrono::duration<double>>(now - startTime).count();
 
        const uint64_t writes = writeCount.load();
        const uint64_t writeErrors = writeErrorCount.load();
        const uint64_t reads = readCount.load();
        const uint64_t readSuccess = readSuccessCount.load();
        const uint64_t readFailure = readFailureCount.load();
 
        const double writeRate = (elapsedSec > 0) ? (writes / elapsedSec) : 0.0;
        const double readRate = (elapsedSec > 0) ? (reads / elapsedSec) : 0.0;
        const double readSuccessRate = (reads > 0) ? (100.0 * readSuccess / reads) : 0.0;
 
        ARMARX_INFO << "\n"
                    << "========================================\n"
                    << "  ProprioceptionStressTest Statistics\n"
                    << "========================================\n"
                    << "Elapsed time:        " << std::fixed << std::setprecision(2) << elapsedSec
                    << " s\n"
                    << "Robot:               " << properties.robotName << "\n"
                    << "----------------------------------------\n"
                    << "WRITE Operations:\n"
                    << "  Total writes:      " << writes << "\n"
                    << "  Write errors:      " << writeErrors << "\n"
                    << "  Write rate:        " << std::fixed << std::setprecision(2) << writeRate
                    << " ops/s\n"
                    << "----------------------------------------\n"
                    << "READ Operations:\n"
                    << "  Total reads:       " << reads << "\n"
                    << "  Successful reads:  " << readSuccess << "\n"
                    << "  Failed reads:      " << readFailure << "\n"
                    << "  Read rate:         " << std::fixed << std::setprecision(2) << readRate
                    << " ops/s\n"
                    << "  Success rate:      " << std::fixed << std::setprecision(1)
                    << readSuccessRate << " %\n"
                    << "========================================";
    }
 
} // namespace armarx::armem::client