ObjectMemory.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::ObjectMemory
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectMemory.h"
#include <VirtualRobot/VirtualRobot.h>
 
#include "ArmarXCore/core/time/Clock.h"
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/Prediction.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
 
 
namespace armarx::armem::server::obj
{
 
    armarx::PropertyDefinitionsPtr ObjectMemory::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs(new ComponentPropertyDefinitions(getConfigIdentifier()));
 
        const std::string prefix = "mem.";
        workingMemory().name() = objects::memoryID.memoryName;
 
        classSegment.defineProperties(defs, prefix + "cls.");
        instance::SegmentAdapter::defineProperties(defs, prefix + "inst.");
 
        attachmentSegment.defineProperties(defs, prefix + "attachments.");
 
 
        // Publish
        defs->topic(debugObserver);
 
        // Subscribe
        defs->topic<objpose::ObjectPoseTopic>(); // "ObjectPoseTopic", "ObjectPoseTopicName", "Name of the Object Pose Topic.");
 
        // Use
        // defs->component(kinematicUnitObserver);  // Optional dependency.
        defs->defineOptionalProperty<std::string>("cmp.KinematicUnitObserverName", "KinematicUnitObserver",
                "Name of the kinematic unit observer.");
        defs->optional(predictionTimeWindow, "prediction.TimeWindow",
                       "Duration of time window into the past to use for predictions"
                       " when requested via the PredictingMemoryInterface (in seconds).");
 
        defs->optional(p.markerMemoryName, prefix + ".marker.Name", "Marker Memory Name");
        defs->optional(p.maxMarkerHistorySize, prefix + ".marker.maxHistorySize", "Maximum marker memory history size");
 
