From 3abd69b6a93f9adedfc19be0d8e296effb508fff Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 May 2021 15:26:14 +0200
Subject: [PATCH] ArticulatedObjectLocalizer now working properly
---
.../ArMemObjectMemory/ArMemObjectMemory.scx | 1 +
.../ArticulatedObjectLocalizerExample.cfg | 246 ++++++++++++++++++
.../ArMemObjectMemory/config/ObjectMemory.cfg | 25 +-
.../ArticulatedObjectLocalizerExample.cpp | 48 +++-
.../ArticulatedObjectLocalizerExample.h | 6 +
.../client/articulated_object/Reader.cpp | 134 ++++++++++
.../client/articulated_object/Reader.h | 10 +-
.../client/articulated_object/Writer.cpp | 123 ++++++++-
.../client/articulated_object/Writer.h | 13 +-
.../articulated_object_class/Segment.cpp | 2 +-
10 files changed, 570 insertions(+), 38 deletions(-)
create mode 100644 scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 95ec0bdda..e6f93e12c 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -9,5 +9,6 @@
<application name="RobotStateComponent" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RobotToArVizApp" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
new file mode 100644
index 000000000..b9d70f0b4
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -0,0 +1,246 @@
+# ==================================================================
+# ArticulatedObjectLocalizerExample 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.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.EnableProfiling = false
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.ObjectName = ""
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
+# Attributes:
+# - Default: ArticulatedObjectClass
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
+# Attributes:
+# - Default: Object
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName = Object
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
+# Attributes:
+# - Default: ArmarXObjects
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ArmarXObjects
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.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.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled = true
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component.
+# Attributes:
+# - Default: MemoryNameSystem
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver: Name of the `DebugObserver` topic to publish data to.
+# Attributes:
+# - Default: DebugObserver
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver = DebugObserver
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.tpc.sub.MemoryListener = MemoryUpdates
+
+
+# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_USER_CONFIG_DIR is set, the cache path will be made relative to ARMARX_USER_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${HOME}/.armarx)
+# 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.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
+
+
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index 67e86da4a..77b3f690f 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -7,7 +7,7 @@
# - Default: Default value not mapped.
# - Case sensitivity: yes
# - Required: no
-# ArmarX.AdditionalPackages = Default value not mapped.
+ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ApplicationName: Application name
@@ -174,14 +174,13 @@
# ArmarX.ObjectMemory.mem.articulated.cls.CoreSegmentName = ArticulatedObjectClass
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage: If true, load the objects from the objects package on startup.
# Attributes:
# - Default: true
# - Case sensitivity: yes
# - Required: no
# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.cls.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.cls.LoadFromObjectsPackage = true
# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
@@ -192,22 +191,20 @@
# ArmarX.ObjectMemory.mem.articulated.cls.MaxHistorySize = -1
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArticulatedObjectInstance
+# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached: If true, no new snapshots are stored while an object is attached to a robot node.
-# If false, new snapshots are stored, but the attachment is kept in the new snapshots.
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
# Attributes:
-# - Default: true
+# - Default: ArticulatedObjectInstance
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.articulated.inst.DiscardSnapshotsWhileAttached = true
+# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName = ArticulatedObjectInstance
# ArmarX.ObjectMemory.mem.articulated.inst.MaxHistorySize: Maximal size of object poses history (-1 for infinite).
@@ -278,12 +275,12 @@
# ArmarX.ObjectMemory.mem.cls.MaxHistorySize = -1
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage: Name of the objects package to load from.
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
# - Default: ArmarXObjects
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackgage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 923eda4d4..5c7aba617 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -20,6 +20,8 @@
*/
#include "ArticulatedObjectLocalizerExample.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem_objects/types.h"
#include <memory>
@@ -46,7 +48,8 @@
namespace armarx::articulated_object
{
ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)) {}
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(*this)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(*this)) {}
armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions()
{
@@ -58,6 +61,7 @@ namespace armarx::articulated_object
// defs->optional(memoryName, "mem.MemoryName", "Name of the memory to use.");
articulatedObjectWriter->registerPropertyDefinitions(defs);
+ articulatedObjectReader->registerPropertyDefinitions(defs);
return defs;
}
@@ -72,6 +76,7 @@ namespace armarx::articulated_object
void ArticulatedObjectLocalizerExample::onConnectComponent()
{
articulatedObjectWriter->connect();
+ articulatedObjectReader->connect();
task = new RunningTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run);
task->start();
@@ -84,17 +89,31 @@ namespace armarx::articulated_object
void ArticulatedObjectLocalizerExample::onExitComponent() {}
- VirtualRobot::RobotPtr createDishwasher()
+ VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
{
- const std::string xml =
- "./ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml";
+ const std::string dishwasherName = "CupboardWithDishwasher";
- return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(xml), VirtualRobot::RobotIO::eStructure);
+ const auto descriptions = articulatedObjectReader->queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Found " << descriptions.size() << " articulated object descriptions";
+
+ const auto it = std::find_if(descriptions.begin(), descriptions.end(), [&](const armem::articulated_object::ArticulatedObjectDescription & desc) -> bool
+ {
+ return desc.name == dishwasherName;
+ });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << dishwasherName << " not (yet) available";
+ return nullptr;
+ }
+
+ return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
}
armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj, const armem::Time& timestamp)
{
- ARMARX_INFO << "Filename is " << obj.getFilename();
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
return
armem::articulated_object::ArticulatedObject
@@ -117,7 +136,8 @@ namespace armarx::articulated_object
{
ARMARX_IMPORTANT << "Running example.";
- std::shared_ptr<VirtualRobot::Robot> dishwasher = createDishwasher();
+ std::shared_ptr<VirtualRobot::Robot> dishwasher;
+
CycleUtil cycle(IceUtil::Time::milliSeconds(100));
IceUtil::Time start = TimeUtil::GetTime();
@@ -126,7 +146,19 @@ namespace armarx::articulated_object
while (not task->isStopped())
{
- ARMARX_INFO << "Reporting articulated objects";
+
+ if (dishwasher == nullptr)
+ {
+ dishwasher = createDishwasher();
+ }
+
+ if (dishwasher == nullptr) // still
+ {
+ c.waitForCycleDuration();
+ continue;
+ }
+
+ ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
const float t = float((now - start).toSecondsDouble());
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index ade1e0547..9c6edb32e 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -15,6 +15,8 @@
#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
#include <RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h>
+#include <VirtualRobot/VirtualRobot.h>
namespace armarx::articulated_object
{
@@ -54,11 +56,15 @@ namespace armarx::articulated_object
private:
+ VirtualRobot::RobotPtr createDishwasher();
+
+
armarx::RunningTask<ArticulatedObjectLocalizerExample>::pointer_type task;
armarx::DebugObserverInterfacePrx debugObserver;
std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
};
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 91b9337bc..1feed487f 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -8,6 +8,7 @@
#include "RobotAPI/libraries/armem/core/Time.h"
#include "RobotAPI/libraries/armem/client/query/Builder.h"
+#include "RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h"
#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
#include "RobotAPI/libraries/armem_robot/aron_conversions.h"
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
@@ -20,6 +21,82 @@ namespace armarx::armem::articulated_object
Reader::Reader(armem::ClientReaderComponentPluginUser& component) : component(component) {}
+ void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+ {
+ ARMARX_DEBUG << "Reader: registerPropertyDefinitions";
+
+ const std::string prefix = propertyPrefix;
+
+ def->optional(properties.memoryName, prefix + "MemoryName");
+
+ def->optional(properties.coreInstanceSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object instances.");
+ def->optional(properties.coreClassSegmentName,
+ prefix + "CoreSegment",
+ "Name of the memory core segment to use for object classes.");
+ def->optional(properties.providerName, prefix + "ProviderName");
+ }
+
+
+ void Reader::connect()
+ {
+ // Wait for the memory to become available and add it as dependency.
+ ARMARX_IMPORTANT << "Reader: Waiting for memory '" << properties.memoryName << "' ...";
+ auto result = component.useMemory(properties.memoryName);
+ if (not result.success)
+ {
+ ARMARX_ERROR << result.errorMessage;
+ return;
+ }
+
+ ARMARX_IMPORTANT << "Reader: Connected to memory '" << properties.memoryName;
+
+ memoryReader.setReadingMemory(result.proxy);
+
+ armem::MemoryID id = armem::MemoryID();
+ id.memoryName = properties.memoryName;
+ id.coreSegmentName = properties.coreClassSegmentName;
+ // listen to all provider segments!
+
+ memoryReader.subscribe(id, this, &Reader::updateKnownObjects);
+ }
+
+ void Reader::updateKnownObject(const armem::MemoryID& snapshotId)
+ {
+ // const std::string& nameWithDataset = snapshotId.providerSegmentName;
+
+ // arondto::RobotDescription aronArticulatedObjectDescription;
+ // aronArticulatedObjectDescription.fromAron(snapshotId.);
+
+ // TODO(fabian.reister): implement
+ }
+
+ void Reader::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ {
+ ARMARX_INFO << "New objects available!";
+
+ // // Query all entities from provider.
+ // armem::client::query::Builder qb;
+
+ // // clang-format off
+ // qb
+ // .coreSegments().withName(properties.coreClassSegmentName)
+ // .providerSegments().all() // TODO(fabian.reister): think about this: which authority is trustworthy?
+ // .entities().withName(name)
+ // .snapshots().atTime(timestamp);
+ // // clang-format on
+
+ // const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+
+ // std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
+ // {
+ // updateKnownObject(snapshotID);
+ // });
+ }
+
+
std::optional<ArticulatedObject> Reader::get(const std::string& name, const armem::Time& timestamp)
{
const auto description = queryDescription(name, timestamp);
@@ -63,6 +140,31 @@ namespace armarx::armem::articulated_object
obj.config = std::move(*state);
}
+ std::vector<robot::RobotDescription> Reader::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
std::optional<robot::RobotDescription> Reader::queryDescription(const std::string& name, const armem::Time& timestamp)
{
// Query all entities from provider.
@@ -176,4 +278,36 @@ namespace armarx::armem::articulated_object
return robot::convertRobotDescription(instance);
}
+ std::vector<robot::RobotDescription> Reader::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::vector<robot::RobotDescription> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ descriptions.push_back(*robotDescription);
+ }
+ }
+ }
+
+ return descriptions;
+ }
+
+
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
index 2e2403ec9..21a4591cd 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -33,12 +33,14 @@ namespace armarx::armem::articulated_object
{
class Reader:
virtual public ReaderInterface
- // virtual public ::armarx::armem::MemoryConnector
{
public:
Reader(armem::ClientReaderComponentPluginUser& component);
virtual ~Reader() = default;
+ void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+ void connect();
+
void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
@@ -47,21 +49,25 @@ namespace armarx::armem::articulated_object
std::optional<robot::RobotState> queryState(const robot::RobotDescription& description, const armem::Time& timestamp);
std::optional<robot::RobotDescription> queryDescription(const std::string& name, const armem::Time& timestamp);
+ std::vector<robot::RobotDescription> queryDescriptions(const armem::Time& timestamp);
// TODO(fabian.reister): register property defs
protected:
std::optional<robot::RobotState> getRobotState(const armarx::armem::wm::Memory& memory) const;
std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::vector<robot::RobotDescription> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
private:
+ void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObject(const armem::MemoryID& snapshotId);
struct Properties
{
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string providerName = "ArmarXObjects";
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
index 97b2ce981..703ef91aa 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,5 +1,7 @@
#include "Writer.h"
+#include <IceUtil/Time.h>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
@@ -11,6 +13,7 @@
#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include "RobotAPI/libraries/armem_robot/robot_conversions.h"
namespace armarx::armem::articulated_object
@@ -21,8 +24,6 @@ namespace armarx::armem::articulated_object
{
ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
- // MemoryConnector::registerPropertyDefinitions(def);
-
const std::string prefix = propertyPrefix;
def->optional(properties.memoryName, prefix + "MemoryName");
@@ -33,7 +34,7 @@ namespace armarx::armem::articulated_object
def->optional(properties.coreClassSegmentName,
prefix + "CoreSegment",
"Name of the memory core segment to use for object classes.");
- def->required(properties.providerName, prefix + "ProviderName");
+ def->optional(properties.providerName, prefix + "ProviderName");
}
void Writer::connect()
@@ -62,6 +63,7 @@ namespace armarx::armem::articulated_object
armem::MemoryID id;
id.setCoreSegmentID(refId); // listen to all provider segments!
+ updateKnownObjects();
memoryReader.subscribe(id, this, &Writer::updateKnownObjects);
}
@@ -77,11 +79,12 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
+ updateKnownObjects();
+ }
- std::for_each(snapshotIDs.begin(), snapshotIDs.end(), [&](const auto & snapshotID)
- {
- updateKnownObject(snapshotID);
- });
+ void Writer::updateKnownObjects()
+ {
+ knownObjects = queryDescriptions(IceUtil::Time::now());
}
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -95,7 +98,12 @@ namespace armarx::armem::articulated_object
}
// otherwise create
- return storeClass(obj);
+ if (properties.allowClassCreation)
+ {
+ return storeClass(obj);
+ }
+
+ return std::nullopt;
}
std::optional<armem::MemoryID> Writer::storeClass(const ArticulatedObject& obj)
@@ -180,7 +188,7 @@ namespace armarx::armem::articulated_object
if (not descriptionId)
{
- ARMARX_ERROR << "Could not get class for object " << obj.description.name;
+ ARMARX_WARNING << "Could not get class for object " << obj.description.name;
return false;
}
@@ -197,7 +205,7 @@ namespace armarx::armem::articulated_object
if (not updateResult.success)
{
- ARMARX_ERROR << updateResult.errorMessage;
+ ARMARX_WARNING << updateResult.errorMessage;
}
return updateResult.success;
@@ -209,7 +217,8 @@ namespace armarx::armem::articulated_object
if (not classId)
{
- ARMARX_WARNING << "Could not get class id!";
+ ARMARX_WARNING << "Could not get class id for object " << obj.description.name << "! "
+ << "Known classes are " << simox::alg::get_keys(knownObjects);
return false;
}
@@ -222,4 +231,96 @@ namespace armarx::armem::articulated_object
// return propertyPrefix;
// }
+
+ // TODO this is a duplicate
+ std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ {
+ // clang-format off
+ const armem::wm::ProviderSegment& providerSegment = memory
+ .getCoreSegment(properties.coreClassSegmentName)
+ .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
+ // clang-format on
+ const auto entities = simox::alg::get_values(providerSegment.entities());
+
+ if (entities.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ return std::nullopt;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entities.front().history());
+
+ if (entitySnapshots.empty())
+ {
+ ARMARX_WARNING << "No entity snapshots found";
+ return std::nullopt;
+ }
+
+ // TODO(fabian.reister): check if 0 available
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+
+ return robot::convertRobotDescription(instance);
+ }
+
+ std::unordered_map<std::string, armem::MemoryID>Writer::getRobotDescriptions(const armarx::armem::wm::Memory& memory) const
+ {
+ std::unordered_map<std::string, armem::MemoryID> descriptions;
+
+ const armem::wm::CoreSegment& coreSegment = memory.getCoreSegment(properties.coreClassSegmentName);
+
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
+ {
+ for (const auto& [name, entity] : providerSegment.entities())
+ {
+ if (entity.empty())
+ {
+ ARMARX_WARNING << "No entity found";
+ continue;
+ }
+
+ const auto entitySnapshots = simox::alg::get_values(entity.history());
+ const armem::wm::EntitySnapshot& sn = entitySnapshots.front();
+ const armem::wm::EntityInstance& instance = sn.getInstance(0);
+
+ const auto robotDescription = robot::convertRobotDescription(instance);
+
+ if (robotDescription)
+ {
+ const armem::MemoryID snapshotID(sn.id());
+ descriptions.insert({robotDescription->name, snapshotID});
+ }
+ }
+
+ }
+
+ return descriptions;
+ }
+
+
+ std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ {
+ // Query all entities from provider.
+ armem::client::query::Builder qb;
+
+ // clang-format off
+ qb
+ .coreSegments().withName(properties.coreClassSegmentName)
+ .providerSegments().all()
+ .entities().all()
+ .snapshots().latest(); // TODO beforeTime(timestamp);
+ // clang-format on
+
+ const armem::client::QueryResult qResult = memoryReader.query(qb.buildQueryInput());
+
+ ARMARX_DEBUG << "Lookup result in reader: " << qResult;
+
+ if (not qResult.success) /* c++20 [[unlikely]] */
+ {
+ return {};
+ }
+
+ return getRobotDescriptions(qResult.memory);
+ }
+
+
} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
index 53ebdfdad..7e450b046 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -56,14 +56,23 @@ namespace armarx::armem::articulated_object
std::optional<armem::MemoryID> storeOrGetClass(const ArticulatedObject& obj);
void updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs);
+ void updateKnownObjects();
void updateKnownObject(const armem::MemoryID& snapshotId);
+ // TODO duplicate
+ std::unordered_map<std::string, armem::MemoryID> queryDescriptions(const armem::Time& timestamp);
+ std::optional<robot::RobotDescription> getRobotDescription(const armarx::armem::wm::Memory& memory) const;
+ std::unordered_map<std::string, armem::MemoryID> getRobotDescriptions(const armarx::armem::wm::Memory& memory) const;
+
+
struct Properties
{
std::string memoryName = "Object";
std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
std::string coreClassSegmentName = "ArticulatedObjectClass";
- std::string providerName;
+ std::string providerName = "ArmarXObjects";
+
+ bool allowClassCreation = false;
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -75,7 +84,7 @@ namespace armarx::armem::articulated_object
std::mutex memoryReaderMutex;
// key: name of object: RobotDescription::name
- std::map<std::string, MemoryID> knownObjects;
+ std::unordered_map<std::string, MemoryID> knownObjects;
armem::ClientComponentPluginUser& component;
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
index d182a3574..62cb54fbc 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_class/Segment.cpp
@@ -93,7 +93,7 @@ namespace armarx::armem::server::obj::articulated_object_class
arondto::RobotDescription aronRobotDescription;
toAron(aronRobotDescription, desc);
- aronRobotDescription.timestamp = now;
+ // TODO toAron(aronRobotDescription.timestamp, now);
update.instancesData = { aronRobotDescription.toAron()};
}
--
GitLab