ObjectInstanceToIndex.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::ObjectInstanceToIndex
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectInstanceToIndex.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/math/pose/pose.h>
#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/operations.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
#include <RobotAPI/libraries/armem_index/memory_ids.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_robot_state/aron/RobotState.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/memory_ids.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
ARMARX_REGISTER_COMPONENT_EXECUTABLE(armarx::ObjectInstanceToIndex, "ObjectInstanceToIndex");
 
namespace armarx
{
 
    armarx::PropertyDefinitionsPtr
    ObjectInstanceToIndex::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr defs =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        defs->optional(properties.object.maxFrequencyHz, "p.object.maxFrequency");
 
        return defs;
    }
 
    ObjectInstanceToIndex::ObjectInstanceToIndex()
    {
        addPlugin(objectClientPlugin);
    }
 
    std::string
    ObjectInstanceToIndex::getDefaultName() const
    {
        return "ObjectInstanceToIndex";
    }
 
    void
    ObjectInstanceToIndex::onInitComponent()
    {
        using This = ObjectInstanceToIndex;
        // This should not be necessary but seems to be. ToDo: Look into this.
        memoryNameSystem().setComponent(this);
 
        memoryNameSystem().subscribe(armem::robot_state::memoryID, this, &This::processRobotState);
        memoryNameSystem().subscribe(
            armem::objects::instaceSegmentID, this, &This::processObjectInstance);
    }
 
    void
    ObjectInstanceToIndex::onConnectComponent()
    {
        try
        {
            indexSpatialMemoryWriter = memoryNameSystem().useWriter(armem::index::spatialSegmentID);
        }
        catch (const armem::error::CouldNotResolveMemoryServer& e)
        {
            ARMARX_ERROR << e.what();
        }
 
        {
            createRemoteGuiTab();
            RemoteGui_startRunningTask();
        }
    }
 
    void
    ObjectInstanceToIndex::onDisconnectComponent()
    {
    }
 
    void
    ObjectInstanceToIndex::onExitComponent()
    {
    }
 
    void
    ObjectInstanceToIndex::createRemoteGuiTab()
    {
        using namespace armarx::RemoteGui::Client;
 
        VBoxLayout root = {VSpacer()};
        RemoteGui_createTab(getName(), root, &tab);
    }
 
    void
    ObjectInstanceToIndex::RemoteGui_update()
    {
    }
 
    void
    ObjectInstanceToIndex::processRobotState(const armem::MemoryID& id,
                                             const std::vector<armem::MemoryID>& snapshotIDs)
    {
#if 0
        if (not robotMemoryReader)
        {
            try
            {
                robotMemoryReader = memoryNameSystem().getReader(objectInstanceSegmentID);
            }
            catch (const armem::error::CouldNotResolveMemoryServer& e)
            {
                ARMARX_INFO << e.what();
                return;
            }
        }
        ARMARX_CHECK(robotMemoryReader);
 
        // Get only the latest snapshot per entity.
        std::map<armem::MemoryID, const armem::MemoryID*> entityToSnapshot;
        for (const armem::MemoryID& snapshotID : snapshotIDs)
        {
            const armem::MemoryID entityID = snapshotID.getEntityID();
            if (auto it = entityToSnapshot.find(entityID); it != entityToSnapshot.end())
            {
                if (it->second->timestamp < snapshotID.timestamp)
                {
                    entityToSnapshot[entityID] = &snapshotID;
                }
            }
            else
            {
                entityToSnapshot[entityID] = &snapshotID;
            }
        }
 
        // Query them.
        std::vector<armem::MemoryID> queryIDs;
        for (const auto& [_, snapshotID] : entityToSnapshot)
        {
            queryIDs.push_back(*snapshotID);
        }
 
        armem::client::QueryResult result = robotMemoryReader.queryMemoryIDs(queryIDs);
        if (result.success)
        {
            // Prepare the commit.
 
            armem::MemoryID provSegID = indexSpatialSegmentID.withProviderSegmentName(getName());
 
            armem::Commit commit;
 
            result.memory.forEachInstance(
                [&commit, &provSegID](const armem::wm::EntityInstance& instance)
            {
                const armem::arondto::ObjectInstance data = instance.dataAs<armem::arondto::ObjectInstance>();
 
                armem::index::arondto::Spatial spatial;
                armem::toAron(spatial.id, instance.id());
                spatial.oobbGlobal;
                spatial.aabbGlobal;
 
                armem::EntityUpdate& update = commit.add();
                update.entityID = provSegID.withEntityName(instance.id().str());
                update.referencedTime = instance.id().timestamp;
                update.instancesData = { spatial.toAron() };
 
                return true;
            });
        }
        else
        {
            ARMARX_INFO << result.errorMessage;
        }
#endif
    }
 
    void
    ObjectInstanceToIndex::processObjectInstance(const armem::MemoryID& id,
                                                 const std::vector<armem::MemoryID>& snapshotIDs)
    {
        std::scoped_lock lock(objectMutex);
        if (not object.has_value())
        {
            object = armem::objects::ObjectInstanceToIndex{
                .objectPoseClient = objectClientPlugin->createClient(),
                .indexSpatialMemoryWriter = indexSpatialMemoryWriter,
                .indexSpatialProviderSegmentID =
                    armem::index::spatialSegmentID.withProviderSegmentName(getName()),
                .indexNamedProviderSegmentID =
                    armem::index::namedSegmentID.withProviderSegmentName(getName()),
                .params =
                    armem::objects::ObjectInstanceToIndex::Parameters{
                        .maxFrequency = armarx::Frequency::Hertz(properties.object.maxFrequencyHz)},
                .state = {}};
        }
        ARMARX_CHECK(object.has_value());
 
        object->fetchAndCommitObjectInstances(snapshotIDs);
    }
 
} // namespace armarx