Skip to content
Snippets Groups Projects
Commit 6722607e authored by Rainer Kartmann's avatar Rainer Kartmann
Browse files

Add save and load buttons, implement saving 

# Conflicts:
#	source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
parent 6dfd04d2
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+ 183
− 3
View file @ 6722607e
#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/workingmemory/Visitor.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())
{
......
source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+ 23
− 1
View file @ 6722607e
#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;
......
source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+ 1
− 1
View file @ 6722607e
......@@ -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);
}
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment