Merge branch...

Merge branch '121-object-memory-allow-specifying-scene-snapshot-via-standard-armarx-data-path-in-addition-to' into 'master'

Object Memory: Allow specifying scene snapshot via standard ArmarX Data path (in addition to inside PriorKnowledgeData), similar to simulator

Closes #121

See merge request !307

scenarios/ArMemObjectMemory/config/ArticulatedObjectLocalizerExample.cfg

@@ -93,6 +93,22 @@ ArmarX.ArticulatedObjectLocalizerExample.mem.obj.articulated.write.ProviderName
 # ArmarX.ArticulatedObjectLocalizerExample.mns.MemoryNameSystemName = MemoryNameSystem
 
 
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class:  
+#  Attributes:
+#  - Default:            mobile-dishwasher
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.class = mobile-dishwasher
+
+
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset:  
+#  Attributes:
+#  - Default:            Kitchen
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ArticulatedObjectLocalizerExample.p.obj.dataset = Kitchen
+
+
 # ArmarX.ArticulatedObjectLocalizerExample.tpc.pub.DebugObserver:  Name of the `DebugObserver` topic to publish data to.
 #  Attributes:
 #  - Default:            DebugObserver

scenarios/ArMemObjectMemory/config/ObjectMemory.cfg

@@ -150,6 +150,22 @@
 # ArmarX.ObjectMemory.cmp.KinematicUnitObserverName = KinematicUnitObserver
 
 
+# ArmarX.ObjectMemory.mem..marker.Name:  Marker Memory Name
+#  Attributes:
+#  - Default:            Marker
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem..marker.Name = Marker
+
+
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize:  Maximum marker memory history size
+#  Attributes:
+#  - Default:            -1
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem..marker.maxHistorySize = -1
+
+
 # ArmarX.ObjectMemory.mem.MemoryName:  Name of this memory server.
 #  Attributes:
 #  - Default:            Object
@@ -375,9 +391,12 @@ ArmarX.ObjectMemory.mem.inst.robots.FallbackName = CameraOnTripod
 # ArmarX.ObjectMemory.mem.inst.scene.11_Directory = scenes
 
 
-# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad:  Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').
-# You can also specify paths relative to 'Package/scenes/'. 
-# You can also specify a ; separated list of scenes.
+# ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad:  Scene(s) to load on startup. 
+# Specify multiple scenes in a ; separated list. 
+# Each entry must be one of the following: 
+# (1) A scene file in 'Package/scenes/' (with or without '.json' extension), e.g. 'MyScene', 'MyScene.json' 
+# (2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), e.g. 'path/to/MyScene', 'path/to/MyScene.json' 
+# (3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'
 #  Attributes:
 #  - Default:            ""
 #  - Case sensitivity:   yes
@@ -496,13 +515,39 @@ ArmarX.ObjectMemory.mem.inst.scene.12_SnapshotToLoad = R003
 # ArmarX.ObjectMemory.mem.inst.visu.oobbs = false
 
 
-# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show:  Show arrows linearly predicting object positions.
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha:  Alpha of linear prediction ghosts.
+#  Attributes:
+#  - Default:            0.699999988
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.ghostAlpha = 0.699999988
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow:  Show arrows from current object poses to the linearly predicted ones.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showArrow = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame:  Show frames at linearly predicted object poses.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showFrame = false
+
+
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost:  Show ghosts at linearly predicted object poses.
 #  Attributes:
 #  - Default:            false
 #  - Case sensitivity:   yes
 #  - Required:           no
 #  - Possible values: {0, 1, false, no, true, yes}
-# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.show = false
+# ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.showGhost = false
 
 
 # ArmarX.ObjectMemory.mem.inst.visu.predictions.linear.timeOffset:  The offset (in seconds) to the current time to make predictions for.

scenarios/ArMemObjectMemory/config/RobotStateMemory.cfg

@@ -274,6 +274,15 @@
 # ArmarX.RobotStateMemory.mem.visu.enabled = true
 
 
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled:  Enable or disable visualization of frames.
+#  Attributes:
+#  - Default:            false
+#  - Case sensitivity:   yes
+#  - Required:           no
+#  - Possible values: {0, 1, false, no, true, yes}
+# ArmarX.RobotStateMemory.mem.visu.famesEnabled = false
+
+
 # ArmarX.RobotStateMemory.mem.visu.frequenzyHz:  Frequency of visualization.
 #  Attributes:
 #  - Default:            25
