ExampleMemory.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/>.
 *
 * @package    RobotAPI::ArmarXObjects::ExampleMemory
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ExampleMemory.h"
 
#include <atomic>
 
#include <SimoxUtility/algorithm/string.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/PythonExampleData.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::ExampleMemory, "ExampleMemory");
 
namespace armarx
{
    armarx::PropertyDefinitionsPtr
    ExampleMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->topic(debugObserver);
 
        setMemoryName("Example");
 
        p.core._defaultSegmentsStr = simox::alg::join(p.core.defaultCoreSegments, ", ");
        defs->optional(p.core._defaultSegmentsStr,
                       "core.DefaultSegments",
                       "Core segments to add on start up (just as example).");
        defs->optional(
            p.core.addOnUsage,
            "core.AddOnUsage",
            "If enabled, core segments are added when required by a new provider segment."
            "This will usually be off for most memory servers.");
 
        defs->optional(p.enableRemoteGui,
                       "p.enableRemoteGui",
                       "If true, the remote gui is enabled.");
        defs->optional(p.showMemoryContent,
                       "p.showMemoryContent",
                       "If true, memory content is shown in remote gui (requires enableRemoteGui=true). "
                       "Can be very slow for high-frequency updates and may cause nullptr errors!");
        defs->optional(p.showClientStats,
                       "p.showClientStats",
                       "If true, client statistics are shown in the remote gui (requires enableRemoteGui=true).");
        defs->optional(p.publishToDebugObserver,
                       "p.publishToDebugObserver",
                       "If true, client statistics are published to DebugObserver. "
                       "Channel format: '<ServerName> | Client<N>' with datafields: commits, queries, address, port, pid (if available).");
        return defs;
    }
 
    std::string
    ExampleMemory::getDefaultName() const
    {
        return "ExampleMemory";
    }
 
    void
    ExampleMemory::onInitComponent()
    {
        // Usually, the memory server will specify a number of core segments with a specific aron type.
        workingMemory().addCoreSegment("ExampleData", armem::example::ExampleData::ToAronType());
        workingMemory().addCoreSegment("LinkedData", armem::example::LinkedData::ToAronType());
 
        workingMemory().addCoreSegment("PythonExampleData",
                                       armem::example::PythonExampleData::ToAronType());
 
 
        // We support the "Latest" prediction engine for the entire memory.
        workingMemory().addPredictor(armem::PredictionEngine{.engineID = "Latest"},
                                     [this](const armem::PredictionRequest& request)
                                     { return this->predictLatest(request); });
 
        // For illustration purposes, we add more segments (without types).
        bool trim = true;
        p.core.defaultCoreSegments = simox::alg::split(p.core._defaultSegmentsStr, ",", trim);
        p.core._defaultSegmentsStr.clear();
 
        for (const std::string& name : p.core.defaultCoreSegments)
        {
            auto& c = workingMemory().addCoreSegment(name);
            c.setMaxHistorySize(100);
        }
    }
 
    void
    ExampleMemory::onConnectComponent()
    {
        if (p.enableRemoteGui)
        {
            tab.lastStatsUpdate = armem::Time::Now();
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
    }
 
    void
    ExampleMemory::onDisconnectComponent()
    {
        // Log client statistics before shutdown
        logClientStatistics();
    }
 
    void
    ExampleMemory::onExitComponent()
    {
    }
 
    // WRITING
 
    armem::data::AddSegmentsResult
    ExampleMemory::addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&)
    {
        // This function is overloaded to trigger the remote gui rebuild.
        armem::data::AddSegmentsResult result =
            ReadWritePluginUser::addSegments(input, p.core.addOnUsage);
        tab.rebuild = true;
        return result;
    }
 
    armem::data::CommitResult
    ExampleMemory::commit(const armem::data::Commit& commit, const Ice::Current& current)
    {
        // Track client statistics
        recordCommit(current);
 
        // This function is overloaded to trigger the remote gui rebuild.
        armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
        tab.rebuild = true;
        return result;
    }
 
    armem::query::data::Result
    ExampleMemory::query(const armem::query::data::Input& input, const Ice::Current& current)
    {
        // Track client statistics (GUI updates only via RemoteGui_update timer)
        recordQuery(current);
 
        // Forward to base implementation
        return ReadWritePluginUser::query(input);
    }
 
    // READING
 
    // Inherited from Plugin
 
 
    // ACTIONS
    armem::actions::GetActionsOutputSeq
    ExampleMemory::getActions(const armem::actions::GetActionsInputSeq& input)
    {
        using namespace armem::actions;
        Action greeting{"hi", "Say hello to " + input[0].id.entityName};
        Action failure{"fail", "Fail dramatically"};
        Action nothing{"null", "Do nothing, but deeply nested"};
 
        SubMenu one{"one", "One", {nothing}};
        SubMenu two{"two", "Two", {one}};
        SubMenu three{"three", "Three", {two}};
        SubMenu four{"four", "Four", {three}};
 
        Menu menu{greeting,
                  failure,
                  four,
                  SubMenu{"mut", "Mutate", {Action{"copy", "Copy latest instance"}}}};
 
        return {{menu.toIce()}};
    }
 
    armem::actions::ExecuteActionOutputSeq
    ExampleMemory::executeActions(const armem::actions::ExecuteActionInputSeq& input)
    {
        using namespace armem::actions;
 
        ExecuteActionOutputSeq output;
        for (const auto& [id, path] : input)
        {
            const armem::MemoryID memoryID = armarx::fromIce<armem::MemoryID>(id);
            if (path == ActionPath{"hi"})
            {
                ARMARX_INFO << "Hello, " << memoryID.str() << "!";
                output.emplace_back(true, "");
            }
            else if (path == ActionPath{"fail"})
            {
                ARMARX_WARNING << "Alas, I am gravely wounded!";
                output.emplace_back(false, "Why would you do that to him?");
            }
            else if (not path.empty() and path.front() == "four" and path.back() == "null")
            {
                // Do nothing.
                ARMARX_INFO << "Nested action (path: " << path << ")";
                output.emplace_back(true, "");
            }
            else if (path == ActionPath{"mut", "copy"})
            {
                auto* instance = workingMemory().findLatestInstance(memoryID);
                if (instance != nullptr)
                {
                    armem::EntityUpdate update;
                    armem::MemoryID newID =
                        memoryID.getCoreSegmentID()
                            .withProviderSegmentName(memoryID.providerSegmentName)
                            .withEntityName(memoryID.entityName);
                    update.entityID = newID;
                    update.referencedTime = armem::Time::Now();
                    update.instancesData = {instance->data()};
 
                    armem::Commit newCommit;
                    newCommit.add(update);
                    this->commit(armarx::toIce<armem::data::Commit>(newCommit));
 
                    tab.rebuild = true;
                    ARMARX_INFO << "Duplicated " << memoryID;
                    output.emplace_back(true, "");
                }
                else
                {
                    output.emplace_back(false, "Couldn't duplicate " + memoryID.str());
                }
            }
        }
 
        return output;
    }
 
    // PREDICTING
    armem::PredictionResult
    ExampleMemory::predictLatest(const armem::PredictionRequest& request)
    {
        armem::PredictionResult result;
        auto memID = request.snapshotID;
        result.snapshotID = memID;
 
        armem::client::QueryBuilder builder;
        builder.latestEntitySnapshot(memID);
        auto queryResult =
            query(armarx::toIce<armem::query::data::Input>(builder.buildQueryInput()));
        if (queryResult.success)
        {
            auto readMemory = fromIce<armem::wm::Memory>(queryResult.memory);
            auto* latest = readMemory.findLatestSnapshot(memID);
            if (latest != nullptr)
            {
                auto instance = memID.hasInstanceIndex()
                                    ? latest->getInstance(memID)
                                    : latest->getInstance(latest->getInstanceIndices().at(0));
                result.success = true;
                result.prediction = instance.data();
            }
            else
            {
                result.success = false;
                result.errorMessage =
                    "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
            }
        }
        else
        {
            result.success = false;
            result.errorMessage =
                "Could not find entity referenced by MemoryID '" + memID.str() + "'.";
        }
 
        return result;
    }
 
    // REMOTE GUI
 
    void
    ExampleMemory::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root;
 
        // Show memory content (if enabled)
        if (p.showMemoryContent)
        {
            try
            {
                // Core segments are locked by MemoryRemoteGui.
                tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
                root.addChild(tab.memoryGroup);
            }
            catch (const std::exception& e)
            {
                ARMARX_WARNING << "Failed to create memory content display: " << e.what();
                GroupBox errorBox;
                errorBox.setLabel("Memory Content");
                errorBox.addChild(Label("Error displaying memory content - see logs"));
                root.addChild(errorBox);
            }
        }
 
        // Create client statistics group box (if enabled)
        if (p.showClientStats)
        {
            tab.clientStatsGroup = createClientStatisticsGroupBox();
            root.addChild(tab.clientStatsGroup);
        }
 
        root.addChild(VSpacer());
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    ExampleMemory::RemoteGui_update()
    {
        // Full rebuild if explicitly requested (e.g., memory content changed)
        if (tab.rebuild.exchange(false))
        {
            createRemoteGuiTab();
            tab.lastStatsUpdate = armem::Time::Now();
            return;
        }
 
        // Periodic update for client statistics (every 10 seconds) - just update labels, no rebuild
        if (p.showClientStats)
        {
            armem::Time now = armem::Time::Now();
            if ((now - tab.lastStatsUpdate).toMilliSecondsDouble() > tab.statsUpdateIntervalMs)
            {
                // Check if we need to rebuild (new client appeared)
                bool needsRebuild = false;
                {
                    std::lock_guard lock(clientStatsMutex);
                    // Rebuild if number of clients changed (new client or client disconnected would require new labels)
                    if (clientStats.size() != tab.clientLabels.size())
                    {
                        needsRebuild = true;
                    }
                }
 
                if (needsRebuild)
                {
                    // Rebuild structure when clients added/removed
                    tab.rebuild = true;
                }
                else
                {
                    // Just update existing labels (much faster)
                    updateClientStatisticsLabels();
                }
 
                tab.lastStatsUpdate = now;
            }
        }
    }
 
    RemoteGui::Client::GroupBox
    ExampleMemory::createClientStatisticsGroupBox()
    {
        using namespace armarx::RemoteGui::Client;
 
        std::lock_guard lock(clientStatsMutex);
 
        GroupBox statsBox;
        statsBox.setLabel("Client Statistics");
 
        // Create summary label
        tab.summaryLabel = Label("No clients connected yet");
        statsBox.addChild(tab.summaryLabel);
 
        // Create labels for each existing client
        tab.clientLabels.clear();
        tab.connectionLabels.clear();
 
        int clientIndex = 1;
        for (const auto& [clientId, stats] : clientStats)
        {
            // Client info label
            Label clientLabel = Label("");
            tab.clientLabels[clientId] = clientLabel;
            statsBox.addChild(clientLabel);
 
            // Connection label
            Label connLabel = Label("");
            tab.connectionLabels[clientId] = connLabel;
            statsBox.addChild(connLabel);
 
            clientIndex++;
        }
 
        // Update the text content (mutex already locked)
        updateClientStatisticsLabelsInternal();
 
        return statsBox;
    }
 
    void
    ExampleMemory::updateClientStatisticsLabels()
    {
        std::lock_guard lock(clientStatsMutex);
        updateClientStatisticsLabelsInternal();
    }
 
    void
    ExampleMemory::updateClientStatisticsLabelsInternal()
    {
        using namespace armarx::RemoteGui::Client;
 
        // NOTE: Assumes clientStatsMutex is already locked by caller
 
        // Update summary
        if (clientStats.empty())
        {
            tab.summaryLabel.setText("No clients connected yet");
        }
        else
        {
            std::stringstream summary;
            summary << "Total clients: " << clientStats.size();
            tab.summaryLabel.setText(summary.str());
        }
 
        // Update each client's labels
        int clientIndex = 1;
        for (const auto& [clientId, stats] : clientStats)
        {
            // Update client info
            if (tab.clientLabels.count(clientId) > 0)
            {
                std::stringstream clientInfo;
                clientInfo << "Client " << clientIndex << ": "
                          << "Commits=" << stats.commitCount << ", "
                          << "Queries=" << stats.queryCount;
                tab.clientLabels[clientId].setText(clientInfo.str());
            }
 
            // Update connection info
            if (tab.connectionLabels.count(clientId) > 0)
            {
                std::stringstream connInfo;
                connInfo << "  " << stats.remoteAddress;
                if (stats.remotePort > 0)
                {
                    connInfo << ":" << stats.remotePort;
                }
                if (stats.clientPid > 0)
                {
                    connInfo << " (PID: " << stats.clientPid << ")";
                }
 
                std::string connStr = connInfo.str();
                if (connStr.length() > 70)
                {
                    connStr = connStr.substr(0, 67) + "...";
                }
                tab.connectionLabels[clientId].setText(connStr);
            }
 
            clientIndex++;
        }
    }
 
    // CLIENT STATISTICS TRACKING
 
    std::string
    ExampleMemory::extractClientIdentifier(const Ice::Current& current) const
    {
        if (current.con)
        {
            // Get connection string which uniquely identifies the client connection
            return current.con->toString();
        }
        return "unknown";
    }
 
    std::string
    ExampleMemory::formatClientIdentifier(const std::string& connectionString) const
    {
        // Ice connection strings have format:
        // "local address = <IP>:<port>\nremote address = <IP>:<port>"
        // Extract the remote address for cleaner display
 
        // Find "remote address = " marker
        size_t remotePos = connectionString.find("remote address = ");
        if (remotePos != std::string::npos)
        {
            // Extract from "remote address = " to end (or newline)
            std::string remote = connectionString.substr(remotePos + 17); // 17 = length of "remote address = "
 
            // Remove any trailing whitespace or newlines
            size_t end = remote.find_first_of("\r\n");
            if (end != std::string::npos)
            {
                remote = remote.substr(0, end);
            }
 
            return remote;
        }
 
        // If no "remote address" found, try to clean up the full string
        // Replace newlines with spaces for single-line display
        std::string cleaned = connectionString;
        size_t pos = 0;
        while ((pos = cleaned.find('\n', pos)) != std::string::npos)
        {
            cleaned.replace(pos, 1, " | ");
            pos += 3;
        }
 
        return cleaned;
    }
 
    void
    ExampleMemory::parseConnectionInfo(const std::string& connectionString, std::string& address, int& port) const
    {
        // Extract remote address and port from Ice connection string
        std::string fullAddress = formatClientIdentifier(connectionString);
 
        // Find last colon to separate address from port
        // Need to handle IPv6 addresses which contain colons
        size_t lastColon = fullAddress.rfind(':');
 
        if (lastColon != std::string::npos)
        {
            // Extract port
            std::string portStr = fullAddress.substr(lastColon + 1);
            try
            {
                port = std::stoi(portStr);
                address = fullAddress.substr(0, lastColon);
            }
            catch (const std::exception&)
            {
                // If port parsing fails, treat entire string as address
                address = fullAddress;
                port = -1;
            }
        }
        else
        {
            // No colon found
            address = fullAddress;
            port = -1;
        }
    }
 
    int
    ExampleMemory::extractClientPid(const Ice::Current& current) const
    {
        // Try to extract PID from Ice context
        auto it = current.ctx.find("clientPid");
        if (it != current.ctx.end())
        {
            try
            {
                return std::stoi(it->second);
            }
            catch (const std::exception&)
            {
                return -1;
            }
        }
        return -1;
    }
 
    int
    ExampleMemory::getClientIndex(const std::string& clientId)
    {
        // NOTE: Assumes clientStatsMutex is already locked by caller
 
        // Check if client already has an index
        auto it = clientIndices.find(clientId);
        if (it != clientIndices.end())
        {
            return it->second;
        }
 
        // Assign new index
        int index = nextClientIndex++;
        clientIndices[clientId] = index;
        return index;
    }
 
    void
    ExampleMemory::recordCommit(const Ice::Current& current)
    {
        std::lock_guard lock(clientStatsMutex);
        std::string clientId = extractClientIdentifier(current);
 
        auto& stats = clientStats[clientId];
        bool isNewClient = stats.connectionString.empty();
 
        if (isNewClient)
        {
            stats.connectionString = clientId;
            stats.firstSeen = armem::Time::Now();
 
            // Parse connection info to extract IP and port
            parseConnectionInfo(clientId, stats.remoteAddress, stats.remotePort);
 
            // Extract PID from Ice context if available
            stats.clientPid = extractClientPid(current);
 
            ARMARX_INFO << "New client connected: " << clientId;
        }
 
        stats.commitCount++;
        stats.lastCommit = armem::Time::Now();
 
        // Publish to DebugObserver if enabled
        if (p.publishToDebugObserver && debugObserver)
        {
            int clientIndex = getClientIndex(clientId);
            std::string channel = getName() + " | Client" + std::to_string(clientIndex);
            StringVariantBaseMap values;
            values["commits"] = new armarx::Variant(static_cast<int>(stats.commitCount));
            values["queries"] = new armarx::Variant(static_cast<int>(stats.queryCount));
            values["address"] = new armarx::Variant(stats.remoteAddress);
            values["port"] = new armarx::Variant(stats.remotePort);
            if (stats.clientPid > 0)
            {
                values["pid"] = new armarx::Variant(stats.clientPid);
            }
            debugObserver->setDebugChannel(channel, values);
        }
    }
 
    void
    ExampleMemory::recordQuery(const Ice::Current& current)
    {
        std::lock_guard lock(clientStatsMutex);
        std::string clientId = extractClientIdentifier(current);
 
        auto& stats = clientStats[clientId];
        bool isNewClient = stats.connectionString.empty();
 
        if (isNewClient)
        {
            stats.connectionString = clientId;
            stats.firstSeen = armem::Time::Now();
 
            // Parse connection info to extract IP and port
            parseConnectionInfo(clientId, stats.remoteAddress, stats.remotePort);
 
            // Extract PID from Ice context if available
            stats.clientPid = extractClientPid(current);
 
            ARMARX_INFO << "New client connected: " << clientId;
        }
 
        stats.queryCount++;
        stats.lastQuery = armem::Time::Now();
 
        // Publish to DebugObserver if enabled
        if (p.publishToDebugObserver && debugObserver)
        {
            int clientIndex = getClientIndex(clientId);
            std::string channel = getName() + " | Client" + std::to_string(clientIndex);
            StringVariantBaseMap values;
            values["commits"] = new armarx::Variant(static_cast<int>(stats.commitCount));
            values["queries"] = new armarx::Variant(static_cast<int>(stats.queryCount));
            values["address"] = new armarx::Variant(stats.remoteAddress);
            values["port"] = new armarx::Variant(stats.remotePort);
            if (stats.clientPid > 0)
            {
                values["pid"] = new armarx::Variant(stats.clientPid);
            }
            debugObserver->setDebugChannel(channel, values);
        }
    }
 
    void
    ExampleMemory::logClientStatistics() const
    {
        std::lock_guard lock(clientStatsMutex);
 
        if (clientStats.empty())
        {
            ARMARX_INFO << "No client statistics available.";
            return;
        }
 
        ARMARX_IMPORTANT << "=== Client Statistics ===";
        ARMARX_IMPORTANT << "Total clients tracked: " << clientStats.size();
 
        for (const auto& [clientId, stats] : clientStats)
        {
            ARMARX_IMPORTANT << "\nClient: " << stats.remoteAddress
                            << (stats.remotePort > 0 ? ":" + std::to_string(stats.remotePort) : "");
            if (stats.clientPid > 0)
            {
                ARMARX_IMPORTANT << "  PID: " << stats.clientPid;
            }
            ARMARX_IMPORTANT << "  Commits: " << stats.commitCount;
            ARMARX_IMPORTANT << "  Queries: " << stats.queryCount;
            ARMARX_IMPORTANT << "  First seen: " << stats.firstSeen.toDateTimeString();
 
            if (stats.commitCount > 0)
            {
                ARMARX_IMPORTANT << "  Last commit: " << stats.lastCommit.toDateTimeString();
            }
            if (stats.queryCount > 0)
            {
                ARMARX_IMPORTANT << "  Last query: " << stats.lastQuery.toDateTimeString();
            }
 
            // Show full connection string for debugging
            ARMARX_INFO << "  Raw connection: " << stats.connectionString;
        }
 
        ARMARX_IMPORTANT << "========================";
    }
 
} // namespace armarx