        return defs;
    }
 
 
    ObjectMemory::ObjectMemory() :
        instance::SegmentAdapter(iceAdapter()),
        classSegment(iceAdapter()),
        attachmentSegment(iceAdapter()), virtualRobotReaderPlugin(nullptr)
    {
        addPlugin(virtualRobotReaderPlugin);
    }
 
 
    ObjectMemory::~ObjectMemory()
    {
    }
 
 
    std::string ObjectMemory::getDefaultName() const
    {
        return "ObjectMemory";
    }
 
 
    void ObjectMemory::onInitComponent()
    {
        const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
        {
            try
            {
                fn();
            }
            catch (const LocalException& e)
            {
                ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
            }
            catch (const std::exception& e)
            {
                ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
            }
            catch (...)
            {
                ARMARX_ERROR << "Failed to init " << segmentName << " segment for unknown reason.";
            }
        };
 
        // Class segment needs to be initialized before instance segment,
        // as the instance segment may refer to and search through the class segment.
        initSegmentWithCatch("class", [&]()
        {
            classSegment.init();
        });
 
        instance::SegmentAdapter::init();
 
        initSegmentWithCatch("attachment", [&]()
        {
            attachmentSegment.init();
        });
 
        initSegmentWithCatch(p.markerMemoryName, [&]()
        {
            workingMemory()
              .addCoreSegment(p.markerMemoryName, arondto::Marker::ToAronType())
              .setMaxHistorySize(p.maxMarkerHistorySize);
        });
    }
 
 
    void ObjectMemory::onConnectComponent()
    {
        ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
        
        getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
 
        // Create first to use the original values.
        createRemoteGuiTab();
 
        // ARMARX_CHECK(virtualRobotReaderPlugin->isAvailable());
 
        instance::SegmentAdapter::connect(
            &virtualRobotReaderPlugin->get(),
            kinematicUnitObserver,
            ArVizComponentPluginUser::getArvizClient(),
            debugObserver
        );
        classSegment.connect(
            ArVizComponentPluginUser::getArvizClient()
        );
 
        attachmentSegment.connect();
 
        RemoteGui_startRunningTask();
    }
 
 
    void ObjectMemory::onDisconnectComponent()
    {
    }
 
 
    void ObjectMemory::onExitComponent()
    {
    }
 
    armem::actions::GetActionsOutputSeq ObjectMemory::getActions(
            const armem::actions::GetActionsInputSeq& inputs)
    {
        using namespace armem::actions;
        GetActionsOutputSeq outputs;
        for (const auto& input : inputs)
        {
            auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
            if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
                and memoryID.hasEntityName() and not memoryID.hasGap())
            {
                Menu menu {
                    Action{"vis", "Visualize object"},
                };
                // For strange C++ reasons, this works, but emplace_back doesn't.
                outputs.push_back({ menu.toIce() });
            }
        }
 
        return outputs;
    }
 
    armem::actions::ExecuteActionOutputSeq ObjectMemory::executeActions(
            const armem::actions::ExecuteActionInputSeq& inputs)
    {
        using namespace armem::actions;
        ExecuteActionOutputSeq outputs;
        
        for (const auto& input : inputs)
        {
            auto memoryID = armarx::fromIce<armem::MemoryID>(input.id);
            if (input.actionPath == ActionPath{"vis"})
            {
                if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
                    and memoryID.hasEntityName() and not memoryID.hasGap())
                {
                    classSegment.visualizeClass(memoryID);
                    outputs.emplace_back(true, "");
                }
                else
                {
                    std::stringstream sstream;
                    sstream << "MemoryID " << memoryID
                            << " does not refer to a valid object class.";
                    outputs.emplace_back(false, sstream.str());
                }
            }
            else
            {
                std::stringstream sstream;
                sstream << "Action path " << input.actionPath << " is not defined.";
                outputs.emplace_back(false, sstream.str());
            }
        }
        return outputs;
    }
 
    armem::prediction::data::PredictionResultSeq
    ObjectMemory::predict(const armem::prediction::data::PredictionRequestSeq& requests)
    {
        std::vector<armem::prediction::data::PredictionResult> results;
        for (const auto& request : requests)
        {
            auto boRequest = armarx::fromIce<armem::PredictionRequest>(request);
            armem::PredictionResult result;
            result.snapshotID = boRequest.snapshotID;
            if (armem::contains(workingMemory().id().withCoreSegmentName("Instance"), boRequest.snapshotID)
                    and not boRequest.snapshotID.hasGap()
                    and boRequest.snapshotID.hasTimestamp())
            {
                objpose::ObjectPosePredictionRequest objPoseRequest;
                toIce(objPoseRequest.timeWindow, Duration::SecondsDouble(predictionTimeWindow));
                objPoseRequest.objectID = toIce(ObjectID(request.snapshotID.entityName));
                objPoseRequest.settings = request.settings;
                toIce(objPoseRequest.timestamp, boRequest.snapshotID.timestamp);
 
                objpose::ObjectPosePredictionResult objPoseResult =
                    predictObjectPoses({objPoseRequest}).at(0);
                result.success = objPoseResult.success;
                result.errorMessage = objPoseResult.errorMessage;
 
                if (objPoseResult.success)
                {
                    armem::client::QueryBuilder builder;
                    builder.latestEntitySnapshot(boRequest.snapshotID);
                    auto queryResult = armarx::fromIce<armem::client::QueryResult>(
                        query(builder.buildQueryInputIce()));
                    std::string instanceError =
                        "Could not find instance '" + boRequest.snapshotID.str() + "' in memory";
                    if (!queryResult.success)
                    {
                        result.success = false;
                        result.errorMessage << instanceError << ":\n" << queryResult.errorMessage;
                    }
                    else
                    {
                        if (not boRequest.snapshotID.hasInstanceIndex())
                        {
                            boRequest.snapshotID.instanceIndex = 0;
                        }
                        auto* aronInstance = queryResult.memory.findLatestInstance(
                            boRequest.snapshotID, boRequest.snapshotID.instanceIndex);
                        if (aronInstance == nullptr)
                        {
                            result.success = false;
                            result.errorMessage << instanceError << ": No latest instance found.";
                        }
                        else
                        {
                            auto instance =
                                armem::arondto::ObjectInstance::FromAron(aronInstance->data());
                            objpose::toAron(
                                instance.pose,
                                armarx::fromIce<objpose::ObjectPose>(objPoseResult.prediction));
                            result.prediction = instance.toAron();
                        }
                    }
                }
            }
            else
            {
                result.success = false;
                result.errorMessage << "No predictions are supported for MemoryID "
                                    << boRequest.snapshotID;
            }
            results.push_back(result.toIce());
        }
 
        return results;
    }
 
    void ObjectMemory::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        tab.reset(new RemoteGuiTab);
 
        tab->instance.setup(*this);
        tab->clazz.setup(classSegment);
 
        HBoxLayout segments =
        {
            tab->instance.group,
            tab->clazz.group
        };
        VBoxLayout root =
        {
            segments,
            VSpacer()
        };
        RemoteGui_createTab(Component::getName(), root, tab.get());
    }
 
 
    void ObjectMemory::RemoteGui_update()
    {
        tab->instance.update(*this);
        tab->clazz.update(classSegment);
 
        if (tab->clazz.data.rebuild)
        {
            createRemoteGuiTab();
        }
    }
 
}