@@ -300,6 +309,14 @@
 # ArmarX.RobotStateMemory.mns.MemoryNameSystemName = MemoryNameSystem
 
 
+# ArmarX.RobotStateMemory.prediction.TimeWindow:  Duration of time window into the past to use for predictions when requested via the PredictingMemoryInterface (in seconds).
+#  Attributes:
+#  - Default:            2
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.RobotStateMemory.prediction.TimeWindow = 2
+
+
 # ArmarX.SecondsStartupDelay:  The startup will be delayed by this number of seconds (useful for debugging)
 #  Attributes:
 #  - Default:            0

scenarios/ArMemObjectMemory/config/SimpleVirtualRobot.cfg

@@ -170,6 +170,14 @@
 # ArmarX.SimpleVirtualRobot.p.oneShot = true
 
 
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues:  Specify a certain joint configuration.
+#  Attributes:
+#  - Default:            ""
+#  - Case sensitivity:   yes
+#  - Required:           no
+# ArmarX.SimpleVirtualRobot.p.robot.jointValues = ""
+
+
 # ArmarX.SimpleVirtualRobot.p.robot.name:  Optional override for the robot name. If not set, the default name from the robot model is used.
 #  Attributes:
 #  - Default:            ""

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -119,10 +119,20 @@ namespace armarx::armem::server::obj::instance
                        "Scene snapshots are expected to be located in Package/data/Package/Scenes/*.json.");
         defs->optional(p.sceneSnapshotsDirectory, prefix + "scene.11_Directory",
                        "Directory in Package/data/Package/ containing the scene snapshots.");
-        defs->optional(p.sceneSnapshotToLoad, prefix + "scene.12_SnapshotToLoad",
-                       "Scene to load on startup (e.g. 'Scene_2021-06-24_20-20-03').\n"
-                       "You can also specify paths relative to 'Package/scenes/'. \n"
-                       "You can also specify a ; separated list of scenes.");
+
+        std::vector<std::string> sceneSnapshotToLoadDescription =
+        {
+            "Scene(s) to load on startup.",
+            "Specify multiple scenes in a ; separated list.",
+            "Each entry must be one of the following:",
+            "(1) A scene file in 'Package/scenes/' (with or without '.json' extension), "
+            "e.g. 'MyScene', 'MyScene.json'",
+            "(2) A path to a scene file relative to 'Package/scenes/' (with or without '.json' extension), "
+            "e.g. 'path/to/MyScene', 'path/to/MyScene.json'",
+            "(3) An ArmarX data path to a scene file, e.g. 'Package/scenes/path/to/MyScene.json'",
+        };
+        defs->optional(p.sceneSnapshotsToLoad, prefix + "scene.12_SnapshotToLoad",
+                       simox::alg::join(sceneSnapshotToLoadDescription, " \n"));
 
         decay.defineProperties(defs, prefix + "decay.");
     }
@@ -132,10 +142,10 @@ namespace armarx::armem::server::obj::instance
     {
         SpecializedCoreSegment::init();
 
-        if (not p.sceneSnapshotToLoad.empty())
+        if (not p.sceneSnapshotsToLoad.empty())
         {
             bool trim = true;
-            const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotToLoad, ";", trim);
+            const std::vector<std::string> scenes = simox::alg::split(p.sceneSnapshotsToLoad, ";", trim);
             for (const std::string& scene : scenes)
             {
                 const bool lockMemory = false;
@@ -934,7 +944,7 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
     {
-        if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+        if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
         {
 
             ARMARX_INFO << "Storing scene snapshot at: \n" << path.value();
@@ -953,7 +963,7 @@ namespace armarx::armem::server::obj::instance
     std::optional<armarx::objects::Scene>
     Segment::loadScene(const std::string& filename)
     {
-        if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
+        if (const std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
         {
             return loadScene(path.value());
         }
@@ -983,17 +993,18 @@ namespace armarx::armem::server::obj::instance
 
 
     std::optional<std::filesystem::path>
-    Segment::resolveSceneFilename(const std::string& _filename)
+    Segment::resolveSceneFilepath(const std::string& _filename)
     {
-        std::string filename = _filename;
+        std::string filepath = _filename;
 
-        filename = simox::alg::replace_all(filename, timestampPlaceholder,
+        filepath = simox::alg::replace_all(filepath, timestampPlaceholder,
                                            Time::Now().toString("%Y-%m-%d_%H-%M-%S"));
-        if (not simox::alg::ends_with(filename, ".json"))
+        if (not simox::alg::ends_with(filepath, ".json"))
         {
-            filename += ".json";
+            filepath += ".json";
         }
 
+        // Try to interprete it as relative to 'Package/scenes/'.
         if (!finder)
         {
             finder.reset(new CMakePackageFinder(p.sceneSnapshotsPackage));
@@ -1004,14 +1015,26 @@ namespace armarx::armem::server::obj::instance
         }
         if (finder->packageFound())
         {
-            std::filesystem::path dataDir = finder->getDataDir();
-            std::filesystem::path path = dataDir / p.sceneSnapshotsPackage / p.sceneSnapshotsDirectory / filename;
-            return path;
+            std::filesystem::path absDataDir = finder->getDataDir();
+            std::filesystem::path absPath = absDataDir / p.sceneSnapshotsPackage
+                    / p.sceneSnapshotsDirectory / filepath;
+            if (std::filesystem::is_regular_file(absPath))
+            {
+                return absPath;
+            }
         }
-        else
+
+        // Try to interprete it as ArmarXDataPath.
         {
-            return std::nullopt;
+            std::string resolved = ArmarXDataPath::resolvePath(filepath);
+            if (resolved != filepath)
+            {
+                return resolved;
+            }
         }
+
+        // Else: Fail
+        return std::nullopt;
     }
 
 
@@ -1114,25 +1137,40 @@ namespace armarx::armem::server::obj::instance
 
     void Segment::commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory)
     {
-        ARMARX_INFO << "Loading scene snapshot '" << filename << "' ...";
-        if (auto path = resolveSceneFilename(filename))
+        std::stringstream ss;
+        ss << "Loading scene '" << filename << "' ...";
+        if (std::optional<std::filesystem::path> path = resolveSceneFilepath(filename))
         {
+            ARMARX_INFO << ss.str() << "\nfrom " << path.value();
+
             if (const auto snapshot = loadScene(path.value()))
             {
-                std::filesystem::path filename = path->filename();
-                filename.replace_extension();  // Removes extension
+                auto makeSceneName = [](const std::filesystem::path& path)
+                {
+                    std::filesystem::path filename = path.filename();
+                    filename.replace_extension();  // Removes extension
+                    return filename.string();
+                };
+                std::string sceneName = makeSceneName(path.value());
 
                 // The check seems useless?
                 if (lockMemory)
                 {
-                    commitSceneSnapshot(snapshot.value(), filename.string());
+                    segmentPtr->doLocked([this,&snapshot, &sceneName]()
+                    {
+                        commitSceneSnapshot(snapshot.value(), sceneName);
+                    });
                 }
                 else
                 {
-                    commitSceneSnapshot(snapshot.value(), filename.string());
+                    commitSceneSnapshot(snapshot.value(), sceneName);
                 }
             }
         }
+        else
+        {
+            ARMARX_INFO << ss.str() << " failed: Could not resolve scene name or path.";
+        }
     }
 
 

source/RobotAPI/libraries/armem_objects/server/instance/Segment.h

@@ -160,7 +160,7 @@ namespace armarx::armem::server::obj::instance
         void storeScene(const std::string& filename, const armarx::objects::Scene& scene);
         std::optional<armarx::objects::Scene> loadScene(const std::string& filename);
         std::optional<armarx::objects::Scene> loadScene(const std::filesystem::path& path);
-        std::optional<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+        std::optional<std::filesystem::path> resolveSceneFilepath(const std::string& filename);
 
         armarx::objects::Scene getSceneSnapshot() const;
         void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
@@ -200,7 +200,7 @@ namespace armarx::armem::server::obj::instance
             /// Package containing the scene snapshots
             std::string sceneSnapshotsPackage = armarx::ObjectFinder::DefaultObjectsPackageName;
             std::string sceneSnapshotsDirectory = "scenes";
-            std::string sceneSnapshotToLoad = "";
+            std::string sceneSnapshotsToLoad = "";
         };
         Properties p;