RequestObjects.cpp

Go to the documentation of this file.

#include "RequestObjects.h"
 
//#include <ArmarXCore/core/time/TimeUtil.h>
//#include <ArmarXCore/observers/variant/DatafieldRef.h>
 
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem_objects/client/instance/ObjectReader.h>
 
#include "ObjectMemoryGroupStatechartContext.generated.h"
 
namespace armarx::ObjectMemoryGroup
{
    // DO NOT EDIT NEXT LINE
    RequestObjects::SubClassRegistry RequestObjects::Registry(RequestObjects::GetName(),
                                                              &RequestObjects::CreateInstance);
 
    RequestObjects::RequestObjects(const XMLStateConstructorParams& stateData) :
        XMLStateTemplate<RequestObjects>(stateData),
        RequestObjectsGeneratedBase<RequestObjects>(stateData)
    {
    }
 
    void
    RequestObjects::onEnter()
    {
 
        // put your user code for the enter-point here
        // execution time should be short (<100ms)
    }
 
    void
    RequestObjects::run()
    {
        if (not in.getEnable())
        {
            emitSuccess();
            return;
        }
 
        using Reader = armarx::armem::obj::instance::Reader;
 
        const std::string provider = in.isProviderSet() ? in.getProvider() : "";
 
        const std::vector<std::string> objectIdsString = in.getObjectIds();
        const armarx::Duration relativeTimeout =
            armarx::Duration::MilliSeconds(in.getRelativeTimeoutMilliseconds());
 
        std::stringstream info;
        std::stringstream warn;
 
        std::vector<armarx::ObjectID> objectIds;
        for (const std::string& idString : objectIdsString)
        {
            try
            {
                armarx::ObjectID id = armarx::ObjectID::FromString(idString);
                objectIds.push_back(id);
                info << "Requesting object " << id << "\n";
            }
            catch (const armarx::LocalException& e)
            {
                warn << "\nGiven object ID '" << idString
                     << "' could not parsed as ObjectID: " << e.what();
            }
        }
 
        auto context = getContext<ObjectMemoryGroupStatechartContext>();
        armarx::armem::client::MemoryNameSystem mns(getMemoryNameSystem(), context);
 
        Reader reader;
        reader.connect(mns);
        objpose::ObjectPoseStorageInterfacePrx storage = reader.getObjectPoseStorage();
 
        armarx::objpose::observer::RequestObjectsInput input;
        input.provider = provider;
        for (const armarx::ObjectID& id : objectIds)
        {
            armarx::toIce(input.request.objectIDs.emplace_back(), id);
        }
        input.request.relativeTimeoutMS = relativeTimeout.toMilliSeconds();
 
 
        objpose::observer::RequestObjectsOutput output;
        try
        {
            output = storage->requestObjects(input);
        }
        catch (const Ice::Exception& e)
        {
            ARMARX_WARNING << "Failed to request object localization: " << e.what();
            emitFailure();
            return;
        }
 
        for (const auto& [id, result] : output.results)
        {
            if (result.result.success)
            {
                info << "Requested object " << id << " via provider '" << result.providerName
                     << "'.\n";
            }
            else
            {
                warn << "\nFailed to request the object " << id << " for localization.";
            }
        }
 
        if (not info.str().empty())
        {
            ARMARX_INFO << info.str();
        }
        if (not warn.str().empty())
        {
            ARMARX_WARNING
                << "The following issues occurred whhen requesting objects for localization:"
                << warn.str();
        }
 
        emitSuccess();
    }
 
    //void RequestObjects::onBreak()
    //{
    //    // put your user code for the breaking point here
    //    // execution time should be short (<100ms)
    //}
 
    void
    RequestObjects::onExit()
    {
        // put your user code for the exit point here
        // execution time should be short (<100ms)
    }
 
    // DO NOT EDIT NEXT FUNCTION
    XMLStateFactoryBasePtr
    RequestObjects::CreateInstance(XMLStateConstructorParams stateData)
    {
        return XMLStateFactoryBasePtr(new RequestObjects(stateData));
    }
} // namespace armarx::ObjectMemoryGroup