Skip to content
Snippets Groups Projects
Commit 86768f87 authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Merge branch...

Merge branch '116-add-index-memory-for-listing-searching-filtering-spatial-and-named-entities' into 'master'

Resolve "Add Index memory for listing/searching/filtering Spatial and Named entities"

Closes #116

See merge request !292
parents a285b988 43846bcb
No related branches found
No related tags found
1 merge request!292Resolve "Add Index memory for listing/searching/filtering Spatial and Named entities"
Showing
with 1416 additions and 17 deletions
<?xml version="1.0" encoding="utf-8"?>
<scenario name="ArMemIndex" creation="2022-11-10.11:51:30" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="IndexMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="ObjectInstanceToIndex" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
# ==================================================================
# IndexMemory properties
# ==================================================================
# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
# ArmarX.AdditionalPackages = Default value not mapped.
# ArmarX.ApplicationName: Application name
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.ApplicationName = ""
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
# - Required: no
# ArmarX.CachePath = mongo/.cache
# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.Config = ""
# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.DataPath = ""
# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
# ArmarX.DefaultPackages = Default value not mapped.
# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
# Attributes:
# - Default: ./config/dependencies.cfg
# - Case sensitivity: yes
# - Required: no
# ArmarX.DependenciesConfig = ./config/dependencies.cfg
# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.DisableLogging = false
# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.EnableProfiling = false
# ArmarX.IndexMemory.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.IndexMemory.EnableProfiling = false
# ArmarX.IndexMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.IndexMemory.MinimumLoggingLevel = Undefined
# ArmarX.IndexMemory.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.ObjectName = ""
# ArmarX.IndexMemory.RemoteGuiName: Name of the remote gui provider
# Attributes:
# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.RemoteGuiName = RemoteGuiProvider
# ArmarX.IndexMemory.mem.MemoryName: Name of this memory server.
# Attributes:
# - Default: Index
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.mem.MemoryName = Index
# ArmarX.IndexMemory.mem.ltm.configuration:
# Attributes:
# - Default: {}
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.mem.ltm.configuration = {}
# ArmarX.IndexMemory.mem.ltm.enabled:
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.IndexMemory.mem.ltm.enabled = false
# ArmarX.IndexMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.IndexMemory.mns.MemoryNameSystemEnabled = true
# ArmarX.IndexMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.mns.MemoryNameSystemName = MemoryNameSystem
# ArmarX.IndexMemory.p.maxHistorySize: The maximum size of entity histories.
# Attributes:
# - Default: 1024
# - Case sensitivity: yes
# - Required: no
# ArmarX.IndexMemory.p.maxHistorySize = 1024
# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.LoadLibraries = ""
# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.LoggingGroup = ""
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.RedirectStdout = true
# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
# - Default: 3000
# - Case sensitivity: yes
# - Required: no
# ArmarX.RemoteHandlesDeletionTimeout = 3000
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
# ArmarX.SecondsStartupDelay = 0
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.StartDebuggerOnCrash = false
# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
# ArmarX.ThreadPoolSize = 1
# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.TopicSuffix = ""
# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.UseTimeServer = false
# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
# - Default: Info
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
# ==================================================================
# ObjectInstanceToIndex properties
# ==================================================================
# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
# ArmarX.AdditionalPackages = Default value not mapped.
# ArmarX.ApplicationName: Application name
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.ApplicationName = ""
# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config)
# Attributes:
# - Default: mongo/.cache
# - Case sensitivity: yes
# - Required: no
# ArmarX.CachePath = mongo/.cache
# ArmarX.Config: Comma-separated list of configuration files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.Config = ""
# ArmarX.DataPath: Semicolon-separated search list for data files
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.DataPath = ""
# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'.
# Attributes:
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
# ArmarX.DefaultPackages = Default value not mapped.
# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited.
# Attributes:
# - Default: ./config/dependencies.cfg
# - Case sensitivity: yes
# - Required: no
# ArmarX.DependenciesConfig = ./config/dependencies.cfg
# ArmarX.DisableLogging: Turn logging off in whole application
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.DisableLogging = false
# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.EnableProfiling = false
# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;...
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.LoadLibraries = ""
# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.LoggingGroup = ""
# ArmarX.ObjectInstanceToIndex.EnableProfiling: enable profiler which is used for logging performance events
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.ObjectInstanceToIndex.EnableProfiling = false
# ArmarX.ObjectInstanceToIndex.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.ObjectInstanceToIndex.MinimumLoggingLevel = Undefined
# ArmarX.ObjectInstanceToIndex.ObjectMemoryName: Name of the object memory.
# Attributes:
# - Default: ObjectMemory
# - Case sensitivity: yes
# - Required: no
# ArmarX.ObjectInstanceToIndex.ObjectMemoryName = ObjectMemory
# ArmarX.ObjectInstanceToIndex.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.ObjectInstanceToIndex.ObjectName = ""
# ArmarX.ObjectInstanceToIndex.RemoteGuiName: Name of the remote gui provider
# Attributes:
# - Default: RemoteGuiProvider
# - Case sensitivity: yes
# - Required: no
# ArmarX.ObjectInstanceToIndex.RemoteGuiName = RemoteGuiProvider
# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
# Set to false to use this memory as a stand-alone.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemEnabled = true
# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
# Attributes:
# - Default: MemoryNameSystem
# - Case sensitivity: yes
# - Required: no
# ArmarX.ObjectInstanceToIndex.mns.MemoryNameSystemName = MemoryNameSystem
# ArmarX.ObjectInstanceToIndex.p.object.maxFrequency:
# Attributes:
# - Default: 10
# - Case sensitivity: yes
# - Required: no
# ArmarX.ObjectInstanceToIndex.p.object.maxFrequency = 10
# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.RedirectStdout = true
# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles)
# Attributes:
# - Default: 3000
# - Case sensitivity: yes
# - Required: no
# ArmarX.RemoteHandlesDeletionTimeout = 3000
# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging)
# Attributes:
# - Default: 0
# - Case sensitivity: yes
# - Required: no
# ArmarX.SecondsStartupDelay = 0
# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger.
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.StartDebuggerOnCrash = false
# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running.
# Attributes:
# - Default: 1
# - Case sensitivity: yes
# - Required: no
# ArmarX.ThreadPoolSize = 1
# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes.
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
# ArmarX.TopicSuffix = ""
# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator)
# Attributes:
# - Default: false
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
# ArmarX.UseTimeServer = false
# ArmarX.Verbosity: Global logging level for whole application
# Attributes:
# - Default: Info
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
# ArmarX.Verbosity = Info
# ==================================================================
# Global Config from Scenario ArMemIndex
# ==================================================================
add_subdirectory(ExampleMemoryClient)
add_subdirectory(GraspProviderExample)
add_subdirectory(ObjectInstanceToIndex)
add_subdirectory(RobotStatePredictionClientExample)
add_subdirectory(SimpleVirtualRobot)
add_subdirectory(VirtualRobotReaderExampleClient)
......
armarx_component_set_name(ObjectInstanceToIndex)
set(COMPONENT_LIBS
# ArmarXCore
ArmarXCore
ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
# ArmarXCore
RobotAPI::armem_index
RobotAPI::armem_objects
RobotAPI::armem_robot_state
aroncommon
)
set(SOURCES
ObjectInstanceToIndex.cpp
impl/ObjectInstanceToIndex.cpp
)
set(HEADERS
ObjectInstanceToIndex.h
impl/ObjectInstanceToIndex.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
armarx_generate_and_add_component_executable()
/*
* 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/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>
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.timeCreated = 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
/*
* 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
*/
#pragma once
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/Frequency.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
#include "impl/ObjectInstanceToIndex.h"
namespace armarx
{
/**
* @defgroup Component-ObjectInstanceToIndex ObjectInstanceToIndex
* @ingroup RobotAPI-Components
*
* Links object instances in the "Index/Spatial" and "Index/Named" memory segments.
*
* @class ObjectInstanceToIndex
* @ingroup Component-ObjectInstanceToIndex
* @brief Brief description of class ObjectInstanceToIndex.
*
* Links object instances in the "Index/Spatial" and "Index/Named" memory segments.
*/
// Class is currently called like component for compatibility with legacy CMake.
class ObjectInstanceToIndex :
virtual public armarx::Component,
virtual public armarx::armem::ListeningClientPluginUser,
virtual public armarx::LightweightRemoteGuiComponentPluginUser
{
public:
ObjectInstanceToIndex();
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
void onExitComponent() override;
// LightweightRemoteGuiComponentPluginUser interface
public:
void createRemoteGuiTab();
void RemoteGui_update() override;
private:
void processRobotState(const armem::MemoryID& id,
const std::vector<armem::MemoryID>& snapshotIDs);
void processObjectInstance(const armem::MemoryID& id,
const std::vector<armem::MemoryID>& snapshotIDs);
private:
struct Properties
{
struct Object
{
float maxFrequencyHz = 10;
};
Object object;
};
Properties properties;
armem::client::Writer indexSpatialMemoryWriter;
armarx::plugins::ObjectPoseClientPlugin* objectClientPlugin = nullptr;
std::mutex objectMutex;
std::optional<armem::objects::ObjectInstanceToIndex> object;
armem::client::Reader robotMemoryReader;
// armarx::RunningTask<ObjectInstanceToIndex>::pointer_type task;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
};
RemoteGuiTab tab;
};
} // namespace armarx
/*
* 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::RobotAndObjectToIndex
* @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 <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
#include <SimoxUtility/shapes/OrientedBox.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/memory_ids.h>
#include <RobotAPI/libraries/armem_objects/utils.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/simox.h>
#include <RobotAPI/libraries/aron/common/aron_conversions/stl.h>
namespace armarx::armem::objects
{
void
ObjectInstanceToIndex::fetchAndCommitObjectInstances(
const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs)
{
ARMARX_CHECK(objectPoseClient.isConnected());
// Fetch the latest poses.
const objpose::ObjectPoseSeq objectPoses = objectPoseClient.fetchObjectPoses();
const auto filtered = filterObjectPoses(objectPoses, updatedObjectInstanceSnapshotIDs);
// Prepare the commit.
armem::Commit commit;
for (const objpose::ObjectPose* objectPosePtr : filtered)
{
const objpose::ObjectPose& objectPose = *objectPosePtr;
armem::MemoryID objectInstanceID =
armem::objects::reconstructObjectInstanceID(objectPose);
// Spatial
std::optional<simox::OrientedBoxf> oobb = objectPose.oobbGlobal();
if (oobb.has_value())
{
simox::AxisAlignedBoundingBox aabb =
simox::AxisAlignedBoundingBox::from_points(oobb->corners());
armem::index::arondto::Spatial spatial;
armem::toAron(spatial.id, objectInstanceID);
aron::toAron(spatial.oobbGlobal, oobb);
toAron(spatial.aabbGlobal, aabb);
armem::EntityUpdate& update = commit.add();
update.entityID = indexSpatialProviderSegmentID.withEntityName(
objectInstanceID.getEntityID().str());
update.timeCreated = objectPose.timestamp;
update.instancesData = {spatial.toAron()};
}
// Named
// Load object class information.
std::optional<ObjectInfo> info =
objectPoseClient.getObjectFinder().findObject(objectPose.objectID);
if (info.has_value())
{
std::optional<std::vector<std::string>> recognized, spoken;
recognized = info->loadRecognizedNames();
spoken = info->loadSpokenNames();
armem::index::arondto::Named named;
armem::toAron(named.id, objectInstanceID);
if (recognized.has_value())
{
named.names.recognized = recognized.value();
}
else
{
named.names.recognized = {info->className()};
}
if (spoken.has_value())
{
named.names.spoken = spoken.value();
}
else
{
named.names.spoken = {info->className()};
}
armem::EntityUpdate& update = commit.add();
update.entityID = indexNamedProviderSegmentID.withEntityName(
objectInstanceID.getEntityID().str());
update.timeCreated = objectPose.timestamp;
update.instancesData = {named.toAron()};
}
}
// Commit.
indexSpatialMemoryWriter.commit(commit);
}
std::vector<const objpose::ObjectPose*>
ObjectInstanceToIndex::filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
const std::vector<MemoryID>& updatedSnapshotIDs)
{
// Returns true to keep the item, false to skip it.
auto filter = [this, &updatedSnapshotIDs](const objpose::ObjectPose& objectPose)
{
auto it = state.latestUpdateDateTimes.find(objectPose.objectID);
if (it == state.latestUpdateDateTimes.end())
{
// Never encountered that before, commit it.
return true;
}
const armarx::DateTime& latestTime = it->second;
armarx::DateTime nextDueTime = latestTime + params.maxFrequency.toCycleDuration();
if (objectPose.timestamp < nextDueTime)
{
// Skip.
return false;
}
armem::MemoryID objectInstanceID =
armem::objects::reconstructObjectInstanceID(objectPose);
bool found = false;
for (const MemoryID& updatedSnapshotID : updatedSnapshotIDs)
{
if (armem::contains(updatedSnapshotID, objectInstanceID))
{
found = true;
break;
}
}
return found;
};
std::vector<const objpose::ObjectPose*> filtered;
for (const objpose::ObjectPose& objectPose : objectPoses)
{
if (filter(objectPose))
{
filtered.push_back(&objectPose);
state.latestUpdateDateTimes[objectPose.objectID] = objectPose.timestamp;
}
}
return filtered;
}
} // namespace armarx::armem::objects
/*
* 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::RobotAndObjectToIndex
* @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
*/
#pragma once
#include <vector>
#include <ArmarXCore/core/time/Frequency.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::objects
{
class ObjectInstanceToIndex
{
public:
void fetchAndCommitObjectInstances(
const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
private:
std::vector<const objpose::ObjectPose*>
filterObjectPoses(const objpose::ObjectPoseSeq& objectPoses,
const std::vector<armem::MemoryID>& updatedObjectInstanceSnapshotIDs);
public:
objpose::ObjectPoseClient objectPoseClient;
armem::client::Writer indexSpatialMemoryWriter;
armem::MemoryID indexSpatialProviderSegmentID;
armem::MemoryID indexNamedProviderSegmentID;
struct Parameters
{
armarx::Frequency maxFrequency = armarx::Frequency::HertzDouble(60);
};
Parameters params;
struct State
{
std::map<armarx::ObjectID, armarx::DateTime> latestUpdateDateTimes;
};
State state;
};
} // namespace armarx::armem::objects
add_subdirectory(ExampleMemory)
add_subdirectory(GeneralPurposeMemory)
add_subdirectory(GraspMemory)
add_subdirectory(IndexMemory)
add_subdirectory(MotionMemory)
add_subdirectory(ObjectMemory)
add_subdirectory(ReasoningMemory)
add_subdirectory(RobotStateMemory)
add_subdirectory(SkillsMemory)
add_subdirectory(GraspMemory)
#add_subdirectory(SubjectMemory)
add_subdirectory(MotionMemory)
add_subdirectory(SystemStateMemory)
armarx_component_set_name(IndexMemory)
set(COMPONENT_LIBS
# ArmarXCore
ArmarXCore ArmarXCoreInterfaces # for DebugObserverInterface
ArmarXGuiComponentPlugins
# RobotAPI
armem_index_server
)
set(SOURCES
IndexMemory.cpp
)
set(HEADERS
IndexMemory.h
)
armarx_add_component("${SOURCES}" "${HEADERS}")
# Generate the application
armarx_generate_and_add_component_executable()
/*
* 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::IndexMemory
* @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 "IndexMemory.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem_index/aron/Named.aron.generated.h>
#include <RobotAPI/libraries/armem_index/aron/Spatial.aron.generated.h>
#include <RobotAPI/libraries/armem_index/memory_ids.h>
namespace armarx
{
armarx::PropertyDefinitionsPtr
IndexMemory::createPropertyDefinitions()
{
armarx::PropertyDefinitionsPtr defs =
new ComponentPropertyDefinitions(getConfigIdentifier());
setMemoryName(armem::index::memoryID.memoryName);
defs->optional(properties.maxHistorySize, "p.maxHistorySize", "The maximum size of entity histories.")
.setMin(1);
return defs;
}
std::string
IndexMemory::getDefaultName() const
{
return "IndexMemory";
}
void
IndexMemory::onInitComponent()
{
size_t maxHistorySize = static_cast<size_t>(std::max(1, properties.maxHistorySize));
workingMemory()
.addCoreSegment(armem::index::namedSegmentID.coreSegmentName,
armem::index::arondto::Named::ToAronType())
.setMaxHistorySize(maxHistorySize);
workingMemory()
.addCoreSegment(armem::index::spatialSegmentID.coreSegmentName,
armem::index::arondto::Spatial::ToAronType())
.setMaxHistorySize(maxHistorySize);
}
void
IndexMemory::onConnectComponent()
{
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
}
void
IndexMemory::onDisconnectComponent()
{
}
void
IndexMemory::onExitComponent()
{
}
void
IndexMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
{
}
VBoxLayout root = {VSpacer()};
RemoteGui_createTab(getName(), root, &tab);
}
void
IndexMemory::RemoteGui_update()
{
}
} // namespace armarx
/*
* 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::IndexMemory
* @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
*/
#pragma once
#include <ArmarXCore/core/Component.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
namespace armarx
{
/**
* @defgroup Component-IndexMemory IndexMemory
* @ingroup RobotAPI-Components
* A description of the component IndexMemory.
*
* @class IndexMemory
* @ingroup Component-IndexMemory
* @brief Brief description of class IndexMemory.
*
* Detailed description of class IndexMemory.
*/
class IndexMemory :
virtual public armarx::Component,
virtual public armem::server::ReadWritePluginUser,
virtual public LightweightRemoteGuiComponentPluginUser
{
public:
std::string getDefaultName() const override;
// LightweightRemoteGuiComponentPluginUser interface
public:
void createRemoteGuiTab();
void RemoteGui_update() override;
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
void onConnectComponent() override;
void onDisconnectComponent() override;
void onExitComponent() override;
private:
struct Properties
{
int maxHistorySize = 1024;
};
Properties properties;
struct RemoteGuiTab : RemoteGui::Client::Tab
{
};
RemoteGuiTab tab;
};
} // namespace armarx
......@@ -170,6 +170,7 @@ module armarx
// useful for memory ice_conversions
sequence<Dict> AronDictSeq;
sequence<Dict> DictSeq;
};
};
};
......
......@@ -23,7 +23,7 @@ namespace armarx::plugins
void ObjectPoseClientPlugin::preOnConnectComponent()
{
parent<ObjectPoseClientPluginUser>().objectPoseStorage = createObjectPoseStorage();
_objectPoseStorage = createObjectPoseStorage();
}
objpose::ObjectPoseStorageInterfacePrx ObjectPoseClientPlugin::createObjectPoseStorage()
......@@ -31,6 +31,11 @@ namespace armarx::plugins
return parent<Component>().getProxyFromProperty<objpose::ObjectPoseStorageInterfacePrx>(makePropertyName(PROPERTY_NAME));
}
objpose::ObjectPoseClient ObjectPoseClientPlugin::createClient()
{
return objpose::ObjectPoseClient(createObjectPoseStorage(), getObjectFinder());
}
const ObjectFinder& ObjectPoseClientPlugin::setObjectFinderPath(const std::string& path)
{
_finder.setPath(path);
......@@ -58,23 +63,18 @@ namespace armarx
objpose::ObjectPoseClient ObjectPoseClientPluginUser::getClient() const
{
return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder());
return plugin->createClient();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPoses()
{
if (!objectPoseStorage)
{
ARMARX_WARNING << "No object pose observer.";
return {};
}
return objpose::fromIce(objectPoseStorage->getObjectPoses());
return getClient().fetchObjectPoses();
}
objpose::ObjectPoseSeq ObjectPoseClientPluginUser::getObjectPosesByProvider(const std::string& providerName)
{
return objpose::ObjectPoseClient(objectPoseStorage, getObjectFinder()).fetchObjectPosesFromProvider(providerName);
return getClient().fetchObjectPosesFromProvider(providerName);
}
......
......@@ -16,8 +16,8 @@ namespace armarx::plugins
using ComponentPlugin::ComponentPlugin;
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseClient createClient();
template<class...Ts>
std::optional<ObjectInfo> findObject(Ts&& ...ts) const
......@@ -43,12 +43,15 @@ namespace armarx::plugins
private:
void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
void preOnInitComponent() override;
void preOnConnectComponent() override;
static constexpr const char* PROPERTY_NAME = "ObjectMemoryName";
ObjectFinder _finder;
objpose::ObjectPoseStorageInterfacePrx _objectPoseStorage;
};
}
......@@ -71,12 +74,8 @@ namespace armarx
ObjectPoseClientPluginUser();
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseStorageInterfacePrx objectPoseStorage;
objpose::ObjectPoseClient getClient() const;
objpose::ObjectPoseStorageInterfacePrx createObjectPoseStorage();
objpose::ObjectPoseSeq getObjectPoses();
objpose::ObjectPoseSeq getObjectPosesByProvider(const std::string& providerName);
......
......@@ -21,6 +21,7 @@ add_subdirectory(aron_component_config)
add_subdirectory(armem)
add_subdirectory(armem_grasping)
add_subdirectory(armem_gui)
add_subdirectory(armem_index)
add_subdirectory(armem_locations)
add_subdirectory(armem_motions)
add_subdirectory(armem_mps)
......
set(LIB_NAME armem_index)
armarx_component_set_name("${LIB_NAME}")
armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
# ArmarXCore
ArmarXCore
# ArmarXGui
ArmarXGuiComponentPlugins
# RobotAPI
RobotAPI::ArViz
RobotAPI::armem
# RobotAPI::armem_robot
HEADERS
forward_declarations.h
aron_conversions.h
aron_forward_declarations.h
memory_ids.h
SOURCES
aron_conversions.cpp
memory_ids.cpp
ARON_FILES
aron/Named.xml
aron/Spatial.xml
)
add_library(${PROJECT_NAME}::armem_index ALIAS armem_index)
add_subdirectory(server)
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<AronIncludes>
<Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" />
<Include include="<RobotAPI/libraries/aron/common/aron/Names.xml>" />
</AronIncludes>
<GenerateTypes>
<Object name="armarx::armem::index::arondto::Named"
doc-brief="Index of a named entity in the memory.">
<ObjectChild key="id" doc-brief="Link to the memory element.">
<armarx::armem::arondto::MemoryID />
</ObjectChild>
<ObjectChild key="names" doc-brief="The entity's names.">
<armarx::arondto::Names />
</ObjectChild>
</Object>
</GenerateTypes>
</AronTypeDefinition>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment