MemoryListener.cpp

Go to the documentation of this file.

#include "MemoryListener.h"
 
#include <sstream>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/logging/Logging.h>
 
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
namespace armarx::armem::client::util
{
 
    std::string
    MemoryListener::MakeMemoryTopicName(const MemoryID& memoryID)
    {
        return "MemoryUpdates." + memoryID.memoryName;
    }
 
    MemoryListener::MemoryListener(ManagedIceObject* component) : component(component)
    {
    }
 
    void
    MemoryListener::setComponent(ManagedIceObject* component)
    {
        this->component = component;
    }
 
    void
    MemoryListener::updated(const std::vector<data::MemoryID>& updatedSnapshotIDs) const
    {
        std::vector<MemoryID> bos;
        armarx::fromIce(updatedSnapshotIDs, bos);
        updated(bos);
    }
 
    void
    MemoryListener::updated(const std::vector<MemoryID>& updatedSnapshotIDs) const
    {
        std::stringstream error;
 
        for (const auto& [subscription, subCallbacks] : this->callbacks)
        {
            std::vector<MemoryID> matchingSnapshotIDs;
 
            for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
            {
                try
                {
                    if (contains(subscription, updatedSnapshotID))
                    {
                        // ARMARX_IMPORTANT << VAROUT(subscription) << " matches " << VAROUT(updatedSnapshotID);
                        matchingSnapshotIDs.push_back(updatedSnapshotID);
                    }
                    else
                    {
                        // ARMARX_IMPORTANT << VAROUT(subscription) << " does not match " << VAROUT(updatedSnapshotID);
                    }
                }
                catch (const armem::error::InvalidMemoryID& e)
                {
                    // Log to debug, but ignore otherwise
                    error << "Error when comparing subscribed ID " << subscription
                          << " with updated ID " << updatedSnapshotID << ":\n"
                          << e.what() << "\n\n";
                }
            }
 
            if (not matchingSnapshotIDs.empty())
            {
                ARMARX_DEBUG << "Calling " << subCallbacks.size() << " callbacks"
                             << " subscribing " << subscription << " with "
                             << matchingSnapshotIDs.size() << " snapshot IDs ...";
                for (auto& managedCallback : subCallbacks)
                {
                    try
                    {
                        managedCallback.callback(subscription, matchingSnapshotIDs);
                    }
                    catch (const armarx::LocalException& e)
                    {
                        error << "Calling callback subscribing " << subscription << " failed."
                              << "\nCaught armarx::LocalException:"
                              << "\n"
                              << e.getReason() << "\n Stacktrace: \n"
                              << e.generateBacktrace() << "\n";
                    }
                    catch (const std::exception& e)
                    {
                        error << "Calling callback subscribing " << subscription << " failed."
                              << "\nCaught armarx::Exception:"
                              << "\n"
                              << e.what() << "\n";
                    }
                    catch (...)
                    {
                        error << "Calling callback subscribing " << subscription << " failed."
                              << "\nCaught unknown exception."
                              << "\n";
                    }
                }
            }
        }
        if (error.str().size() > 0)
        {
            ARMARX_WARNING << "The following issues were encountered during MemoryListener::"
                           << __FUNCTION__ << "(): \n\n"
                           << error.str();
        }
    }
 
    SubscriptionHandle
    MemoryListener::subscribe(const MemoryID& memoryID, Callback callback)
    {
        ARMARX_CHECK_NOT_EMPTY(memoryID.memoryName)
            << "The memoryName must be specified to subscribe";
 
        if (component and memoryRefCount[memoryID.memoryName] == 0)
        {
            component->usingTopic(MakeMemoryTopicName(memoryID));
        }
 
        auto id = nextId++;
        callbacks[memoryID].push_back({id, callback});
 
        memoryRefCount[memoryID.memoryName]++;
 
        return SubscriptionHandle(this, memoryID, id);
    }
 
    SubscriptionHandle
    MemoryListener::subscribe(const MemoryID& subscriptionID, CallbackUpdatedOnly callback)
    {
        return subscribe(
            subscriptionID,
            [callback](const MemoryID&, const std::vector<MemoryID>& updatedSnapshotIDs)
            { callback(updatedSnapshotIDs); });
    }
 
    void
    MemoryListener::unsubscribe(SubscriptionHandle& handle)
    {
        if (not handle.valid)
        {
            return;
        }
        handle.valid = false;
 
        // Remove ManagedCallback with ManagedCallback.id == handle.id from callbacks[handle.memoryID]
        auto it = std::find_if(callbacks[handle.memoryID].begin(),
                               callbacks[handle.memoryID].end(),
                               [&handle](ManagedCallback& mCb) { return mCb.id == handle.id; });
 
        std::iter_swap(it, callbacks[handle.memoryID].end() - 1);
        callbacks[handle.memoryID].pop_back();
 
        memoryRefCount[handle.memoryID.memoryName]--;
 
        if (callbacks[handle.memoryID].empty())
        {
            callbacks.erase(handle.memoryID);
 
            // unsubscribe from memory topic if no remainig callback needs it
            if (component and memoryRefCount[handle.memoryID.memoryName] == 0)
            {
                component->unsubscribeFromTopic(MakeMemoryTopicName(handle.memoryID));
            }
        }
    }
 
 
} // namespace armarx::armem::client::util