From e5323e13939e11e1a4ded9ec08355071a1c355e4 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 24 Jun 2021 20:32:54 +0200
Subject: [PATCH] Implement scene snapshot loading
---
.../libraries/armem_objects/SceneSnapshot.cpp | 10 +-
.../libraries/armem_objects/SceneSnapshot.h | 5 +-
.../armem_objects/server/instance/Segment.cpp | 162 +++++++++++-------
.../armem_objects/server/instance/Segment.h | 10 +-
4 files changed, 118 insertions(+), 69 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
index 8c9297558..c410c7e35 100644
--- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
@@ -2,6 +2,8 @@
#include <SimoxUtility/json.h>
+// #include <RobotAPI/libraries/armem/core/json_conversions.h>
+
namespace armarx::armem::obj
{
@@ -10,18 +12,20 @@ namespace armarx::armem::obj
void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs)
{
+ // j["instanceID"] = rhs.instanceID;
j["class"] = rhs.classID.str();
j["collection"] = rhs.collection;
- j["postion"] = rhs.postion;
+ 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);
rhs.classID = ObjectID(j.at("class").get<std::string>());
- rhs.collection = j.at("collection");
- j.at("postion").get_to(rhs.postion);
+ j.at("collection").get_to(rhs.collection);
+ j.at("position").get_to(rhs.position);
j.at("orientation").get_to(rhs.orientation);
}
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
index 5fe5192a4..b7c9ed5d8 100644
--- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
+++ b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
@@ -26,6 +26,7 @@
#include <Eigen/Geometry>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+// #include <RobotAPI/libraries/armem/core/MemoryID.h>
namespace armarx::armem::obj
@@ -35,10 +36,12 @@ namespace armarx::armem::obj
{
struct Object
{
+ // MemoryID instanceID;
+
ObjectID classID;
std::string collection;
- Eigen::Vector3f postion = Eigen::Vector3f::Zero();
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
};
std::vector<Object> objects;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 29e3b2d56..8d876520b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -11,6 +11,8 @@
#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>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
@@ -167,43 +169,41 @@ 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());
}
-
+ toAron(dto.sourceID, MemoryID());
+ update.instancesData.push_back(dto.toAron());
}
iceMemory.commit(commit);
}
@@ -674,14 +674,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;
}
@@ -715,20 +718,7 @@ namespace armarx::armem::server::obj::instance
{
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;
+ return loadScene(path.value());
}
else
{
@@ -737,8 +727,27 @@ namespace armarx::armem::server::obj::instance
}
- static const std::string timestampPlaceholder = "%TIMESTAMP";
- static const std::string targetPackage = "ArmarXObjects";
+ 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";
+ const std::string Segment::targetPackage = "ArmarXObjects";
std::optional<std::filesystem::path> Segment::resolveSceneFilename(const std::string& _filename)
@@ -780,24 +789,24 @@ namespace armarx::armem::server::obj::instance
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);
+ 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.classID = objectInstance->classID.entityName;
+ 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();
}
- 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;
};
@@ -806,9 +815,35 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene) const
+ void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName)
{
- ARMARX_WARNING << "ToDo: Implement";
+ const Time now = TimeUtil::GetTime();
+ std::map<ObjectID, int> idCounters;
+
+ objpose::ObjectPoseSeq objectPoses;
+
+ for (const auto& object : scene.objects)
+ {
+ objpose::ObjectPose& pose = objectPoses.emplace_back();
+
+ pose.providerName = sceneName;
+ pose.objectType = objpose::ObjectTypeEnum::KnownObject;
+ pose.objectID = object.classID.withInstanceName(std::to_string(idCounters[object.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(object.classID);
+ pose.timestamp = now;
+ }
+
+ commitObjectPoses(objectPoses);
}
@@ -824,15 +859,16 @@ namespace armarx::armem::server::obj::instance
storeLoadLine.setValue("Scene_" + timestampPlaceholder);
loadButton.setLabel("Load Scene");
storeButton.setLabel("Store Scene");
- HBoxLayout storeLoadLineLayout;
- storeLoadLineLayout.addChildren({Label(targetPackage + "/Scenes/"), storeLoadLine, Label(".json")});
+
+ HBoxLayout storeLoadLineLayout({Label(targetPackage + "/Scenes/"), storeLoadLine, Label(".json")});
+ HBoxLayout storeLoadButtonsLayout({loadButton, storeButton});
GridLayout grid;
int row = 0;
grid.add(storeLoadLineLayout, {row, 0}, {1, 2});
row++;
- grid.add(loadButton, {row, 0}).add(storeButton, {row, 1});
+ grid.add(storeLoadButtonsLayout, {row, 0}, {1, 2});
row++;
grid.add(Label("Max History Size"), {row, 0}).add(maxHistorySize, {row, 1});
@@ -851,14 +887,16 @@ namespace armarx::armem::server::obj::instance
{
if (loadButton.wasClicked())
{
- ARMARX_IMPORTANT_S << "Store button clicked";
- if (const auto snapshot = data.loadScene(storeLoadLine.getValue()))
+ if (auto path = data.resolveSceneFilename(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 (const auto snapshot = data.loadScene(path.value()))
+ {
+ std::filesystem::path filename = path->filename();
+ filename.replace_extension(); // Removes extension
+
+ std::scoped_lock lock(data.memoryMutex);
+ data.commitSceneSnapshot(snapshot.value(), filename.string());
+ }
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 910b816e8..57e918dd0 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -59,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();
@@ -135,7 +135,7 @@ 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;
@@ -145,10 +145,11 @@ namespace armarx::armem::server::obj::instance
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;
+ void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName);
public:
@@ -190,6 +191,9 @@ namespace armarx::armem::server::obj::instance
std::unique_ptr<CMakePackageFinder> finder;
+ static const std::string timestampPlaceholder;
+ static const std::string targetPackage;
+
public:
--
GitLab