From fae88f38b8fbb50fd47d6c7453ffde53d1b0904c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 24 Jun 2021 18:39:34 +0200
Subject: [PATCH] Add save and load buttons, implement saving
---
.../armem_objects/server/instance/Segment.cpp | 184 +++++++++++++++++-
.../armem_objects/server/instance/Segment.h | 24 ++-
.../server/instance/SegmentAdapter.cpp | 2 +-
3 files changed, 206 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 7b7dc7a42..9d2486baa 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,12 +1,15 @@
#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/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
@@ -17,10 +20,16 @@
#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 <Eigen/Geometry>
#include <sstream>
+
namespace armarx::armem::server::obj::instance
{
@@ -59,6 +68,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.");
@@ -70,6 +80,7 @@ namespace armarx::armem::server::obj::instance
decay.defineProperties(defs, prefix + "decay.");
}
+
void Segment::init()
{
ARMARX_CHECK_NOT_NULL(iceMemory.workingMemory);
@@ -161,6 +172,7 @@ namespace armarx::armem::server::obj::instance
return stats;
}
+
void Segment::commitObjectPoses(const std::string& providerName, const ObjectPoseSeq& objectPoses)
{
ARMARX_CHECK_NOT_NULL(coreSegment);
@@ -203,12 +215,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 +231,7 @@ namespace armarx::armem::server::obj::instance
}
+
objpose::ObjectPoseSeq Segment::getObjectPosesByProvider(
const std::string& providerName,
IceUtil::Time now)
@@ -227,6 +242,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 +345,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 +360,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPoseSeq Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSeg)
{
ObjectPoseSeq result;
@@ -349,6 +368,7 @@ namespace armarx::armem::server::obj::instance
return result;
}
+
objpose::ObjectPose Segment::getLatestObjectPose(const armem::wm::Entity& entity)
{
ObjectPose result;
@@ -356,6 +376,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 +385,7 @@ namespace armarx::armem::server::obj::instance
}
}
+
void Segment::getLatestObjectPoses(const armem::wm::ProviderSegment& provSegment, ObjectPoseSeq& out)
{
for (const auto& [_, entity] : provSegment)
@@ -376,6 +398,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 +425,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 +452,6 @@ namespace armarx::armem::server::obj::instance
}
-
objpose::AttachObjectToRobotNodeOutput
Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
{
@@ -517,6 +541,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
objpose::DetachObjectFromRobotNodeOutput Segment::detachObjectFromRobotNode(
const objpose::DetachObjectFromRobotNodeInput& input)
{
@@ -581,6 +606,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 +616,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 +638,7 @@ namespace armarx::armem::server::obj::instance
return output;
}
+
void Segment::storeDetachedSnapshot(
armem::wm::Entity& entity,
const arondto::ObjectInstance& data,
@@ -667,6 +693,124 @@ 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))
+ {
+ ARMARX_INFO << "Loading scene snapshot from: \n" << path.value();
+ nlohmann::json j;
+ try
+ {
+ j = nlohmann::read_json(path.value());
+ }
+ 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;
+ }
+ else
+ {
+ return std::nullopt;
+ }
+ }
+
+
+ static const std::string timestampPlaceholder = "%TIMESTAMP";
+ static const std::string targetPackage = "ArmarXObjects";
+
+
+ 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(targetPackage));
+ if (not finder->packageFound())
+ {
+ ARMARX_WARNING << "Could not find CMake package " << targetPackage << ".";
+ }
+ }
+ if (finder->packageFound())
+ {
+ std::filesystem::path dataDir = finder->getDataDir();
+ std::filesystem::path path = dataDir / targetPackage / "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)
+ {
+ ARMARX_IMPORTANT_S << "Processing entity " << entity.id();
+ std::optional<arondto::ObjectInstance> instance;
+ try
+ {
+ const wm::EntityInstance& entityInstance = entity.getLatestSnapshot().getInstance(0);
+ instance = tryCast<arondto::ObjectInstance>(entityInstance);
+ }
+ catch (const armem::error::ArMemError& e)
+ {
+ ARMARX_WARNING_S << e.what();
+ }
+ if (instance)
+ {
+ armem::obj::SceneSnapshot::Object& object = scene.objects.emplace_back();
+ object.classID = instance->classID.entityName;
+ object.postion = simox::math::position(instance->pose.objectPoseGlobal);
+ object.orientation = simox::math::orientation(instance->pose.objectPoseGlobal);
+ }
+ return false;
+ };
+
+ visitor.applyTo(*coreSegment);
+ return scene;
+ }
+
+
+ void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene) const
+ {
+ ARMARX_WARNING << "ToDo: Implement";
+ }
+
void Segment::RemoteGui::setup(const Segment& data)
{
@@ -677,8 +821,20 @@ 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;
+ storeLoadLineLayout.addChildren({Label(targetPackage + "/Scenes/"), storeLoadLine, Label(".json")});
+
GridLayout grid;
int row = 0;
+
+ grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
+ row++;
+ grid.add(loadButton, {row, 0}).add(storeButton, {row, 1});
+ 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 +846,32 @@ namespace armarx::armem::server::obj::instance
group.addChild(grid);
}
+
void Segment::RemoteGui::update(Segment& data)
{
+ if (loadButton.wasClicked())
+ {
+ ARMARX_IMPORTANT_S << "Store button clicked";
+ if (const auto snapshot = data.loadScene(storeLoadLine.getValue()))
+ {
+ ARMARX_IMPORTANT_S << "Snapshot loaded";
+ std::scoped_lock lock(data.memoryMutex);
+ ARMARX_IMPORTANT_S << "Committing";
+ data.commitSceneSnapshot(snapshot.value());
+ ARMARX_IMPORTANT_S << "Committed";
+ }
+ }
+
+ 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 5b7c268bb..910b816e8 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
{
@@ -135,6 +141,16 @@ namespace armarx::armem::server::obj::instance
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<std::filesystem::path> resolveSceneFilename(const std::string& filename);
+
+ armem::obj::SceneSnapshot getSceneSnapshot() const;
+ void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene) const;
+
+
public:
RobotStateComponentInterfacePrx robotStateComponent;
@@ -172,6 +188,8 @@ namespace armarx::armem::server::obj::instance
/// Class name -> dataset name.
simox::caching::CacheMap<std::string, std::string> classNameToDatasetCache;
+ std::unique_ptr<CMakePackageFinder> finder;
+
public:
@@ -179,6 +197,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 3773c087b..46a7a4fba 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);
}
{
--
GitLab