From 17b91e30e727249752a666c2d8a278c54c6ac40c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 30 Jun 2021 18:06:31 +0200
Subject: [PATCH] articulated object reader and writer now storing instances in
class / instance segments
---
.../ArMemObjectMemory/ArMemObjectMemory.scx | 2 +-
.../ArticulatedObjectLocalizerExample.cfg | 8 +-
...ulatedObjectLocalizerExample.instance1.cfg | 254 ++++++++++++++++++
.../ArMemObjectMemory/config/ObjectMemory.cfg | 9 +
.../ArticulatedObjectLocalizerExample.cpp | 82 +++---
.../ArticulatedObjectLocalizerExample.h | 25 +-
.../RobotStateMemory/RobotStateMemory.cpp | 8 +-
.../libraries/ArmarXObjects/ObjectPose.h | 5 +
.../RobotUnitComponentPlugin.cpp | 30 +--
.../RobotUnitComponentPlugin.h | 21 +-
.../armem/server/MemoryToIceAdapter.cpp | 2 +-
.../libraries/armem_objects/CMakeLists.txt | 6 +
.../ArticulatedObjectReader.cpp | 61 +++++
.../ArticulatedObjectReader.h | 16 ++
.../ArticulatedObjectWriter.cpp | 49 ++++
.../ArticulatedObjectWriter.h | 21 ++
.../client/articulated_object/Reader.cpp | 38 +--
.../client/articulated_object/Reader.h | 41 +--
.../client/articulated_object/Writer.cpp | 103 ++++---
.../client/articulated_object/Writer.h | 4 +-
.../client/articulated_object/utils.cpp | 34 +++
.../client/articulated_object/utils.h | 16 ++
22 files changed, 656 insertions(+), 179 deletions(-)
create mode 100644 scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
create mode 100644 source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index 186377708..9e30d4e39 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -9,6 +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="false" iceAutoRestart="false"/>
- <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="false" 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
index 82769e0f2..9d225df2f 100644
--- a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg
@@ -46,10 +46,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment: Name of the memory core segment to use for object classes.
# Attributes:
-# - Default: ArticulatedObjectClass
+# - Default: Class
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = ArticulatedObjectClass
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.MemoryName:
@@ -62,10 +62,10 @@
# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName:
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = ExampleProvider
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
# ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS).
diff --git a/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
new file mode 100644
index 000000000..9d225df2f
--- /dev/null
+++ b/scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.instance1.cfg
@@ -0,0 +1,254 @@
+# ==================================================================
+# 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: Class
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.CoreSegment = Class
+
+
+# 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: PriorKnowledgeData
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.ProviderName = PriorKnowledgeData
+
+
+# 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.ArticulatedObjectLocalizerExample.updateFrequency: Memory update frequency (write).
+# Attributes:
+# - Default: 25
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArticulatedObjectLocalizerExample.updateFrequency = 25
+
+
+# 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 3a94ad28d..e67a4c411 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -550,6 +550,15 @@
# ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels: Prefer articulated object models if available.
+# Attributes:
+# - Default: true
+# - Case sensitivity: yes
+# - Required: no
+# - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.useArticulatedModels = true
+
+
# ArmarX.ObjectMemory.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:
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
index 7009a9267..487c9c6fc 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp
@@ -20,8 +20,6 @@
*/
#include "ArticulatedObjectLocalizerExample.h"
-#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-
#include <memory>
@@ -30,30 +28,29 @@
#include <IceUtil/Time.h>
#include <SimoxUtility/math/pose/pose.h>
-
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/XML/RobotIO.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/PackagePath.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem_objects/types.h>
-
namespace armarx::articulated_object
{
ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() :
- articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(memoryNameSystem)),
- articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem))
+ articulatedObjectWriter(new ::armarx::armem::articulated_object::ArticulatedObjectWriter(memoryNameSystem)),
+ articulatedObjectReader(new ::armarx::armem::articulated_object::ArticulatedObjectReader(memoryNameSystem))
{
}
@@ -92,7 +89,8 @@ namespace armarx::articulated_object
ARMARX_IMPORTANT << "Running example.";
start = armem::Time::now();
- task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
+ task = new PeriodicTask<ArticulatedObjectLocalizerExample>(
+ this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency);
task->start();
}
@@ -101,20 +99,23 @@ namespace armarx::articulated_object
task->stop();
}
- void ArticulatedObjectLocalizerExample::onExitComponent() {}
+ void ArticulatedObjectLocalizerExample::onExitComponent()
+ {
+ }
VirtualRobot::RobotPtr ArticulatedObjectLocalizerExample::createDishwasher()
{
- const std::string dishwasherName = "CupboardWithDishwasher";
+ const std::string dishwasherName = "Kitchen/mobile-dishwasher";
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;
- });
+ 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())
{
@@ -122,30 +123,23 @@ namespace armarx::articulated_object
return nullptr;
}
- return VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path), VirtualRobot::RobotIO::eStructure);
- }
+ auto obj =
+ 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_DEBUG << "Filename is " << obj.getFilename();
-
- return
- armem::articulated_object::ArticulatedObject
+ if (not obj)
{
- .description = {
- .name = obj.getName(),
- .xml = PackagePath(armarx::ArmarXDataPath::getProject({"ArmarXObjects"}, obj.getFilename()), obj.getFilename())
- },
- .instance = "", // TODO(fabian.reister):
- .config = {
- .timestamp = timestamp,
- .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
- .jointMap = obj.getJointValues()
- },
- .timestamp = timestamp
- };
+ return nullptr;
+ }
+
+ obj->setName("MobileDishwasher0");
+ obj->setType(it->name);
+
+ return obj;
}
+
+
void ArticulatedObjectLocalizerExample::run()
{
if (dishwasher == nullptr)
@@ -161,23 +155,19 @@ namespace armarx::articulated_object
ARMARX_DEBUG << "Reporting articulated objects";
const IceUtil::Time now = TimeUtil::GetTime();
- const float t = float((now - start).toSecondsDouble());
+ const float t = float((now - start).toSecondsDouble());
// move joints at certain frequency
const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1]
- const std::map<std::string, float> jointValues
- {
- {"dishwasher_door_joint", M_PIf32 / 2 * k},
- {"drawer_joint", 350 * k}
- };
+ const std::map<std::string, float> jointValues{{"dishwasher_door_joint", M_PIf32 / 2 * k},
+ {"drawer_joint", 350 * k}};
dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0)));
dishwasher->setJointValues(jointValues);
- armarx::armem::articulated_object::ArticulatedObject armemDishwasher =
- convert(*dishwasher, IceUtil::Time::now());
- articulatedObjectWriter->store(armemDishwasher);
+
+ articulatedObjectWriter->storeArticulatedObject(dishwasher, IceUtil::Time::now());
}
} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
index 75c0764cf..f85a804e9 100644
--- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
+++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.h
@@ -1,24 +1,22 @@
#pragma once
+#include <VirtualRobot/VirtualRobot.h>
-// ArmarX
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/util/tasks.h>
+
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// RobotAPI
-#include <RobotAPI/interface/armem/server/MemoryInterface.h>
+#include "RobotAPI/libraries/armem/core/Time.h"
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
#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>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h>
+#include <RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h>
namespace armarx::articulated_object
{
@@ -45,7 +43,6 @@ namespace armarx::articulated_object
std::string getDefaultName() const override;
protected:
-
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
@@ -55,9 +52,7 @@ namespace armarx::articulated_object
void run();
-
private:
-
VirtualRobot::RobotPtr createDishwasher();
std::shared_ptr<VirtualRobot::Robot> dishwasher;
@@ -68,14 +63,14 @@ namespace armarx::articulated_object
armarx::DebugObserverInterfacePrx debugObserver;
- std::unique_ptr<::armarx::armem::articulated_object::Writer> articulatedObjectWriter;
- std::unique_ptr<::armarx::armem::articulated_object::Reader> articulatedObjectReader;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectWriter> articulatedObjectWriter;
+ std::unique_ptr<::armarx::armem::articulated_object::ArticulatedObjectReader>
+ articulatedObjectReader;
struct Properties
{
float updateFrequency{25.F};
} p;
-
};
-} // namespace armarx::articulated_object
+} // namespace armarx::articulated_object
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 8c41ace39..111cced66 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -89,6 +89,8 @@ namespace armarx::armem::server::robot_state
localizationSegment.defineProperties(defs);
commonVisu.defineProperties(defs);
+ setRobotUnitAsOptionalDependency();
+
return defs;
}
@@ -135,7 +137,11 @@ namespace armarx::armem::server::robot_state
void RobotStateMemory::onConnectComponent()
{
- waitUntilRobotUnitIsRunning();
+
+ if (getRobotUnit())
+ {
+ waitUntilRobotUnitIsRunning();
+ }
ARMARX_CHECK_NOT_NULL(getRobotUnit()->getKinematicUnit());
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index ce653e092..23d54901f 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -9,6 +9,7 @@
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
+#include "ArmarXCore/core/PackagePath.h"
#include <IceUtil/Time.h>
@@ -96,6 +97,10 @@ namespace armarx::objpose
* @param agent The agent/robot.
*/
void updateAttached(VirtualRobot::RobotPtr agent);
+
+
+ std::optional<PackagePath> articulatedSimoxXmlPath;
+
};
using ObjectPoseSeq = std::vector<ObjectPose>;
using ObjectPoseMap = std::map<ObjectID, ObjectPose>;
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
index 7d32ae794..3259d2011 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.cpp
@@ -63,27 +63,25 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::preOnInitComponent()
{
- if (!_deactivated)
+ if (_robotUnitName.empty())
+ {
+ parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
+ }
+
+ if (not _isRobotUnitOptionalDependency)
{
- if (_robotUnitName.empty())
- {
- parent<Component>().getProperty(_robotUnitName, PROPERTY_NAME);
- }
parent<Component>().usingProxy(_robotUnitName);
}
}
void RobotUnitComponentPlugin::preOnConnectComponent()
{
- if (!_deactivated)
- {
- parent<Component>().getProxy(_robotUnit, _robotUnitName);
- }
+ parent<Component>().getProxy(_robotUnit, _robotUnitName);
}
void RobotUnitComponentPlugin::postCreatePropertyDefinitions(armarx::PropertyDefinitionsPtr& properties)
{
- if (!_deactivated && !properties->hasDefinition(PROPERTY_NAME))
+ if (!properties->hasDefinition(PROPERTY_NAME))
{
properties->defineRequiredProperty<std::string>(
PROPERTY_NAME,
@@ -98,13 +96,11 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::setRobotUnitName(const std::string& name)
{
- if (!_deactivated)
- {
- ARMARX_CHECK_NOT_EMPTY(name);
- ARMARX_CHECK_EMPTY(_robotUnitName);
- _robotUnitName = name;
- }
+ ARMARX_CHECK_NOT_EMPTY(name);
+ ARMARX_CHECK_EMPTY(_robotUnitName);
+ _robotUnitName = name;
}
+
const std::string& RobotUnitComponentPlugin::getRobotUnitName() const
{
return _robotUnitName;
@@ -112,7 +108,7 @@ namespace armarx::plugins
void RobotUnitComponentPlugin::deactivate()
{
- _deactivated = true;
+ _isRobotUnitOptionalDependency = true;
}
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
index 63d78de7d..80deaf3e5 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h
@@ -29,6 +29,12 @@ namespace armarx
void deactivate();
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+
+ }
+
+
//controllers
public:
template<class PrxT>
@@ -63,7 +69,7 @@ namespace armarx
static constexpr const char* PROPERTY_NAME = "RobotUnitName";
RobotUnitInterfacePrx _robotUnit;
std::string _robotUnitName;
- bool _deactivated = false;
+ bool _isRobotUnitOptionalDependency = false;
std::set<std::string> _ctrls;
};
}
@@ -81,13 +87,18 @@ namespace armarx
RobotUnitInterfacePrx getRobotUnit() const;
+ void setRobotUnitAsOptionalDependency(bool isOptional = true)
+ {
+ plugin->setRobotUnitAsOptionalDependency(isOptional);
+ }
+
/**
* @brief Waits until the robot unit is running.
- *
+ *
* Although the robot unit proxy might be initialized (\see getRobotUnit()), the robot unit might
- * not be fully initialized.
- *
- * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
+ * not be fully initialized.
+ *
+ * @param termCond Termination condition. If it evaluates to true, waitUntilRobotUnitIsRunning returns without waiting
* for the robot unit to become available.
*/
void waitUntilRobotUnitIsRunning(const std::function<bool()>& termCond = [] {return false;}) const;
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index db546ddc5..f47ffe346 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -159,7 +159,7 @@ namespace armarx::armem::server
// TODO: Add if and param here (fabian.peller)
for (const auto& snapshot : updateResult.removedSnapshots)
{
- ARMARX_IMPORTANT << "The id " << snapshot.id() << " was removed from wm";
+ ARMARX_DEBUG << "The id " << snapshot.id() << " was removed from wm";
}
if (publishUpdates)
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 51296814f..d78bddfa8 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -40,7 +40,10 @@ armarx_add_library(
client/articulated_object/Reader.h
client/articulated_object/Writer.h
+ client/articulated_object/ArticulatedObjectReader.h
+ client/articulated_object/ArticulatedObjectWriter.h
client/articulated_object/interfaces.h
+ client/articulated_object/utils.h
client/attachment/Reader.h
client/attachment/Writer.h
@@ -69,6 +72,9 @@ armarx_add_library(
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
+ client/articulated_object/ArticulatedObjectReader.cpp
+ client/articulated_object/ArticulatedObjectWriter.cpp
+ client/articulated_object/utils.cpp
client/attachment/Reader.cpp
client/attachment/Writer.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
new file mode 100644
index 000000000..c973cf248
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.cpp
@@ -0,0 +1,61 @@
+#include "ArticulatedObjectReader.h"
+
+#include <mutex>
+#include <optional>
+
+#include <VirtualRobot/XML/RobotIO.h>
+
+#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+
+namespace armarx::armem::articulated_object
+{
+
+ VirtualRobot::RobotPtr ArticulatedObjectReader::getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp)
+ {
+ const auto descriptions = 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 == name; });
+
+ if (it == descriptions.end())
+ {
+ ARMARX_WARNING << "Articulated object " << name << " not (yet) available";
+ return nullptr;
+ }
+
+ auto obj =
+ VirtualRobot::RobotIO::loadRobot(ArmarXDataPath::resolvePath(it->xml.serialize().path),
+ VirtualRobot::RobotIO::eStructure);
+
+ if (not obj)
+ {
+ return nullptr;
+ }
+
+ obj->setName("");
+ obj->setType(it->name);
+
+ return obj;
+ }
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
new file mode 100644
index 000000000..5d918ddf4
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectReader.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include "Reader.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectReader : virtual public Reader
+ {
+ public:
+ using Reader::Reader;
+
+ VirtualRobot::RobotPtr getArticulatedObject(const std::string& name,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
new file mode 100644
index 000000000..f86f5f90d
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.cpp
@@ -0,0 +1,49 @@
+#include "ArticulatedObjectWriter.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/Robot.h>
+
+namespace armarx::armem::articulated_object
+{
+ armem::articulated_object::ArticulatedObject convert(const VirtualRobot::Robot& obj,
+ const armem::Time& timestamp)
+ {
+ ARMARX_DEBUG << "Filename is " << obj.getFilename();
+
+ // TODO(fabian.reister): remove "PriorKnowledgeData" below
+
+ return armem::articulated_object::ArticulatedObject
+ {
+ .description = {
+ .name = obj.getType(),
+ .xml = PackagePath(armarx::ArmarXDataPath::getProject(
+ {"PriorKnowledgeData"}, obj.getFilename()),
+ obj.getFilename())
+ },
+ .instance = "", // TODO(fabian.reister):
+ .config = {
+ .timestamp = timestamp,
+ .globalPose = Eigen::Affine3f(obj.getRootNode()->getGlobalPose()),
+ .jointMap = obj.getJointValues()
+ },
+ .timestamp = timestamp};
+ }
+
+ bool
+ ArticulatedObjectWriter::storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp)
+ {
+
+ ARMARX_CHECK_NOT_NULL(articulatedObject);
+
+ armarx::armem::articulated_object::ArticulatedObject armemArticulatedObject =
+ convert(*articulatedObject, IceUtil::Time::now());
+
+ return store(armemArticulatedObject);
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
new file mode 100644
index 000000000..ff39d70f0
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/ArticulatedObjectWriter.h
@@ -0,0 +1,21 @@
+
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "Writer.h"
+
+namespace armarx::armem::articulated_object
+{
+
+ class ArticulatedObjectWriter : virtual public Writer
+ {
+ public:
+ using Writer::Writer;
+
+ bool
+ storeArticulatedObject(const VirtualRobot::RobotPtr& articulatedObject,
+ const armem::Time& timestamp);
+ };
+} // namespace armarx::armem::articulated_object
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 8824b1e7f..d0bfb3570 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -3,16 +3,22 @@
#include <mutex>
#include <optional>
-#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/PackagePath.h>
+#include <ArmarXCore/core/logging/Logging.h>
+#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
+#include "RobotAPI/libraries/ArmarXObjects/ObjectPose.h"
+#include "RobotAPI/libraries/armem_objects/aron_conversions.h"
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/Robot.aron.generated.h>
+
+#include "utils.h"
namespace fs = ::std::filesystem;
@@ -21,7 +27,8 @@ namespace armarx::armem::articulated_object
Reader::Reader(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
void Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -235,7 +242,7 @@ namespace armarx::armem::articulated_object
.getCoreSegment(properties.coreInstanceSegmentName);
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -263,16 +270,16 @@ namespace armarx::armem::articulated_object
return std::nullopt;
}
+
std::optional<robot::RobotDescription>
Reader::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::CoreSegment& coreSegment = memory
.getCoreSegment(properties.coreClassSegmentName);
- // .getProviderSegment(properties.providerName); // TODO(fabian.reister): all
// clang-format on
- for (const auto &[_, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [_, providerSegment] : coreSegment.providerSegments())
{
const auto entities = simox::alg::get_values(providerSegment.entities());
@@ -280,7 +287,6 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity found";
continue;
- // return std::nullopt;
}
const auto entitySnapshots = simox::alg::get_values(entities.front().history());
@@ -289,13 +295,13 @@ namespace armarx::armem::articulated_object
{
ARMARX_WARNING << "No entity snapshots found";
continue;
- // return std::nullopt;
}
- // TODO(fabian.reister): check if 0 available
- const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
-
- return robot::convertRobotDescription(instance);
+ if (entitySnapshots.front().hasInstance(0))
+ {
+ const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
+ return convertRobotDescription(instance);
+ }
}
return std::nullopt;
@@ -309,9 +315,9 @@ namespace armarx::armem::articulated_object
const armem::wm::CoreSegment& coreSegment =
memory.getCoreSegment(properties.coreClassSegmentName);
- for (const auto &[providerName, providerSegment] : coreSegment.providerSegments())
+ for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
- for (const auto &[name, entity] : providerSegment.entities())
+ for (const auto& [name, entity] : providerSegment.entities())
{
if (entity.empty())
{
@@ -322,7 +328,7 @@ namespace armarx::armem::articulated_object
const auto entitySnapshots = simox::alg::get_values(entity.history());
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- const auto robotDescription = robot::convertRobotDescription(instance);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
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 5bc5efc3e..f5dac4035 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.h
@@ -24,6 +24,8 @@
#include <mutex>
#include <optional>
+#include <VirtualRobot/VirtualRobot.h>
+
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
@@ -31,11 +33,10 @@
#include "interfaces.h"
-
namespace armarx::armem::articulated_object
{
- class Reader:
- virtual public ReaderInterface
+
+ class Reader : virtual public ReaderInterface
{
public:
Reader(armem::client::MemoryNameSystem& memoryNameSystem);
@@ -46,11 +47,15 @@ namespace armarx::armem::articulated_object
void synchronize(ArticulatedObject& obj, const armem::Time& timestamp) override;
- std::optional<ArticulatedObject> get(const std::string& name, const armem::Time& timestamp) override;
- ArticulatedObject get(const ArticulatedObjectDescription& description, const armem::Time& timestamp) override;
+ std::optional<ArticulatedObject> get(const std::string& name,
+ const armem::Time& timestamp) override;
+ ArticulatedObject get(const ArticulatedObjectDescription& description,
+ const armem::Time& timestamp) override;
- 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::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);
@@ -60,20 +65,24 @@ namespace armarx::armem::articulated_object
// 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;
+ 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 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 = "ArmarXObjects";
+ std::string memoryName = "Object";
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
+ std::string providerName = "PriorKnowledgeData";
} properties;
const std::string propertyPrefix = "mem.obj.articulated.";
@@ -85,4 +94,4 @@ namespace armarx::armem::articulated_object
};
-} // namespace armarx::armem::articulated_object
+} // namespace armarx::armem::articulated_object
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 f351382f3..1c5134842 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.cpp
@@ -1,28 +1,34 @@
#include "Writer.h"
-#include <IceUtil/Time.h>
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
#include <mutex>
#include <optional>
+#include <IceUtil/Time.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include "ArmarXCore/core/exceptions/LocalException.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/error.h>
-#include <RobotAPI/libraries/armem/client/query.h>
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.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/aron/RobotDescription.aron.generated.h>
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/robot_conversions.h>
+#include "utils.h"
namespace armarx::armem::articulated_object
{
Writer::Writer(armem::client::MemoryNameSystem& memoryNameSystem) :
memoryNameSystem(memoryNameSystem)
- {}
+ {
+ }
void Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
{
@@ -59,7 +65,8 @@ namespace armarx::armem::articulated_object
return;
}
- const auto resultCoreClassSegment = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto resultCoreClassSegment =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
const auto resultCoreInstanceSegmentName =
memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
@@ -82,7 +89,8 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): implement
}
- void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID, const std::vector<armem::MemoryID>& snapshotIDs)
+ void Writer::updateKnownObjects(const armem::MemoryID& subscriptionID,
+ const std::vector<armem::MemoryID>& snapshotIDs)
{
ARMARX_INFO << "New objects available!";
updateKnownObjects();
@@ -91,6 +99,8 @@ namespace armarx::armem::articulated_object
void Writer::updateKnownObjects()
{
knownObjects = queryDescriptions(IceUtil::Time::now());
+
+ ARMARX_INFO << "Known articulated objects " << simox::alg::get_keys(knownObjects);
}
std::optional<armem::MemoryID> Writer::storeOrGetClass(const ArticulatedObject& obj)
@@ -103,6 +113,8 @@ namespace armarx::armem::articulated_object
return objectId->second;
}
+ throw LocalException("articulated object class " + obj.description.name + " not found");
+
// otherwise create
if (properties.allowClassCreation)
{
@@ -119,7 +131,8 @@ namespace armarx::armem::articulated_object
ARMARX_DEBUG << "Trying to create core segment + provider segment";
// TODO(fabian.reister): variable provider segment
- const auto result = memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
+ const auto result =
+ memoryWriter.addSegment(properties.coreClassSegmentName, properties.providerName);
if (not result.success)
{
@@ -131,9 +144,7 @@ namespace armarx::armem::articulated_object
const auto providerId = armem::MemoryID(result.segmentID);
const auto entityID =
- providerId
- .withEntityName(obj.description.name)
- .withTimestamp(timestamp);
+ providerId.withEntityName(obj.description.name).withTimestamp(timestamp);
armem::EntityUpdate update;
update.entityID = entityID;
@@ -175,26 +186,6 @@ namespace armarx::armem::articulated_object
{
std::lock_guard g{memoryWriterMutex};
- ARMARX_DEBUG << "Trying to create core segment + provider segment";
-
- // Provider segments are now added when necessary.
- // Adding them explicitly is only needed to set a deriving provider segment type.
- if (false)
- {
- const auto result =
- memoryWriter.addSegment(properties.coreInstanceSegmentName, properties.providerName);
-
- if (not result.success)
- {
- ARMARX_ERROR << "Creating core segment failed. Reason: " << result.errorMessage;
- return false;
- }
- else
- {
- ARMARX_IMPORTANT << VAROUT(result.segmentID);
- }
- }
-
const auto& timestamp = obj.timestamp;
const auto providerId = armem::MemoryID()
@@ -206,21 +197,28 @@ namespace armarx::armem::articulated_object
update.entityID = providerId.withEntityName(obj.description.name);
// .withTimestamp(timestamp); // You only need to specify the entity ID, not the snapshot ID
- arondto::Robot aronArticulatedObject;
- robot::toAron(aronArticulatedObject, obj);
+ // arondto::Robot aronArticulatedObject;
+ // robot::toAron(aronArticulatedObject, obj);
+
+ arondto::ObjectInstance objectInstance;
+ toAron(objectInstance, obj.config);
- const auto descriptionId = storeOrGetClass(obj);
- if (not descriptionId)
+ const auto classId = storeOrGetClass(obj);
+
+ if (not classId)
{
ARMARX_WARNING << "Could not get class for object " << obj.description.name;
return false;
}
+
+
// install memory link
- toAron(aronArticulatedObject.description, *descriptionId);
+ // toAron(aronArticulatedObject.description, *descriptionId);
+ toAron(objectInstance.classID, *classId);
- update.instancesData = {aronArticulatedObject.toAron()};
+ update.instancesData = {objectInstance.toAron()};
update.timeCreated = timestamp;
ARMARX_DEBUG << "Committing " << update << " at time " << timestamp;
@@ -251,14 +249,9 @@ namespace armarx::armem::articulated_object
return storeInstance(obj);
}
- // const std::string& Writer::getPropertyPrefix() const
- // {
- // return propertyPrefix;
- // }
-
-
// TODO this is a duplicate
- std::optional<robot::RobotDescription> Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
+ std::optional<robot::RobotDescription>
+ Writer::getRobotDescription(const armarx::armem::wm::Memory& memory) const
{
// clang-format off
const armem::wm::ProviderSegment& providerSegment = memory
@@ -284,14 +277,16 @@ namespace armarx::armem::articulated_object
// TODO(fabian.reister): check if 0 available
const armem::wm::EntityInstance& instance = entitySnapshots.front().getInstance(0);
- return robot::convertRobotDescription(instance);
+ return 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>
+ 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);
+ const armem::wm::CoreSegment& coreSegment =
+ memory.getCoreSegment(properties.coreClassSegmentName);
for (const auto& [providerName, providerSegment] : coreSegment.providerSegments())
{
@@ -303,11 +298,11 @@ namespace armarx::armem::articulated_object
continue;
}
- const auto entitySnapshots = simox::alg::get_values(entity.history());
+ 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);
+ const auto robotDescription = convertRobotDescription(instance);
if (robotDescription)
{
@@ -315,14 +310,13 @@ namespace armarx::armem::articulated_object
descriptions.insert({robotDescription->name, snapshotID});
}
}
-
}
return descriptions;
}
-
- std::unordered_map<std::string, armem::MemoryID> Writer::queryDescriptions(const armem::Time& timestamp)
+ std::unordered_map<std::string, armem::MemoryID>
+ Writer::queryDescriptions(const armem::Time& timestamp)
{
// Query all entities from provider.
armem::client::query::Builder qb;
@@ -347,5 +341,4 @@ namespace armarx::armem::articulated_object
return getRobotDescriptions(qResult.memory);
}
-
} // namespace armarx::armem::articulated_object
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 c5fa97dd6..954952207 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Writer.h
@@ -74,8 +74,8 @@ namespace armarx::armem::articulated_object
struct Properties
{
std::string memoryName = "Object";
- std::string coreInstanceSegmentName = "ArticulatedObjectInstance";
- std::string coreClassSegmentName = "ArticulatedObjectClass";
+ std::string coreInstanceSegmentName = "Instance";
+ std::string coreClassSegmentName = "Class";
std::string providerName = "";
bool allowClassCreation = false;
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
new file mode 100644
index 000000000..c6da5e109
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.cpp
@@ -0,0 +1,34 @@
+#include "utils.h"
+
+#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
+
+namespace armarx::armem::articulated_object
+{
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance)
+ {
+
+ arondto::ObjectClass aronObjectInfo;
+ try
+ {
+ aronObjectInfo.fromAron(instance.data());
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Conversion to ObjectPose failed!";
+ return std::nullopt;
+ }
+
+ robot::RobotDescription robotDescription;
+ fromAron(aronObjectInfo, robotDescription);
+
+ // check if robot description is valid
+ const auto xml = robotDescription.xml.serialize();
+ if (robotDescription.name.empty() or xml.package.empty() or xml.path.empty())
+ {
+ return std::nullopt;
+ }
+
+ return robotDescription;
+ }
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
new file mode 100644
index 000000000..2e51a9e4b
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/utils.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <optional>
+
+#include "RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h"
+#include "RobotAPI/libraries/armem_robot/types.h"
+#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
+
+
+namespace armarx::armem::articulated_object
+{
+
+ std::optional<robot::RobotDescription>
+ convertRobotDescription(const armem::wm::EntityInstance& instance);
+
+} // namespace armarx::armem::articulated_object
\ No newline at end of file
--
GitLab