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/time/Clock.h"
#include <ArmarXCore/core/ManagedIceObject.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/server/ltm/Memory.h>
#include <RobotAPI/libraries/armem_objects/aron/Marker.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::armem::server::obj::ObjectMemory, "ObjectMemory");
 
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.");
        familiar_object_instance::SegmentAdapter::defineProperties(defs, prefix + "fam_obj_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()),
        familiar_object_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();
        familiar_object_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);
 
        // Pass the debug observer to the long-term memory for timing measurements
        if (debugObserver)
        {
            longtermMemory().setDebugObserver(debugObserver);
            ARMARX_INFO << "Debug observer set for LTM timing measurements";
        }
 
        // Create first to use the original values.
        createRemoteGuiTab();
 
        // ARMARX_CHECK(virtualRobotReaderPlugin->isAvailable());
 
        instance::SegmentAdapter::connect(&virtualRobotReaderPlugin->get(),
                                          kinematicUnitObserver,
                                          ArVizComponentPluginUser::getArvizClient(),
                                          debugObserver);
 
        familiar_object_instance::SegmentAdapter::connect(
            std::experimental::make_observer(&virtualRobotReaderPlugin->get()),
            ArVizComponentPluginUser::getArvizClient(),
            debugObserver);
 
        classSegment.connect(ArVizComponentPluginUser::getArvizClient());
 
        attachmentSegment.connect();
 
        RemoteGui_startRunningTask();
    }
 
    void
    ObjectMemory::onDisconnectComponent()
    {
        instance::SegmentAdapter::disconnect();
        // FIXME
        // familiar_object_instance::SegmentAdapter::disconnect();
        // classSegment.disconnect();
        // attachmentSegment.disconnect();
    }
 
    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();
        }
    }
 
    void
    ObjectMemory::reloadKnownObjectClasses(const Ice::Current& current)
    {
        ARMARX_INFO << "Reloading object classes ...";
        classSegment.reloadObjectClassesByObjectFinder();
        ARMARX_INFO << "Reloaded object classes.";
    }
 
 
} // namespace armarx::armem::server::obj