diff --git a/etc/mongocxx/install_mongocxx.sh b/etc/mongocxx/install_mongocxx.sh
index 54889f9f6a82788061e726e339439fd3d3e929bd..8820931ce0ab923c8a2ff451ee84f62c9b62918c 100755
--- a/etc/mongocxx/install_mongocxx.sh
+++ b/etc/mongocxx/install_mongocxx.sh
@@ -12,6 +12,9 @@ fi
# Stolen from mycroft-core/dev_setup.sh // Rainer Kartmann
+function found_exe() {
+ hash "$1" 2>/dev/null
+}
# If tput is available and can handle multiple colors
if found_exe tput ; then
diff --git a/scenarios/ArMemCore/ArMemCore.scx b/scenarios/ArMemCore/ArMemCore.scx
index 421e3a1277c89e919677097f3841ae90061d8e25..c58f240d86e1aa7b8d01b04f6d15d0504eba8d21 100644
--- a/scenarios/ArMemCore/ArMemCore.scx
+++ b/scenarios/ArMemCore/ArMemCore.scx
@@ -3,5 +3,6 @@
<application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
<application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemCore/config/ArVizStorage.cfg b/scenarios/ArMemCore/config/ArVizStorage.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..0dcbd8d5775b8c0f562bba62b5f2fe282e2da6d0
--- /dev/null
+++ b/scenarios/ArMemCore/config/ArVizStorage.cfg
@@ -0,0 +1,212 @@
+# ==================================================================
+# ArVizStorage 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.ArVizStorage.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.ArVizStorage.EnableProfiling = false
+
+
+# ArmarX.ArVizStorage.HistoryPath: Destination path where the history are serialized to
+# Attributes:
+# - Default: RobotAPI/ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.HistoryPath = RobotAPI/ArVizStorage
+
+
+# ArmarX.ArVizStorage.MaxHistorySize: How many layer updates are saved in the history until they are compressed
+# Attributes:
+# - Default: 1000
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.MaxHistorySize = 1000
+
+
+# ArmarX.ArVizStorage.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.ArVizStorage.MinimumLoggingLevel = Undefined
+
+
+# ArmarX.ArVizStorage.ObjectName: Name of IceGrid well-known object
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.ObjectName = ""
+
+
+# ArmarX.ArVizStorage.TopicName: Layer updates are sent over this topic.
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ArVizStorage.TopicName = ArVizTopic
+
+
+# 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/ArMemExample/config/ExampleMemory.cfg b/scenarios/ArMemExample/config/ExampleMemory.cfg
index d9e24e657c2b57a11bac7d34b2df65dff2e9eb8d..117331158ed0929bccc03456cfe93ab1281d1d3f 100644
--- a/scenarios/ArMemExample/config/ExampleMemory.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemory.cfg
@@ -84,21 +84,53 @@ ArmarX.ArMemExampleMemory.tpc.pub.MemoryListener = MemoryUpdates
# ArmarX.EnableProfiling = false
-# ArmarX.ExampleMemory.:
+# ArmarX.ExampleMemory.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.ExampleMemory.EnableProfiling = false
+
+
+# ArmarX.ExampleMemory.Longtermmemorydatabase:
+# Attributes:
+# - Default: Test
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemorydatabase = Test
+
+
+# ArmarX.ExampleMemory.Longtermmemoryhost:
+# Attributes:
+# - Default: localhost
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemoryhost = localhost
+
+
+# ArmarX.ExampleMemory.Longtermmemorypassword:
# Attributes:
# - Default: ""
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ExampleMemory. = ""
+# ArmarX.ExampleMemory.Longtermmemorypassword = ""
-# ArmarX.ExampleMemory.EnableProfiling: enable profiler which is used for logging performance events
+# ArmarX.ExampleMemory.Longtermmemoryport:
# Attributes:
-# - Default: false
+# - Default: 27017
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ExampleMemory.EnableProfiling = false
+# ArmarX.ExampleMemory.Longtermmemoryport = 27017
+
+
+# ArmarX.ExampleMemory.Longtermmemoryuser:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemory.Longtermmemoryuser = ""
# ArmarX.ExampleMemory.MinimumLoggingLevel: Local logging level only for this component
diff --git a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
index 43b1e45191b3eb1df8d39ffb36d8990ec70e3c0e..fa8e5a747031ddbb8e87a5c5d5b60446d2393c2c 100644
--- a/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
+++ b/scenarios/ArMemExample/config/ExampleMemoryClient.cfg
@@ -118,6 +118,14 @@ ArmarX.ArMemExampleClient.tpc.sub.MemoryListener = MemoryUpdates
# ArmarX.ExampleMemoryClient.RemoteGuiName = RemoteGuiProvider
+# ArmarX.ExampleMemoryClient.ex.CommitFrequency: Frequency in which example data is commited.
+# Attributes:
+# - Default: 10
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ExampleMemoryClient.ex.CommitFrequency = 10
+
+
# ArmarX.ExampleMemoryClient.mem.UsedMemoryName: Name of the memory to use.
# Attributes:
# - Default: Example
diff --git a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
index e6f93e12cda04bf7a80e6a41daada5189f2ef787..186377708d49981f0a052fcfc33d8c8286acb2fd 100644
--- a/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
+++ b/scenarios/ArMemObjectMemory/ArMemObjectMemory.scx
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="utf-8"?>
<scenario name="ArMemObjectMemory" creation="2021-04-22.11:29:22" globalConfigName="./config/global.cfg" package="RobotAPI" deploymentType="local" nodeName="NodeMain">
<application name="ObjectMemory" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
- <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="true" iceAutoRestart="false"/>
+ <application name="MemoryNameSystem" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="DebugObserver" instance="" package="ArmarXCore" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="RemoteGuiProviderApp" instance="" package="ArmarXGui" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ObjectPoseProviderExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ArVizStorage" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
<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"/>
+ <application name="ObjectPoseClientExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
+ <application name="ArticulatedObjectLocalizerExample" instance="" package="RobotAPI" nodeName="" enabled="false" iceAutoRestart="false"/>
</scenario>
diff --git a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
index df483eb31778702f71c8a1ff6c13bf260f86c605..abb6c293cfe6ead2229ce0cf06a15f6d343e7577 100644
--- a/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
+++ b/scenarios/ArMemObjectMemory/config/ObjectMemory.cfg
@@ -92,14 +92,6 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.LoggingGroup = ""
-# ArmarX.ObjectMemory.:
-# Attributes:
-# - Default: ""
-# - Case sensitivity: yes
-# - Required: no
-# ArmarX.ObjectMemory. = ""
-
-
# ArmarX.ObjectMemory.ArVizTopicName: Name of the ArViz topic
# Attributes:
# - Default: ArVizTopic
@@ -117,6 +109,46 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.EnableProfiling = false
+# ArmarX.ObjectMemory.Longtermmemorydatabase:
+# Attributes:
+# - Default: Test
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.Longtermmemorydatabase = Test
+
+
+# ArmarX.ObjectMemory.Longtermmemoryhost:
+# Attributes:
+# - Default: localhost
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.Longtermmemoryhost = localhost
+
+
+# ArmarX.ObjectMemory.Longtermmemorypassword:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.Longtermmemorypassword = ""
+
+
+# ArmarX.ObjectMemory.Longtermmemoryport:
+# Attributes:
+# - Default: 27017
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.Longtermmemoryport = 27017
+
+
+# ArmarX.ObjectMemory.Longtermmemoryuser:
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.Longtermmemoryuser = ""
+
+
# ArmarX.ObjectMemory.MinimumLoggingLevel: Local logging level only for this component
# Attributes:
# - Default: Undefined
@@ -193,10 +225,10 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.articulated.cls.ObjectsPackage = PriorKnowledgeData
# ArmarX.ObjectMemory.mem.articulated.inst.CoreSegmentName: Name of the object instance core segment.
@@ -293,10 +325,10 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.cls.ObjectsPackage: Name of the objects package to load from.
# Attributes:
-# - Default: ArmarXObjects
+# - Default: PriorKnowledgeData
# - Case sensitivity: yes
# - Required: no
-# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = ArmarXObjects
+# ArmarX.ObjectMemory.mem.cls.ObjectsPackage = PriorKnowledgeData
# ArmarX.ObjectMemory.mem.inst.CoreSegmentName: Name of the object instance core segment.
@@ -415,6 +447,24 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.mem.inst.head.maxJointVelocity = 0.0500000007
+# ArmarX.ObjectMemory.mem.inst.scene.Package: ArmarX package containing the scene snapshots.
+# Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.
+# Attributes:
+# - Default: ArmarXObjects
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.Package = ArmarXObjects
+
+
+# ArmarX.ObjectMemory.mem.inst.scene.SnapshotToLoad: Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
+# You can also specify paths relative to 'Package/Scenes/'.
+# Attributes:
+# - Default: ""
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.mem.inst.scene.SnapshotToLoad = ""
+
+
# ArmarX.ObjectMemory.mem.inst.visu.alpha: Alpha of objects (1 = solid, 0 = transparent).
# Attributes:
# - Default: 1
@@ -518,6 +568,14 @@ ArmarX.AdditionalPackages = ArmarXObjects
# ArmarX.ObjectMemory.tpc.pub.MemoryListener = MemoryUpdates
+# ArmarX.ObjectMemory.tpc.sub.MemoryListener: Name of the `MemoryListener` topic to subscribe to.
+# Attributes:
+# - Default: MemoryUpdates
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.ObjectMemory.tpc.sub.MemoryListener = MemoryUpdates
+
+
# ArmarX.ObjectMemory.tpc.sub.ObjectPoseTopic: Name of the `ObjectPoseTopic` topic to subscribe to.
# Attributes:
# - Default: ObjectPoseTopic
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.cpp b/source/RobotAPI/components/ArViz/Client/Elements.cpp
index 3ce3ea6f48c69658788abc3ba2ed00c6a81e1002..daec8bc655e0f6431dc531f0401478838e7f053b 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.cpp
+++ b/source/RobotAPI/components/ArViz/Client/Elements.cpp
@@ -9,15 +9,18 @@ namespace armarx::viz
{
const std::string Object::DefaultObjectsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
+ const std::string Object::DefaultRelativeObjectsDirectory = armarx::ObjectFinder::DefaultObjectsDirectory;
- Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage);
+ return this->fileByObjectFinder(armarx::ObjectID(objectID), objectsPackage, relativeObjectsDirectory);
}
- Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage)
+ Object& Object::fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage,
+ const std::string& relativeObjectsDirectory)
{
- ObjectInfo info(objectsPackage, "", objectID);
+ ObjectInfo info(objectsPackage, "", relativeObjectsDirectory, objectID);
armarx::PackageFileLocation file = info.simoxXML();
return this->file(file.package, file.relativePath);
}
diff --git a/source/RobotAPI/components/ArViz/Client/Elements.h b/source/RobotAPI/components/ArViz/Client/Elements.h
index 66f07992e0ba2dc0c382b3a9742a5413c9f192b6..e9157a24d9f60b9ba71d70cccd2141b0258665c3 100644
--- a/source/RobotAPI/components/ArViz/Client/Elements.h
+++ b/source/RobotAPI/components/ArViz/Client/Elements.h
@@ -461,6 +461,7 @@ namespace armarx::viz
{
private:
static const std::string DefaultObjectsPackage;
+ static const std::string DefaultRelativeObjectsDirectory;
public:
using ElementOps::ElementOps;
@@ -483,8 +484,12 @@ namespace armarx::viz
* @param objectsPackage The objects package ("ArmarXObjects" by default)
* @see <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
*/
- Object& fileByObjectFinder(const armarx::ObjectID& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
- Object& fileByObjectFinder(const std::string& objectID, const std::string& objectsPackage = DefaultObjectsPackage);
+ Object& fileByObjectFinder(const armarx::ObjectID& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
+ Object& fileByObjectFinder(const std::string& objectID,
+ const std::string& objectsPackage = DefaultObjectsPackage,
+ const std::string& relativeObjectsDirectory = DefaultRelativeObjectsDirectory);
Object& alpha(float alpha);
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index b6d997f6882daa1b9ee95d97fb4502ae181aa9ab..0c5d82c8da19676c0ab3b544ea381ed7ae5f8776 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -86,8 +86,6 @@ namespace armarx::armem::server::obj
{
workingMemory.name() = defaultMemoryName;
- instance::SegmentAdapter::init();
-
const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
{
try
@@ -108,11 +106,15 @@ namespace armarx::armem::server::obj
}
};
+ // Class segment needs to be initialized before instance segment,
+ // as the instance segment may refer to and search through the class segment.
initSegmentWithCatch("class", [&]()
{
classSegment.init();
});
+ instance::SegmentAdapter::init();
+
initSegmentWithCatch("articulated object class", [&]()
{
articulatedObjectClassSegment.init();
@@ -185,28 +187,34 @@ namespace armarx::armem::server::obj
{
using namespace armarx::RemoteGui::Client;
- tab.instance.setup(*this);
- tab.clazz.setup(classSegment);
+ tab.reset(new RemoteGuiTab);
+
+ tab->instance.setup(*this);
+ tab->clazz.setup(classSegment);
HBoxLayout segments =
{
- tab.instance.group,
- tab.clazz.group
+ tab->instance.group,
+ tab->clazz.group
};
VBoxLayout root =
{
segments,
VSpacer()
};
- RemoteGui_createTab(Component::getName(), root, &tab);
+ RemoteGui_createTab(Component::getName(), root, tab.get());
}
+
void ObjectMemory::RemoteGui_update()
{
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- tab.instance.update(*this);
- tab.clazz.update(classSegment);
+ tab->instance.update(*this);
+ tab->clazz.update(classSegment);
+
+ if (tab->clazz.data.rebuild)
+ {
+ createRemoteGuiTab();
+ }
}
}
-
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 2dfd668d1e43df914ff7b2f02205e699ad2ea5e9..a513016a3da9f0376a092768e4a977974493ca32 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -122,7 +122,7 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::RemoteGui instance;
clazz::Segment::RemoteGui clazz;
};
- RemoteGuiTab tab;
+ std::unique_ptr<RemoteGuiTab> tab;
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index d39e94c87ce0ea5d5ed5b21e252d31f1e2ca41f9..b00a86caf56aec6a6e3f6c59e734ded8aea893b8 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -345,10 +345,12 @@ module armarx
void setMPWeights(DoubleSeqSeq weights);
DoubleSeqSeq getMPWeights();
- void setLinearVelocityKd(Ice::FloatSeq kd);
- void setLinearVelocityKp(Ice::FloatSeq kp);
- void setAngularVelocityKd(Ice::FloatSeq kd);
- void setAngularVelocityKp(Ice::FloatSeq kp);
+ void setLinearVelocityKd(Eigen::Vector3f kd);
+ void setLinearVelocityKp(Eigen::Vector3f kp);
+ void setAngularVelocityKd(Eigen::Vector3f kd);
+ void setAngularVelocityKp(Eigen::Vector3f kp);
+ void setNullspaceVelocityKd(Eigen::VectorXf jointValues);
+ void setNullspaceVelocityKp(Eigen::VectorXf jointValues);
};
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
index 8e86d96dc398d707ab4e8629e4b971519db8dc65..92a5760ae47f099c3408606903efb25db007f764 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.cpp
@@ -1,23 +1,24 @@
#include <VirtualRobot/XML/ObjectIO.h>
-#include <boost/algorithm/string.hpp>
+#include <set>
#include <SimoxUtility/algorithm/string.h>
#include <SimoxUtility/filesystem/list_directory.h>
-#include <VirtualRobot/XML/RobotIO.h>
-
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include "ObjectFinder.h"
+
namespace armarx
{
namespace fs = std::filesystem;
- ObjectFinder::ObjectFinder(const std::string& objectsPackageName) : packageName(objectsPackageName)
+
+ ObjectFinder::ObjectFinder(const std::string& objectsPackageName, const ObjectFinder::path& relObjectsDir) :
+ packageName(objectsPackageName), relObjectsDir(relObjectsDir)
{
Logging::setTag("ObjectFinder");
}
@@ -25,7 +26,7 @@ namespace armarx
void ObjectFinder::setPath(const std::string& path)
{
packageName = path;
- packageDataDir.clear();
+ absPackageDataDir.clear();
}
std::string ObjectFinder::getPackageName() const
@@ -35,11 +36,11 @@ namespace armarx
void ObjectFinder::init() const
{
- if (packageDataDir.empty())
+ if (absPackageDataDir.empty())
{
CMakePackageFinder packageFinder(packageName);
- packageDataDir = packageFinder.getDataDir();
- if (packageDataDir.empty())
+ absPackageDataDir = packageFinder.getDataDir();
+ if (absPackageDataDir.empty())
{
ARMARX_WARNING << "Could not find package '" << packageName << "'.";
// throw LocalException() << "Could not find package '" << packageName << "'.";
@@ -49,12 +50,18 @@ namespace armarx
ARMARX_VERBOSE << "Objects root directory: " << _rootDirAbs();
// make sure this data path is available => e.g. for findArticulatedObjects
- armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {packageDataDir});
+ armarx::ArmarXDataPath::addDataPaths(std::vector<std::string> {absPackageDataDir});
}
}
}
+ bool ObjectFinder::isDatasetDirValid(const path& path) const
+ {
+ return std::filesystem::is_directory(path);
+ }
+
+
std::optional<ObjectInfo> ObjectFinder::findObject(const std::string& dataset, const std::string& name) const
{
init();
@@ -64,15 +71,15 @@ namespace armarx
}
if (!dataset.empty())
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
// Search for object in datasets.
- const auto& datasets = getDatasets();
- for (const path& dataset : datasets)
+ const std::vector<std::string>& datasets = getDatasets();
+ for (const std::string& dataset : datasets)
{
if (fs::is_directory(_rootDirAbs() / dataset / name))
{
- return ObjectInfo(packageName, packageDataDir, dataset, name);
+ return ObjectInfo(packageName, absPackageDataDir, relObjectsDir, dataset, name);
}
}
@@ -106,7 +113,7 @@ namespace armarx
{
// init(); // Done by called methods.
std::vector<std::string> datasets;
- for (const auto& dir : getDatasetDirectories())
+ for (const path& dir : getDatasetDirectories())
{
datasets.push_back(dir.filename());
}
@@ -123,9 +130,9 @@ namespace armarx
const bool local = false;
std::vector<path> dirs = simox::fs::list_directory(_rootDirAbs(), local);
std::vector<path> datasetDirs;
- for (const auto& p : dirs)
+ for (const path& p : dirs)
{
- if (std::filesystem::is_directory(p))
+ if (isDatasetDirValid(p))
{
datasetDirs.push_back(p);
}
@@ -144,7 +151,7 @@ namespace armarx
std::vector<ObjectInfo> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
std::vector<ObjectInfo> dataset = findAllObjectsOfDataset(datasetDir, checkPaths);
for (const auto& o : dataset)
@@ -169,7 +176,7 @@ namespace armarx
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
objects.insert(objects.end(), dataset.begin(), dataset.end());
@@ -211,7 +218,7 @@ namespace armarx
{
if (fs::is_directory(datasetDir / dir))
{
- ObjectInfo object(packageName, packageDataDir, dataset, dir.filename());
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
if (!checkPaths || object.checkPaths())
{
objects.push_back(object);
@@ -221,7 +228,9 @@ namespace armarx
return objects;
}
- std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
+
+ std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>>
+ ObjectFinder::findAllArticulatedObjectsByDataset(bool checkPaths) const
{
init();
if (!_ready())
@@ -234,7 +243,7 @@ namespace armarx
std::unordered_map<std::string, std::vector<armem::articulated_object::ArticulatedObjectDescription>> datasets;
for (const path& datasetDir : simox::fs::list_directory(_rootDirAbs(), local))
{
- if (fs::is_directory(_rootDirAbs() / datasetDir))
+ if (isDatasetDirValid(_rootDirAbs() / datasetDir))
{
const auto dataset = findAllArticulatedObjectsOfDataset(datasetDir, checkPaths);
datasets[datasetDir] = dataset;
@@ -244,7 +253,8 @@ namespace armarx
}
- std::vector<armem::articulated_object::ArticulatedObjectDescription> ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
+ std::vector<armem::articulated_object::ArticulatedObjectDescription>
+ ObjectFinder::findAllArticulatedObjectsOfDataset(const std::string& dataset, bool checkPaths) const
{
init();
if (!_ready())
@@ -252,58 +262,29 @@ namespace armarx
return {};
}
path datasetDir = _rootDirAbs() / dataset;
- if (!fs::is_directory(datasetDir))
+ if (!isDatasetDirValid(datasetDir))
{
ARMARX_WARNING << "Expected dataset directory for dataset '" << dataset << "': \n"
<< datasetDir;
return {};
}
- const std::vector<std::string> validExtensions{".urdf", ".xml"};
-
- const auto hasValidExtension = [&](const std::string & file) -> bool
- {
- return std::find(validExtensions.begin(), validExtensions.end(), boost::algorithm::to_lower_copy(std::filesystem::path(file).extension().string())) != validExtensions.end();
- };
-
std::vector<armem::articulated_object::ArticulatedObjectDescription> objects;
const bool local = true;
for (const path& dir : simox::fs::list_directory(datasetDir, local))
{
- if (not fs::is_directory(datasetDir / dir))
- {
- continue;
- }
-
- for (const auto& file : std::filesystem::directory_iterator(datasetDir / dir))
+ if (fs::is_directory(datasetDir / dir))
{
- const std::string xml = std::filesystem::path(file).string();
-
- if (hasValidExtension(xml))
+ ObjectInfo object(packageName, absPackageDataDir, relObjectsDir, dataset, dir.filename());
+ std::optional<PackageFileLocation> modelFile = object.getArticulatedModel();
+ if (modelFile.has_value())
{
- try
- {
- const auto robot = VirtualRobot::RobotIO::loadRobot(xml, VirtualRobot::RobotIO::RobotDescription::eStructure);
- if (robot != nullptr && robot->isPassive())
- {
- const std::string relativeXMLPath = armarx::ArmarXDataPath::getRelativeArmarXPath(xml);
-
- objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
- {
- .name = robot->getName(),
- .xml = {packageName, relativeXMLPath}
- // .dataset = dataset
- });
- }
- }
- catch (const armarx::LocalException& ex)
+ objects.emplace_back(armem::articulated_object::ArticulatedObjectDescription
{
- ARMARX_WARNING << ex.what();
- }
- catch (...)
- {
-
- }
+ .name = object.idStr(),
+ .xml = {modelFile->package, modelFile->relativePath}
+ // .dataset = dataset
+ });
}
}
}
@@ -392,7 +373,7 @@ namespace armarx
ObjectFinder::path ObjectFinder::_rootDirAbs() const
{
- return packageDataDir / packageName;
+ return absPackageDataDir / packageName / relObjectsDir;
}
ObjectFinder::path ObjectFinder::_rootDirRel() const
@@ -402,7 +383,7 @@ namespace armarx
bool ObjectFinder::_ready() const
{
- return !packageDataDir.empty();
+ return !absPackageDataDir.empty();
}
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
index 2b342f3ac1151d0589bbc4b0212c3aed2fde798d..ea3a7e9cb08f423716931086f309e6dabeedd1b9 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectFinder.h
@@ -7,32 +7,40 @@
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/libraries/armem_objects/types.h>
+
#include "ObjectInfo.h"
#include "ObjectPose.h"
-#include <RobotAPI/libraries/armem_objects/types.h>
namespace armarx
{
/**
- * @brief Used to find objects in the ArmarX objects repository [1].
+ * @brief Used to find objects in the ArmarX objects repository [1] (formerly [2]).
*
- * @see [1] https://gitlab.com/ArmarX/ArmarXObjects
+ * @see [1] https://gitlab.com/ArmarX/PriorKnowledgeData
+ * @see [2] https://gitlab.com/ArmarX/ArmarXObjects
*/
class ObjectFinder : Logging
{
public:
+
using path = std::filesystem::path;
- inline static const std::string DefaultObjectsPackageName = "ArmarXObjects";
+ inline static const std::string DefaultObjectsPackageName = "PriorKnowledgeData";
+ inline static const std::string DefaultObjectsDirectory = "objects";
+
public:
- ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName);
+
+ ObjectFinder(const std::string& objectsPackageName = DefaultObjectsPackageName,
+ const path& relObjectsDir = DefaultObjectsDirectory);
ObjectFinder(ObjectFinder&&) = default;
ObjectFinder(const ObjectFinder&) = default;
ObjectFinder& operator=(ObjectFinder&&) = default;
ObjectFinder& operator=(const ObjectFinder&) = default;
+
void setPath(const std::string& path);
std::string getPackageName() const;
@@ -89,14 +97,18 @@ namespace armarx
private:
+
void init() const;
+ bool isDatasetDirValid(const std::filesystem::path& path) const;
path _rootDirAbs() const;
path _rootDirRel() const;
bool _ready() const;
+
private:
+
/// Name of package containing the object models (ArmarXObjects by default).
mutable std::string packageName;
@@ -104,6 +116,10 @@ namespace armarx
* @brief Absolute path to data directory (e.g. "/.../repos/ArmarXObjects/data").
* Empty if package could not be found.
*/
- mutable path packageDataDir;
+ mutable path absPackageDataDir;
+
+ /// Path to the directory containing objects in the package's data directory.
+ path relObjectsDir;
+
};
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
index adb8260199706797be7b36b65e4fd504ad2e8a63..f2d76a501ecf8e1f53dcf9bf80a2e8a0f66d7536 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.cpp
@@ -57,6 +57,11 @@ namespace armarx
return _className == rhs._className && _dataset == rhs._dataset;
}
+ ObjectID ObjectID::withInstanceName(const std::string& instanceName) const
+ {
+ return ObjectID(_dataset, _className, instanceName);
+ }
+
bool ObjectID::operator==(const ObjectID& rhs) const
{
return _className == rhs._className
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
index 73e2c6ca5f8d272fd90257fd5aedfd839266174d..389b203a6ecc67f2f6cd074ce103c5b52687a2be 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectID.h
@@ -43,6 +43,8 @@ namespace armarx
/// Indicates whether dataset and class name are equal.
bool equalClass(const ObjectID& rhs) const;
+ ObjectID withInstanceName(const std::string& instanceName) const;
+
/// Indicates whether dataset, class name and instance name are equal.
bool operator==(const ObjectID& rhs) const;
inline bool operator!=(const ObjectID& rhs) const
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
index 348a5b25419813a22717f71c92d0c3484042361c..47728cbd7249f4983717f9a1dbb079e79215fadc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.cpp
@@ -14,14 +14,17 @@ namespace armarx
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const ObjectID& id) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(id)
+ const path& relObjectsPath, const ObjectID& id) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(id)
{
}
ObjectInfo::ObjectInfo(const std::string& packageName, const ObjectInfo::path& packageDataDir,
- const std::string& dataset, const std::string& name) :
- _packageName(packageName), _packageDataDir(packageDataDir), _id(dataset, name)
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className) :
+ _packageName(packageName), _absPackageDataDir(packageDataDir),
+ _relObjectsPath(relObjectsPath), _id(dataset, className)
{
}
@@ -40,7 +43,7 @@ namespace armarx
return _id.dataset();
}
- std::string ObjectInfo::name() const
+ std::string ObjectInfo::className() const
{
return _id.className();
}
@@ -57,7 +60,7 @@ namespace armarx
ObjectInfo::path ObjectInfo::objectDirectory() const
{
- return path(_packageName) / _id.dataset() / _id.className();
+ return path(_packageName) / _relObjectsPath / _id.dataset() / _id.className();
}
PackageFileLocation ObjectInfo::file(const std::string& _extension, const std::string& suffix) const
@@ -72,15 +75,49 @@ namespace armarx
PackageFileLocation loc;
loc.package = _packageName;
loc.relativePath = objectDirectory() / filename;
- loc.absolutePath = _packageDataDir / loc.relativePath;
+ loc.absolutePath = _absPackageDataDir / loc.relativePath;
return loc;
}
+
PackageFileLocation ObjectInfo::simoxXML() const
{
return file(".xml");
}
+ PackageFileLocation ObjectInfo::articulatedSimoxXML() const
+ {
+ return file(".xml", "_articulated");
+ }
+
+ PackageFileLocation ObjectInfo::articulatedUrdf() const
+ {
+ return file(".urdf", "_articulated");
+ }
+
+ std::optional<PackageFileLocation> ObjectInfo::getArticulatedModel() const
+ {
+ if (fs::is_regular_file(articulatedSimoxXML().absolutePath))
+ {
+ return articulatedSimoxXML();
+ }
+ else if (fs::is_regular_file(articulatedUrdf().absolutePath))
+ {
+ return articulatedUrdf();
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+
+ PackageFileLocation ObjectInfo::meshWrl() const
+ {
+ return file(".wrl");
+ }
+
PackageFileLocation ObjectInfo::wavefrontObj() const
{
return file(".obj");
@@ -232,15 +269,15 @@ namespace armarx
{
if (_logError)
{
- ARMARX_WARNING << "Expected simox object file for object '" << *this << "': " << simoxXML().absolutePath;
+ ARMARX_WARNING << "Expected simox object file for object " << *this << ": " << simoxXML().absolutePath;
}
result = false;
}
- if (!fs::is_regular_file(wavefrontObj().absolutePath))
+ if (false and not fs::is_regular_file(wavefrontObj().absolutePath))
{
if (_logError)
{
- ARMARX_WARNING << "Expected wavefront object file (.obj) for object '" << *this << "': " << wavefrontObj().absolutePath;
+ ARMARX_WARNING << "Expected wavefront object file (.obj) for object " << *this << ": " << wavefrontObj().absolutePath;
}
result = false;
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
index 634944e4cc52379977f784c370f48af1e84ed28d..ebd0fc51b4ddb660613dd0c5f18aa322c2942795 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectInfo.h
@@ -41,10 +41,19 @@ namespace armarx
public:
+ /**
+ * @brief ObjectInfo
+ *
+ * @param packageName The ArmarX package.
+ * @param absPackageDataDir Absolute path to the package's data directory.
+ * @param localObjectsPath The path where objects are stored in the data directory.
+ * @param id The object class ID (with dataset and class name).
+ */
+ ObjectInfo(const std::string& packageName, const path& absPackageDataDir,
+ const path& relObjectsPath, const ObjectID& id);
ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const ObjectID& id);
- ObjectInfo(const std::string& packageName, const path& packageDataDir,
- const std::string& dataset, const std::string& name);
+ const path& relObjectsPath,
+ const std::string& dataset, const std::string& className);
virtual ~ObjectInfo() = default;
@@ -56,14 +65,30 @@ namespace armarx
std::string package() const;
std::string dataset() const;
- std::string name() const;
+ std::string className() const;
+ [[deprecated("This function is deprecated. Use className() instead.")]]
+ std::string name() const
+ {
+ return className();
+ }
+
/// Return "dataset/name".
ObjectID id() const;
std::string idStr() const;
+
PackageFileLocation file(const std::string& extension, const std::string& suffix = "") const;
+
PackageFileLocation simoxXML() const;
+
+ PackageFileLocation articulatedSimoxXML() const;
+ PackageFileLocation articulatedUrdf() const;
+ /// Return the articulated Simox XML or URDF, if one exists.
+ std::optional<PackageFileLocation> getArticulatedModel() const;
+
+
+ PackageFileLocation meshWrl() const;
PackageFileLocation wavefrontObj() const;
PackageFileLocation boundingBoxJson() const;
@@ -113,7 +138,8 @@ namespace armarx
private:
std::string _packageName;
- path _packageDataDir;
+ path _absPackageDataDir;
+ path _relObjectsPath;
ObjectID _id;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
index 28a302b49f98aec283958bdb368cc9e3a164818b..85ca31bec07bbe8af4108a64a24805da44dd19d5 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/test/ArmarXObjectsTest.cpp
@@ -29,7 +29,14 @@
#include <iostream>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron/ObjectPose.aron.generated.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+
+namespace fs = std::filesystem;
+
+
+BOOST_AUTO_TEST_SUITE(arondto_ObjectPose_test)
BOOST_AUTO_TEST_CASE(test_ObjectType_copy_assignment)
@@ -64,3 +71,38 @@ BOOST_AUTO_TEST_CASE(test_ObjectPose_copy_assignment)
BOOST_TEST_MESSAGE("Done");
}
+
+
+BOOST_AUTO_TEST_SUITE_END()
+
+
+
+BOOST_AUTO_TEST_SUITE(ObjectFinderTest)
+
+
+BOOST_AUTO_TEST_CASE(test_find)
+{
+ using namespace armarx;
+
+ ObjectFinder finder;
+
+ bool checkPaths = false;
+ std::vector<ObjectInfo> objects = finder.findAllObjects(checkPaths);
+ BOOST_CHECK_GT(objects.size(), 0);
+
+ for (const ObjectInfo& object : objects)
+ {
+ fs::path simoxXML = object.simoxXML().absolutePath;
+ fs::path objectDir = simoxXML.parent_path();
+ BOOST_TEST_CONTEXT("Object: " << object.id() << " at " << objectDir)
+ {
+ BOOST_CHECK(fs::is_directory(objectDir));
+ BOOST_CHECK(fs::is_regular_file(simoxXML)
+ || fs::is_regular_file(object.articulatedSimoxXML().absolutePath));
+ }
+ }
+}
+
+
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index c375e084dea6b48720d50e59cc9e828e070826ba..762d3f240eb99b6f37b4ecf7d47b63a55e7932f0 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -84,22 +84,21 @@ namespace armarx
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
Eigen::Vector3f dori(cfg->Dori[0], cfg->Dori[1], cfg->Dori[2]);
-
- CtrlParams initParams = {kpos, dpos, kori, dori};
- ctrlParams.reinitAllBuffers(initParams);
+ Eigen::VectorXf knull(targets.size());
+ Eigen::VectorXf dnull(targets.size());
ARMARX_CHECK_EQUAL(cfg->Knull.size(), targets.size());
ARMARX_CHECK_EQUAL(cfg->Dnull.size(), targets.size());
- knull.setZero(targets.size());
- dnull.setZero(targets.size());
-
for (size_t i = 0; i < targets.size(); ++i)
{
knull(i) = cfg->Knull.at(i);
dnull(i) = cfg->Dnull.at(i);
}
+ CtrlParams initParams = {kpos, dpos, kori, dori, knull, dnull};
+ ctrlParams.reinitAllBuffers(initParams);
+
torqueLimit = cfg->torqueLimit;
timeDuration = cfg->timeDuration;
@@ -284,6 +283,8 @@ namespace armarx
Eigen::Vector3f dpos = ctrlParams.getUpToDateReadBuffer().dpos;
Eigen::Vector3f kori = ctrlParams.getUpToDateReadBuffer().kori;
Eigen::Vector3f dori = ctrlParams.getUpToDateReadBuffer().dori;
+ Eigen::Vector3f knull = ctrlParams.getUpToDateReadBuffer().knull;
+ Eigen::Vector3f dnull = ctrlParams.getUpToDateReadBuffer().dnull;
Eigen::Vector6f jointControlWrench;
{
@@ -363,6 +364,7 @@ namespace armarx
debugOutputData.getWriteBuffer().targetPose_qx = targetQuat.x;
debugOutputData.getWriteBuffer().targetPose_qy = targetQuat.y;
debugOutputData.getWriteBuffer().targetPose_qz = targetQuat.z;
+ debugOutputData.getWriteBuffer().currentCanVal = rtGetControlStruct().canVal;
debugOutputData.getWriteBuffer().currentPose_x = currentPose(0, 3);
debugOutputData.getWriteBuffer().currentPose_y = currentPose(1, 3);
@@ -374,6 +376,26 @@ namespace armarx
debugOutputData.getWriteBuffer().currentPose_qz = currentQuat.z;
debugOutputData.getWriteBuffer().deltaT = deltaT;
+ debugOutputData.getWriteBuffer().currentKpos_x = kpos.x();
+ debugOutputData.getWriteBuffer().currentKpos_y = kpos.y();
+ debugOutputData.getWriteBuffer().currentKpos_z = kpos.z();
+ debugOutputData.getWriteBuffer().currentKori_x = kori.x();
+ debugOutputData.getWriteBuffer().currentKori_y = kori.y();
+ debugOutputData.getWriteBuffer().currentKori_z = kori.z();
+ debugOutputData.getWriteBuffer().currentKnull_x = knull.x();
+ debugOutputData.getWriteBuffer().currentKnull_y = knull.y();
+ debugOutputData.getWriteBuffer().currentKnull_z = knull.z();
+
+ debugOutputData.getWriteBuffer().currentDpos_x = dpos.x();
+ debugOutputData.getWriteBuffer().currentDpos_y = dpos.y();
+ debugOutputData.getWriteBuffer().currentDpos_z = dpos.z();
+ debugOutputData.getWriteBuffer().currentDori_x = dori.x();
+ debugOutputData.getWriteBuffer().currentDori_y = dori.y();
+ debugOutputData.getWriteBuffer().currentDori_z = dori.z();
+ debugOutputData.getWriteBuffer().currentDnull_x = dnull.x();
+ debugOutputData.getWriteBuffer().currentDnull_y = dnull.y();
+ debugOutputData.getWriteBuffer().currentDnull_z = dnull.z();
+
debugOutputData.commitWrite();
}
@@ -547,6 +569,26 @@ namespace armarx
datafields["currentPose_qy"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qy);
datafields["currentPose_qz"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentPose_qz);
+ datafields["currentKpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_x);
+ datafields["currentKpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_y);
+ datafields["currentKpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKpos_z);
+ datafields["currentKori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_x);
+ datafields["currentKori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_y);
+ datafields["currentKori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKori_z);
+ datafields["currentKnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_x);
+ datafields["currentKnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_y);
+ datafields["currentKnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentKnull_z);
+
+ datafields["currentDpos_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_x);
+ datafields["currentDpos_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_y);
+ datafields["currentDpos_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDpos_z);
+ datafields["currentDori_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_x);
+ datafields["currentDori_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_y);
+ datafields["currentDori_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDori_z);
+ datafields["currentDnull_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_x);
+ datafields["currentDnull_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_y);
+ datafields["currentDnull_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().currentDnull_z);
+
datafields["forceDesired_x"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_x);
datafields["forceDesired_y"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_y);
datafields["forceDesired_z"] = new Variant(debugOutputData.getUpToDateReadBuffer().forceDesired_z);
@@ -613,42 +655,64 @@ namespace armarx
dmpCtrl->removeAllViaPoints();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set linear kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dpos << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dpos = kd;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kpos << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kpos = kp;
ctrlParams.commitWrite();
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kd.size(), 3);
+ ARMARX_INFO << "set angular kd " << VAROUT(kd);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().dori << kd[0], kd[1], kd[2];
+ ctrlParams.getWriteBuffer().dori = kd;
ctrlParams.commitWrite();
-
}
- void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&)
+ void NJointTaskSpaceImpedanceDMPController::setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&)
{
ARMARX_CHECK_EQUAL(kp.size(), 3);
+ ARMARX_INFO << "set angular kp " << VAROUT(kp);
LockGuardType guard(controllerMutex);
- ctrlParams.getWriteBuffer().kori << kp[0], kp[1], kp[2];
+ ctrlParams.getWriteBuffer().kori = kp;
ctrlParams.commitWrite();
}
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kd.size(), targets.size());
+ ARMARX_INFO << "set nullspace kd " << VAROUT(kd);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().dnull = kd;
+ ctrlParams.commitWrite();
+ }
+
+ void NJointTaskSpaceImpedanceDMPController::setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&)
+ {
+ ARMARX_CHECK_EQUAL(kp.size(), targets.size());
+ ARMARX_INFO << "set linear kp " << VAROUT(kp);
+ LockGuardType guard(controllerMutex);
+ ctrlParams.getWriteBuffer().knull = kp;
+ ctrlParams.commitWrite();
+ }
+
+
void NJointTaskSpaceImpedanceDMPController::setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues,
const Ice::Current&)
{
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index ea8cef187de6296e07eeb270e4ca5ac2ad7b78f6..9ded363e9e110bf803ada28b5523057b69b3bd4f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -76,10 +76,12 @@ namespace armarx
void removeAllViaPoints(const Ice::Current&) override;
- void setLinearVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setLinearVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
- void setAngularVelocityKd(const Ice::FloatSeq& kd, const Ice::Current&) override;
- void setAngularVelocityKp(const Ice::FloatSeq& kp, const Ice::Current&) override;
+ void setLinearVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setLinearVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setAngularVelocityKd(const Eigen::Vector3f& kd, const Ice::Current&) override;
+ void setAngularVelocityKp(const Eigen::Vector3f& kp, const Ice::Current&) override;
+ void setNullspaceVelocityKd(const Eigen::VectorXf& kd, const Ice::Current&) override;
+ void setNullspaceVelocityKp(const Eigen::VectorXf& kp, const Ice::Current&) override;
void setUseNullSpaceJointDMP(bool enable, const Ice::Current&) override;
void setDefaultNullSpaceJointValues(const Eigen::VectorXf& jointValues, const Ice::Current&) override;
@@ -112,6 +114,26 @@ namespace armarx
float currentPose_qy;
float currentPose_qz;
+ float currentKpos_x;
+ float currentKpos_y;
+ float currentKpos_z;
+ float currentKori_x;
+ float currentKori_y;
+ float currentKori_z;
+ float currentKnull_x;
+ float currentKnull_y;
+ float currentKnull_z;
+
+ float currentDpos_x;
+ float currentDpos_y;
+ float currentDpos_z;
+ float currentDori_x;
+ float currentDori_y;
+ float currentDori_z;
+ float currentDnull_x;
+ float currentDnull_y;
+ float currentDnull_z;
+
StringFloatDictionary desired_torques;
StringFloatDictionary desired_nullspaceJoint;
float forceDesired_x;
@@ -123,9 +145,11 @@ namespace armarx
float deltaT;
+
+
};
- TripleBuffer<DebugBufferData> debugOutputData;
+ WriteBufferedTripleBuffer<DebugBufferData> debugOutputData;
struct NJointTaskSpaceImpedanceDMPControllerSensorData
{
@@ -134,14 +158,14 @@ namespace armarx
Eigen::Matrix4f currentPose;
Eigen::VectorXf currentTwist;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerSensorData> controllerSensorData;
struct NJointTaskSpaceImpedanceDMPControllerInterfaceData
{
Eigen::Matrix4f currentTcpPose;
};
- TripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
+ WriteBufferedTripleBuffer<NJointTaskSpaceImpedanceDMPControllerInterfaceData> interfaceData;
struct CtrlParams
{
@@ -149,9 +173,11 @@ namespace armarx
Eigen::Vector3f dpos;
Eigen::Vector3f kori;
Eigen::Vector3f dori;
+ Eigen::VectorXf knull;
+ Eigen::VectorXf dnull;
};
- TripleBuffer<CtrlParams> ctrlParams;
+ WriteBufferedTripleBuffer<CtrlParams> ctrlParams;
DMP::Vec<DMP::DMPState> currentJointState;
@@ -188,15 +214,15 @@ namespace armarx
// Eigen::Vector3f kori;
// Eigen::Vector3f dpos;
// Eigen::Vector3f dori;
- Eigen::VectorXf knull;
- Eigen::VectorXf dnull;
+ // Eigen::VectorXf knull;
+ // Eigen::VectorXf dnull;
int numOfJoints;
std::atomic_bool useNullSpaceJointDMP;
bool isNullSpaceJointDMPLearned;
- armarx::TripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
+ WriteBufferedTripleBuffer<Eigen::VectorXf> defaultNullSpaceJointValues;
std::vector<std::string> jointNames;
mutable MutexType controllerMutex;
PeriodicTask<NJointTaskSpaceImpedanceDMPController>::pointer_type controllerTask;
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index 3e0900893a8d8e7a0145b1ec2a4951e94ae85b6a..38d9515cc25818652636ca4b8c5e969eaaf9519a 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -35,6 +35,7 @@ set(LIB_FILES
core/Time.cpp
core/ice_conversions.cpp
core/aron_conversions.cpp
+ core/json_conversions.cpp
core/base/detail/MemoryItem.cpp
core/base/detail/MaxHistorySize.cpp
@@ -136,6 +137,7 @@ set(LIB_HEADERS
core/SuccessHeader.h
core/Time.h
core/aron_conversions.h
+ core/json_conversions.h
core/ice_conversions.h
core/ice_conversions_templates.h
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
index 826a567ab68378d29ba600bfab1801b5a0013a40..2478cba21ad3b73730ed8aa9c11902c3d2038689 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.cpp
@@ -291,14 +291,15 @@ namespace armarx::armem::client
}
catch (const error::ArMemError& e)
{
- errors << "#" << ++errorCounter << "\n"
- << "Failed to retrieve " << id << " from query result: \n" << e.what() << "\n";
+ errors << "\n#" << ++errorCounter << "\n"
+ << "Failed to retrieve " << id << " from query result: \n" << e.what();
}
}
}
else
{
- errors << "# Failed to query '" << memoryName << "': \n" << queryResult.errorMessage << "\n";
+ errors << "\n# " << ++errorCounter << "\n"
+ << "Failed to query '" << memoryName << "': \n" << queryResult.errorMessage;
}
}
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.cpp b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4da9d89b5e04351e5e528132406a962470364e3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.cpp
@@ -0,0 +1,26 @@
+#include "json_conversions.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+void armarx::armem::to_json(nlohmann::json& j, const MemoryID& bo)
+{
+ j["memoryName"] = bo.memoryName;
+ j["coreSegmentName"] = bo.coreSegmentName;
+ j["providerSegmentName"] = bo.providerSegmentName;
+ j["entityName"] = bo.entityName;
+ j["timestamp_usec"] = bo.timestamp.toMicroSeconds();
+ j["timestamp_datetime"] = toDateTimeMilliSeconds(bo.timestamp);
+ j["instanceIndex"] = bo.instanceIndex;
+}
+
+void armarx::armem::from_json(const nlohmann::json& j, MemoryID& bo)
+{
+ j.at("memoryName").get_to(bo.memoryName);
+ j.at("coreSegmentName").get_to(bo.coreSegmentName);
+ j.at("providerSegmentName").get_to(bo.providerSegmentName);
+ j.at("entityName").get_to(bo.entityName);
+ bo.timestamp = Time::microSeconds(j.at("timestamp_usec").get<int64_t>());
+ // j.at("timestamp_datetime").get_to(toDateTimeMilliSeconds(bo.timestamp));
+ j.at("instanceIndex").get_to(bo.instanceIndex);
+}
diff --git a/source/RobotAPI/libraries/armem/core/json_conversions.h b/source/RobotAPI/libraries/armem/core/json_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..86e508c228fe57063216c2f1c6fa7710066992bc
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/json_conversions.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace armarx::armem
+{
+ class MemoryID;
+
+ void to_json(nlohmann::json& j, const MemoryID& bo);
+ void from_json(const nlohmann::json& j, MemoryID& bo);
+}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
index 2ef26181179ae749da153ee3cec87654ae2cb632..53016fc6c83d9ef6e24a2e75942a12bced7d0eaa 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.cpp
@@ -9,4 +9,15 @@
namespace armarx::armem::wm
{
+ FunctionalVisitor::FunctionalVisitor()
+ {
+ }
+
+
+ FunctionalVisitor::~FunctionalVisitor()
+ {
+ }
+
+
+
}
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
index 2cd0e205727bd16a880f95ccbe59855f9225f744..1c1b46e138c7eb2d713ba15b76ba77a2588ea0ce 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/visitor/FunctionalVisitor.h
@@ -19,6 +19,7 @@ namespace armarx::armem::wm
FunctionalVisitor();
virtual ~FunctionalVisitor() override;
+
bool visitEnter(Memory& memory) override
{
return memoryFn ? memoryFn(memory) : Visitor::visitEnter(memory);
@@ -49,13 +50,6 @@ namespace armarx::armem::wm
// Const versions
- bool applyTo(const Memory& memory);
- bool applyTo(const CoreSegment& coreSegment);
- bool applyTo(const ProviderSegment& providerSegment);
- bool applyTo(const Entity& entity);
- bool applyTo(const EntitySnapshot& snapshot);
- bool applyTo(const EntityInstance& instance);
-
bool visitEnter(const Memory& memory) override
{
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
index f0ee979a814fff0257a4f87d1003b34ab0b3aa58..8d60c2252d4e9d6593a228cad75075cc41a21e5c 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotForm.cpp
@@ -191,7 +191,7 @@ namespace armarx::armem::gui
sb->setMinimum(-1000);
sb->setMaximum(1000);
}
- firstSpinBox->setValue(-10);
+ firstSpinBox->setValue(-2);
lastSpinBox->setValue(-1);
QString tooltip = "Python index semantics: Negative indices count from the end (-1 is the last entry).";
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 8c0f625f022e3efeb7cf7763841a25e6469b6ac8..51296814ff03036d197a96683b15827ef9af5fe5 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -20,6 +20,8 @@ armarx_add_library(
aron_conversions.h
aron_forward_declarations.h
+ SceneSnapshot.h
+
server/class/FloorVis.h
server/class/Segment.h
@@ -46,6 +48,8 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
+ SceneSnapshot.cpp
+
server/class/FloorVis.cpp
server/class/Segment.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d491662edda9ff00244178735c0c84c26a923688
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
@@ -0,0 +1,68 @@
+#include "SceneSnapshot.h"
+
+#include <SimoxUtility/json.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+// #include <RobotAPI/libraries/armem/core/json_conversions.h>
+
+
+namespace armarx::armem::obj
+{
+
+ ObjectID SceneSnapshot::Object::getClassID() const
+ {
+ return ObjectID(className);
+ }
+
+
+ ObjectID SceneSnapshot::Object::getClassID(ObjectFinder& finder) const
+ {
+ ObjectID id = getClassID();
+ if (id.dataset().empty())
+ {
+ if (std::optional<ObjectInfo> info = finder.findObject(id.className()))
+ {
+ return info->id();
+ }
+ }
+ return id;
+ }
+
+}
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs)
+{
+ // j["instanceID"] = rhs.instanceID;
+ j["class"] = rhs.className;
+ j["collection"] = rhs.collection;
+ j["position"] = rhs.position;
+ j["orientation"] = rhs.orientation;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs)
+{
+ // j.at("instanceID").get_to(rhs.instanceID);
+ j.at("class").get_to(rhs.className);
+ j.at("collection").get_to(rhs.collection);
+ j.at("position").get_to(rhs.position);
+ j.at("orientation").get_to(rhs.orientation);
+}
+
+
+
+void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot& rhs)
+{
+ j["objects"] = rhs.objects;
+}
+
+
+void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot& rhs)
+{
+ j.at("objects").get_to(rhs.objects);
+}
+
+
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
new file mode 100644
index 0000000000000000000000000000000000000000..add5f1f183d15b6ec7c0588a5d4a9abc7e558d82
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObjectMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+#include <Eigen/Geometry>
+
+
+namespace armarx
+{
+ class ObjectID;
+ class ObjectFinder;
+}
+namespace armarx::armem::obj
+{
+
+ struct SceneSnapshot
+ {
+ struct Object
+ {
+ std::string className;
+ std::string collection;
+
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+
+ ObjectID getClassID() const;
+ ObjectID getClassID(ObjectFinder& finder) const;
+ };
+ std::vector<Object> objects;
+ };
+
+ void to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs);
+
+ void to_json(nlohmann::json& j, const SceneSnapshot& rhs);
+ void from_json(const nlohmann::json& j, SceneSnapshot& rhs);
+
+}
diff --git a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
index 650082eb4ef8b924c8368fdd6cdf8da4578e105a..cb3c418e68b53cb24b781b22ff057386f995d2dc 100644
--- a/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
+++ b/source/RobotAPI/libraries/armem_objects/aron/ObjectClass.xml
@@ -31,6 +31,10 @@ Core segment type of Object/Class.
<armarx::arondto::PackagePath />
</ObjectChild>
+ <ObjectChild key="articulatedSimoxXmlPath">
+ <armarx::arondto::PackagePath />
+ </ObjectChild>
+
<ObjectChild key="meshWrlPath">
<armarx::arondto::PackagePath />
</ObjectChild>
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
index 4c3be719fce4148970fa0ef16d8465d2df7a10d3..dcce63977438bdd0d646b50aa8e5dd9679354df8 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/FloorVis.h
@@ -41,9 +41,9 @@ namespace armarx::armem::server::obj::clazz
{
bool show = true;
- std::string entityName = "Environment/floor-20x20";
+ std::string entityName = "Building/floor-20x20";
std::string layerName = "Floor";
- bool height = 1;
+ float height = -1;
void define(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
};
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
index 026dd879ff4c25e8bc368e96098bc06289cddd87..3e6ecfed37e3d75d60b3d509f97520c7627f0fcc 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.cpp
@@ -79,7 +79,10 @@ namespace armarx::armem::server::obj::clazz
std::vector<ObjectInfo> infos = objectFinder.findAllObjects(checkPaths);
const MemoryID providerID = coreSegment->id().withProviderSegmentName(objectFinder.getPackageName());
- coreSegment->addProviderSegment(providerID.providerSegmentName);
+ if (not coreSegment->hasProviderSegment(providerID.providerSegmentName))
+ {
+ coreSegment->addProviderSegment(providerID.providerSegmentName);
+ }
ARMARX_INFO << "Loading up to " << infos.size() << " object classes from '"
<< objectFinder.getPackageName() << "' ...";
@@ -169,18 +172,23 @@ namespace armarx::armem::server::obj::clazz
toAron(data.id, info.id());
- if (fs::is_regular_file(info.simoxXML().absolutePath))
- {
- toAron(data.simoxXmlPath, info.simoxXML());
- }
- if (fs::is_regular_file(info.wavefrontObj().absolutePath))
+ auto setPathIfExists = [](
+ armarx::arondto::PackagePath & aron,
+ const PackageFileLocation & location)
{
- toAron(data.meshObjPath, info.wavefrontObj());
- }
- if (fs::is_regular_file(info.file(".wrl").absolutePath))
- {
- toAron(data.meshWrlPath, info.file(".wrl"));
- }
+ if (fs::is_regular_file(location.absolutePath))
+ {
+ toAron(aron, location);
+ }
+ else
+ {
+ toAron(aron, PackageFileLocation());
+ }
+ };
+ setPathIfExists(data.simoxXmlPath, info.simoxXML());
+ setPathIfExists(data.articulatedSimoxXmlPath, info.articulatedSimoxXML());
+ setPathIfExists(data.meshObjPath, info.wavefrontObj());
+ setPathIfExists(data.meshWrlPath, info.meshWrl());
auto aabb = info.loadAABB();
toAron(data.aabb, aabb ? aabb.value() : simox::AxisAlignedBoundingBox());
@@ -225,12 +233,16 @@ namespace armarx::armem::server::obj::clazz
{
using namespace armarx::RemoteGui::Client;
+ reloadButton.setLabel("Reload");
+
maxHistorySize.setValue(std::max(1, int(segment.p.maxHistorySize)));
maxHistorySize.setRange(1, 1e6);
infiniteHistory.setValue(segment.p.maxHistorySize == -1);
GridLayout grid;
int row = 0;
+ grid.add(reloadButton, {row, 0}, {1, 2});
+ row++;
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -243,6 +255,12 @@ namespace armarx::armem::server::obj::clazz
void Segment::RemoteGui::Data::update(Segment& segment)
{
+ if (reloadButton.wasClicked())
+ {
+ std::scoped_lock lock(segment.memoryMutex);
+ segment.loadByObjectFinder();
+ rebuild = true;
+ }
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged())
{
std::scoped_lock lock(segment.memoryMutex);
diff --git a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
index 7356e6a1d9e490840f7c19b5e41e27506ddf2ecf..88eaedbefb6529b873dfe6033e6a2857672c0586 100644
--- a/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/class/Segment.h
@@ -76,11 +76,14 @@ namespace armarx::armem::server::obj::clazz
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::Button reloadButton;
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
void setup(const Segment& segment);
void update(Segment& segment);
+
+ bool rebuild = false;
};
Data data;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 7b7dc7a428af8896fd9084f887e0c883c9dd4034..646c004013fa28dc961f6a6c404157d492da9e14 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,12 +1,17 @@
#include "Segment.h"
#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/armem_objects/SceneSnapshot.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/workingmemory/visitor.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/armem/client/query/Builder.h>
#include <RobotAPI/libraries/armem/client/query/query_fns.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
@@ -17,10 +22,17 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/json.h>
+
+#include <Eigen/Geometry>
#include <sstream>
+
namespace armarx::armem::server::obj::instance
{
@@ -37,6 +49,7 @@ namespace armarx::armem::server::obj::instance
{
try
{
+ objectInfo->setLogError(false); // Don't log missing files
return objectInfo->loadOOBB();
}
catch (const std::ios_base::failure& e)
@@ -59,6 +72,7 @@ namespace armarx::armem::server::obj::instance
});
}
+
void Segment::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
{
defs->optional(p.coreSegmentName, prefix + "CoreSegmentName", "Name of the object instance core segment.");
@@ -67,15 +81,30 @@ namespace armarx::armem::server::obj::instance
"If true, no new snapshots are stored while an object is attached to a robot node.\n"
"If false, new snapshots are stored, but the attachment is kept in the new snapshots.");
+ defs->optional(p.sceneSnapshotsPackage, prefix + "scene.Package",
+ "ArmarX package containing the scene snapshots.\n"
+ "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
+ defs->optional(p.sceneSnapshotToLoad, prefix + "scene.SnapshotToLoad",
+ "Scene snapshot to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
+ "You can also specify paths relative to 'Package/Scenes/'.");
+
decay.defineProperties(defs, prefix + "decay.");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
coreSegment = &iceMemory.workingMemory->addCoreSegment(p.coreSegmentName, arondto::ObjectInstance::toAronType());
coreSegment->setMaxHistorySize(p.maxHistorySize);
+
+
+ if (not p.sceneSnapshotToLoad.empty())
+ {
+ bool lockMemory = false;
+ commitSceneSnapshotFromFilename(p.sceneSnapshotToLoad, lockMemory);
+ }
}
@@ -156,42 +185,45 @@ namespace armarx::armem::server::obj::instance
}
}
- commitObjectPoses(providerName, newObjectPoses);
+ commitObjectPoses(newObjectPoses, providerName);
return stats;
}
- void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
+
+ void Segment::commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
+ Time now = TimeUtil::GetTime();
- // Update memory.
- const MemoryID providerSegmentID = coreSegment->id().withProviderSegmentName(providerName);
- if (!coreSegment->hasProviderSegment(providerSegmentID.providerSegmentName))
- {
- coreSegment->addProviderSegment(providerSegmentID.providerSegmentName);
- }
+ const MemoryID coreSegmentID = coreSegment->id();
Commit commit;
for (const objpose::ObjectPose& pose : objectPoses)
{
EntityUpdate& update = commit.updates.emplace_back();
+
+ const MemoryID providerID = coreSegmentID.withProviderSegmentName(
+ providerName.empty() ? pose.providerName : providerName);
+
+ update.entityID = providerID.withEntityName(pose.objectID.str());
+ update.timeArrived = now;
+ update.timeCreated = pose.timestamp;
+ update.confidence = pose.confidence;
+
+ arondto::ObjectInstance dto;
+ toAron(dto, pose);
+ // Search for object class.
+ if (auto instance = findClassInstance(pose.objectID))
{
- update.entityID = providerSegmentID.withEntityName(pose.objectID.str());
- update.timeArrived = TimeUtil::GetTime();
- update.timeCreated = pose.timestamp;
- update.confidence = pose.confidence;
-
- arondto::ObjectInstance dto;
- toAron(dto, pose);
- // Search for object class.
- if (auto instance = findClassInstance(pose.objectID))
- {
- toAron(dto.classID, instance->id());
- }
- update.instancesData.push_back(dto.toAron());
+ toAron(dto.classID, instance->id());
}
-
+ else
+ {
+ toAron(dto.classID, MemoryID());
+ }
+ toAron(dto.sourceID, MemoryID());
+ update.instancesData.push_back(dto.toAron());
}
iceMemory.commit(commit);
}
@@ -203,12 +235,14 @@ namespace armarx::armem::server::obj::instance
return *coreSegment;
}
+
const wm::CoreSegment& Segment::getCoreSegment() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return *coreSegment;
}
+
objpose::ObjectPoseSeq Segment::getObjectPoses(IceUtil::Time now)
{
ObjectPoseSeq objectPoses = getLatestObjectPoses();
@@ -217,6 +251,7 @@ namespace armarx::armem::server::obj::instance
}
+
objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
@@ -227,6 +262,7 @@ namespace armarx::armem::server::obj::instance
return filterObjectPoses(objectPoses);
}
+
armem::wm::Entity* Segment::findObjectEntity(const ObjectID& objectID, const std::string& providerName)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
@@ -329,12 +365,14 @@ namespace armarx::armem::server::obj::instance
objectPose.updateAttached(agent);
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses() const
{
ARMARX_CHECK_NOT_NULL(coreSegment);
return getLatestObjectPoses(*coreSegment);
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg)
{
ObjectPoseSeq result;
@@ -342,6 +380,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
{
ObjectPoseSeq result;
@@ -349,6 +388,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
{
ObjectPose result;
@@ -356,6 +396,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
void Segment::getLatestObjectPoses(const armem::wm::CoreSegment& coreSeg, ObjectPoseSeq& out)
{
for (const auto& [_, provSegment] : coreSeg)
@@ -364,6 +405,7 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
{
for (const auto& [_, entity] : provSegment)
@@ -376,6 +418,7 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::getLatestObjectPose(const armem::wm::Entity& entity, ObjectPose& out)
{
for (const armem::wm::EntityInstance& instance : entity.getLatestSnapshot())
@@ -402,11 +445,13 @@ namespace armarx::armem::server::obj::instance
return data;
}
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
}
+
objpose::ProviderInfo Segment::getProviderInfo(const std::string& providerName)
{
try
@@ -427,7 +472,6 @@ namespace armarx::armem::server::obj::instance
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
@@ -517,6 +561,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input)
{
@@ -581,6 +626,7 @@ namespace armarx::armem::server::obj::instance
virtual bool visitEnter(armem::wm::Entity& entity) override;
};
+
bool DetachVisitor::visitEnter(armem::wm::Entity& entity)
{
const arondto::ObjectInstance data = owner.getLatestInstanceData(entity);
@@ -590,7 +636,6 @@ namespace armarx::armem::server::obj::instance
// Store non-attached pose in new snapshot.
owner.storeDetachedSnapshot(entity, data, now, commitAttachedPose);
}
-
return false; // Stop descending.
}
@@ -613,6 +658,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
void Segment::storeDetachedSnapshot(
armem::wm::Entity& entity,
const arondto::ObjectInstance& data,
@@ -648,14 +694,17 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID)
+ std::optional<wm::EntityInstance> Segment::findClassInstance(const ObjectID& objectID) const
{
const ObjectID classID = { objectID.dataset(), objectID.className() };
try
{
for (const auto& [_, provSeg] : iceMemory.workingMemory->getCoreSegment("Class"))
{
- return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ if (provSeg.hasEntity(classID.str()))
+ {
+ return provSeg.getEntity(classID.str()).getLatestSnapshot().getInstance(0);
+ }
}
return std::nullopt;
}
@@ -667,6 +716,181 @@ namespace armarx::armem::server::obj::instance
}
+ void Segment::storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ const nlohmann::json j = scene;
+ ARMARX_INFO << "Storing scene snapshot at: \n" << path.value() << "\n\n" << j.dump(2);
+ try
+ {
+ nlohmann::write_json(path.value(), j, 2);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Storing scene snapshot failed: \n" << e.what();
+ }
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::string& filename)
+ {
+ if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+ {
+ return loadScene(path.value());
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ std::optional<armem::obj::SceneSnapshot> Segment::loadScene(const std::filesystem::path& path)
+ {
+ ARMARX_INFO << "Loading scene snapshot from: \n" << path;
+ nlohmann::json j;
+ try
+ {
+ j = nlohmann::read_json(path);
+ }
+ catch (const std::ios_base::failure& e)
+ {
+ ARMARX_WARNING << "Loading scene snapshot failed: \n" << e.what();
+ return std::nullopt;
+ }
+
+ armem::obj::SceneSnapshot snapshot = j;
+ return snapshot;
+ }
+
+
+ const std::string Segment::timestampPlaceholder = "%TIMESTAMP";
+
+
+ std::optional<std::filesystem::path> Segment::resolveSceneFilename(const std::string& _filename)
+ {
+ std::string filename = _filename;
+
+ filename = simox::alg::replace_all(filename, timestampPlaceholder,
+ Time::now().toString("%Y-%m-%d_%H-%M-%S"));
+ if (not simox::alg::ends_with(filename, ".json"))
+ {
+ filename += ".json";
+ }
+
+ if (!finder)
+ {
+ finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
+ if (not finder->packageFound())
+ {
+ ARMARX_WARNING << "Could not find CMake package " << p.sceneSnapshotsPackage << ".";
+ }
+ }
+ if (finder->packageFound())
+ {
+ std::filesystem::path dataDir = finder->getDataDir();
+ std::filesystem::path path = dataDir / p.sceneSnapshotsPackage / "Scenes" / filename;
+ return path;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ armem::obj::SceneSnapshot Segment::getSceneSnapshot() const
+ {
+ armem::obj::SceneSnapshot scene;
+
+ wm::FunctionalVisitor visitor;
+ visitor.entityFn = [&scene](wm::Entity & entity)
+ {
+ try
+ {
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
+ if (objectInstance)
+ {
+ armem::obj::SceneSnapshot::Object& object = scene.objects.emplace_back();
+ // object.instanceID = entityInstance.id();
+ object.className = ObjectID(objectInstance->classID.entityName).getClassID().str();
+ object.collection = "";
+ object.position = simox::math::position(objectInstance->pose.objectPoseGlobal);
+ object.orientation = simox::math::orientation(objectInstance->pose.objectPoseGlobal);
+ }
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING_S << e.what();
+ }
+ return false;
+ };
+
+ visitor.applyTo(*coreSegment);
+ return scene;
+ }
+
+
+ void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName)
+ {
+ const Time now = TimeUtil::GetTime();
+ std::map<ObjectID, int> idCounters;
+
+ objpose::ObjectPoseSeq objectPoses;
+
+ for (const auto& object : scene.objects)
+ {
+ const ObjectID classID = object.getClassID(objectFinder);
+
+ objpose::ObjectPose& pose = objectPoses.emplace_back();
+
+ pose.providerName = sceneName;
+ pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.objectID = classID.withInstanceName(std::to_string(idCounters[classID]++));
+
+ pose.objectPoseGlobal = simox::math::pose(object.position, object.orientation);
+ pose.objectPoseRobot = pose.objectPoseGlobal;
+ pose.objectPoseOriginal = pose.objectPoseGlobal;
+ pose.objectPoseOriginalFrame = armarx::GlobalFrame;
+
+ pose.robotConfig = {};
+ pose.robotPose = Eigen::Matrix4f::Identity();
+
+ pose.confidence = 1.0;
+ pose.localOOBB = getObjectOOBB(classID);
+ pose.timestamp = now;
+ }
+
+ commitObjectPoses(objectPoses);
+ }
+
+
+ void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
+ {
+ ARMARX_INFO << "Loading scene snapshot '" << filename << "' ...";
+ if (auto path = resolveSceneFilename(filename))
+ {
+ if (const auto snapshot = loadScene(path.value()))
+ {
+ std::filesystem::path filename = path->filename();
+ filename.replace_extension(); // Removes extension
+
+ if (lockMemory)
+ {
+ std::scoped_lock lock(memoryMutex);
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ else
+ {
+ commitSceneSnapshot(snapshot.value(), filename.string());
+ }
+ }
+ }
+ }
+
void Segment::RemoteGui::setup(const Segment& data)
{
@@ -677,8 +901,21 @@ namespace armarx::armem::server::obj::instance
infiniteHistory.setValue(data.p.maxHistorySize == -1);
discardSnapshotsWhileAttached.setValue(data.p.discardSnapshotsWhileAttached);
+ storeLoadLine.setValue("Scene_" + timestampPlaceholder);
+ loadButton.setLabel("Load Scene");
+ storeButton.setLabel("Store Scene");
+
+ HBoxLayout storeLoadLineLayout({Label(data.p.sceneSnapshotsPackage + "/Scenes/"), storeLoadLine, Label(".json")});
+ HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
+
GridLayout grid;
int row = 0;
+
+ grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
+ row++;
+ grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
+ row++;
+
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
row++;
grid.add(Label("Infinite History Size"), {row, 0}).add(infiniteHistory, {row, 1});
@@ -690,8 +927,25 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
+
void Segment::RemoteGui::update(Segment& data)
{
+ if (loadButton.wasClicked())
+ {
+ bool lockMemory = true;
+ data.commitSceneSnapshotFromFilename(storeLoadLine.getValue(), lockMemory);
+ }
+
+ if (storeButton.wasClicked())
+ {
+ armem::obj::SceneSnapshot scene;
+ {
+ std::scoped_lock lock(data.memoryMutex);
+ scene = data.getSceneSnapshot();
+ }
+ data.storeScene(storeLoadLine.getValue(), scene);
+ }
+
if (infiniteHistory.hasValueChanged() || maxHistorySize.hasValueChanged()
|| discardSnapshotsWhileAttached.hasValueChanged())
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 5b7c268bb28530b82906bc07cee31431d6738f5c..0ac63b410d8d5593ac3c6b9be1fb9ea1cc58eb4d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -1,8 +1,9 @@
#pragma once
+#include <filesystem>
#include <map>
-#include <string>
#include <optional>
+#include <string>
#include <SimoxUtility/caching/CacheMap.h>
#include <SimoxUtility/shapes/OrientedBox.h>
@@ -23,6 +24,11 @@
#include "Decay.h"
+namespace armarx::armem::obj
+{
+ struct SceneSnapshot;
+}
+
namespace armarx::armem::server::obj::instance
{
@@ -53,7 +59,7 @@ namespace armarx::armem::server::obj::instance
const std::string& providerName,
const objpose::data::ProvidedObjectPoseSeq& providedPoses,
std::optional<Time> discardUpdatesUntil = std::nullopt);
- void commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses);
+ void commitObjectPoses(const ObjectPoseSeq& objectPoses, const std::string& providerName = "");
wm::CoreSegment& getCoreSegment();
@@ -129,12 +135,24 @@ namespace armarx::armem::server::obj::instance
bool commitAttachedPose);
- std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID);
+ std::optional<wm::EntityInstance> findClassInstance(const ObjectID& objectID) const;
friend struct DetachVisitor;
+ private:
+
+ void storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::string& filename);
+ std::optional<armem::obj::SceneSnapshot> loadScene(const std::filesystem::path& path);
+ std::optional<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+
+ armem::obj::SceneSnapshot getSceneSnapshot() const;
+ void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName);
+ void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);
+
+
public:
RobotStateComponentInterfacePrx robotStateComponent;
@@ -162,6 +180,10 @@ namespace armarx::armem::server::obj::instance
// -1 would be infinite, but this can let the RAM grow quickly.
long maxHistorySize = 25;
bool discardSnapshotsWhileAttached = true;
+
+ /// Package containing the scene snapshots
+ std::string sceneSnapshotsPackage = "ArmarXObjects";
+ std::string sceneSnapshotToLoad = "";
};
Properties p;
@@ -172,6 +194,9 @@ namespace armarx::armem::server::obj::instance
/// Class name -> dataset name.
simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+ std::unique_ptr<CMakePackageFinder> finder;
+
+ static const std::string timestampPlaceholder;
public:
@@ -179,6 +204,10 @@ namespace armarx::armem::server::obj::instance
{
armarx::RemoteGui::Client::GroupBox group;
+ armarx::RemoteGui::Client::LineEdit storeLoadLine;
+ armarx::RemoteGui::Client::Button storeButton;
+ armarx::RemoteGui::Client::Button loadButton;
+
armarx::RemoteGui::Client::IntSpinBox maxHistorySize;
armarx::RemoteGui::Client::CheckBox infiniteHistory;
armarx::RemoteGui::Client::CheckBox discardSnapshotsWhileAttached;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 3773c087bf70fa68981be86696b01c6fae7645d6..46a7a4fba84f5c81799d6e7330359b4e2ad7a52f 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -484,9 +484,9 @@ namespace armarx::armem::server::obj::instance
std::scoped_lock lock(adapter.visuMutex);
this->visu.update(adapter.visu);
}
+ this->segment.update(adapter.segment);
{
std::scoped_lock lock(adapter.memoryMutex);
- this->segment.update(adapter.segment);
this->decay.update(adapter.segment.decay);
}
{