diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
index 4b8db8b5486785744603c2da09dc4993f368dba3..32f1fe6d5fbc139ca5406e0af0fde119bdbd2717 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.cpp
@@ -2,6 +2,7 @@
#include <RobotAPI/components/InteractiveMemoryEditor/InteractionObserver.h>
#include <RobotAPI/libraries/ArmarXObjects/ProvidedObjectPose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <utility>
@@ -20,7 +21,13 @@ namespace armarx
, isMemoryVizRequired(true)
, isMetaVizRequired(true)
{
+ placeholderOptions.emplace_back("Remove");
+ placeholderOptions.emplace_back(lineString);
+ for (auto const& availableObject : this->properties.availableObjects)
+ {
+ placeholderOptions.push_back(armarx::fromIce(availableObject).str());
+ }
}
void Editor::step()
@@ -135,7 +142,7 @@ namespace armarx
}
}
- changes.visualizeNewObjects(this);
+ changes.visualizeNewObjects();
}
void Editor::visualizeObject(objpose::ObjectPose& objectPose)
@@ -221,23 +228,20 @@ namespace armarx
{
observer.clearObservedLayer(metaLayer);
- placeholders.visualizePlaceholders(this);
+ placeholders.visualizePlaceholders();
}
void Editor::visualizePlaceholder(PlaceholderState::Placeholder const& placeholder, size_t id)
{
viz::InteractionDescription interaction = viz::interaction()
- .selection()
- .transform()
- .hideDuringTransform()
- .contextMenu({"Remove"});
+ .selection().transform().hideDuringTransform().contextMenu(placeholderOptions);
viz::Box box = viz::Box("placeholder_" + std::to_string(id))
.set(placeholder.box.transformed(placeholder.transform))
.color(simox::Color::blue())
.enable(interaction);
- observer.addObserved(metaLayer, box)
+ auto& observation = observer.addObserved(metaLayer, box)
.onContextMenu(0, [this, id]()
{
placeholders.removePlaceholder(id);
@@ -248,6 +252,19 @@ namespace armarx
placeholders.movePlaceholder(id, transform);
isMetaVizRequired = true;
});
+
+ for (size_t index = 2; index < placeholderOptions.size(); index++)
+ {
+ std::string const& object = placeholderOptions[index];
+
+ observation.onContextMenu(index, [this, id, &object]
+ {
+ placeholders.specifyObject(id, object, changes);
+
+ isMetaVizRequired = true;
+ isMemoryVizRequired = true;
+ });
+ }
}
void Editor::ChangeState::clear()
@@ -282,7 +299,7 @@ namespace armarx
return isChanged;
}
- void Editor::ChangeState::visualizeNewObjects(Editor* editor)
+ void Editor::ChangeState::visualizeNewObjects()
{
for (objpose::ObjectPose & objectPose: newPoses)
{
@@ -347,7 +364,7 @@ namespace armarx
description.options.emplace_back("Create Placeholder");
description.prototypeIndex = currentIndex++;
- description.options.emplace_back("---------------------------");
+ description.options.emplace_back(lineString);
currentIndex++;
description.options.emplace_back("Commit All Changes");
@@ -392,6 +409,36 @@ namespace armarx
clonedChange.transform *= originalChange.transform;
}
}
+
+ void Editor::ChangeState::createObject(const std::string &objectID, const Eigen::Matrix4f &pose)
+ {
+ std::string suffix = std::to_string(std::chrono::duration_cast<std::chrono::seconds>(
+ std::chrono::system_clock::now().time_since_epoch()).count());
+
+ objpose::ObjectPose& newPose = newPoses.emplace_back();
+
+ armarx::ObjectID id(objectID);
+ newPose.objectID = id.withInstanceName(id.instanceName() + suffix);
+
+ newPose.providerName = editor->properties.providerName;
+ newPose.objectType = objpose::ObjectType::KnownObject;
+ newPose.isStatic = true;
+
+ newPose.objectPoseGlobal = pose;
+ newPose.confidence = 1;
+ newPose.timestamp = DateTime::Now();
+
+ std::optional<armarx::ObjectInfo> info = editor->properties.getObjectInfo(id);
+ if (info.has_value())
+ {
+ newPose.localOOBB = info->loadOOBB();
+ }
+
+ Change& createdChange = changed[newPose.objectID.str()];
+ createdChange.kind = CREATE;
+ createdChange.transform = Eigen::Matrix4f::Identity();
+ createdChange.iterator = std::prev(newPoses.end());
+ }
void Editor::ChangeState::deleteObject(const objpose::ObjectPose& object)
{
@@ -436,14 +483,14 @@ namespace armarx
void Editor::PlaceholderState::addPlaceholder(simox::OrientedBoxf box)
{
- size_t id = getId();
+ size_t id = getID();
auto& [placeholder, isActive] = placeholders[id];
placeholder = { .box = std::move(box), .transform = Eigen::Matrix4f::Identity() };
isActive = true;
}
- void Editor::PlaceholderState::visualizePlaceholders(Editor *editor)
+ void Editor::PlaceholderState::visualizePlaceholders()
{
for (size_t id = 0; id < placeholders.size(); id++)
{
@@ -467,20 +514,30 @@ namespace armarx
auto& [placeholder, isActive] = placeholders[id];
isActive = false;
- unusedIds.push(id);
+ unusedIDs.push(id);
}
- size_t Editor::PlaceholderState::getId()
+ size_t Editor::PlaceholderState::getID()
{
- if (unusedIds.empty())
+ if (unusedIDs.empty())
{
size_t id = placeholders.size();
placeholders.emplace_back(std::make_pair(Placeholder(), false));
return id;
}
- size_t id = unusedIds.top();
- unusedIds.pop();
+ size_t id = unusedIDs.top();
+ unusedIDs.pop();
return id;
}
+
+ void Editor::PlaceholderState::specifyObject(size_t id, std::string const& objectID, ChangeState& changeState)
+ {
+ auto& [placeholder, isActive] = placeholders[id];
+
+ Eigen::Matrix4f pose = placeholder.transform * placeholder.box.transformation();
+ changeState.createObject(objectID, pose);
+
+ removePlaceholder(id);
+ }
} // namespace armarx
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
index 5b9a4a7d68018b916707511c1bd424c6c9814eb1..6ec576d1981601221d18712f984666e05a257ed0 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/Editor.h
@@ -17,8 +17,12 @@ namespace armarx
struct Properties
{
std::string providerName;
+
float const& objectScaling;
float const& confidenceThreshold;
+
+ std::vector<data::ObjectID> const& availableObjects;
+ std::function<std::optional<armarx::ObjectInfo>(armarx::ObjectID const&)> getObjectInfo;
};
explicit Editor(viz::Client& client,
@@ -30,6 +34,9 @@ namespace armarx
private:
static constexpr const char* memoryLayerName = "Memory";
static constexpr const char* metaLayerName = "Meta";
+ static constexpr const char* lineString = "---------------------------";
+
+ std::vector<std::string> placeholderOptions;
Properties properties;
@@ -65,14 +72,17 @@ namespace armarx
class ChangeState
{
public:
+ explicit ChangeState(Editor* editor) : editor(editor) {} ;
+
void clear();
void moveNewObjectsTo(objpose::ObjectPoseSeq& seq);
bool applyTo(objpose::ObjectPose& pose);
- void visualizeNewObjects(Editor* editor);
+ void visualizeNewObjects();
VisualizationDescription buildVisualizationDescription(objpose::ObjectPose& object);
Eigen::Matrix4f getTransform(objpose::ObjectPose const& object);
void cloneObject(objpose::ObjectPose const& object);
+ void createObject(std::string const& objectID, Eigen::Matrix4f const& pose);
void deleteObject(objpose::ObjectPose const& object);
void resetObject(objpose::ObjectPose const& object);
void moveObject(objpose::ObjectPose const& object, Eigen::Matrix4f const& transform);
@@ -94,15 +104,19 @@ namespace armarx
ObjectPoseLst::iterator iterator {};
};
+ Editor* editor;
+
ObjectPoseLst newPoses;
std::map<std::string, Change> changed;
};
- ChangeState changes;
+ ChangeState changes{this};
class PlaceholderState
{
public:
+ explicit PlaceholderState(Editor* editor) : editor(editor) {} ;
+
struct Placeholder
{
simox::OrientedBoxf box;
@@ -110,18 +124,21 @@ namespace armarx
};
void addPlaceholder(simox::OrientedBoxf box);
- void visualizePlaceholders(Editor* editor);
+ void visualizePlaceholders();
void movePlaceholder(size_t id, Eigen::Matrix4f const& transform);
void removePlaceholder(size_t id);
+ void specifyObject(size_t id, std::string const& objectID, ChangeState& changeState);
private:
- size_t getId();
+ size_t getID();
+
+ Editor* editor;
- std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIds;
+ std::priority_queue<size_t, std::vector<size_t>, std::greater<>> unusedIDs;
std::vector<std::pair<Placeholder, bool>> placeholders;
};
- PlaceholderState placeholders;
+ PlaceholderState placeholders{this};
bool isCommitRequired;
bool isUpdateRequired;
diff --git a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
index 0d5b646a8fe83ffbc1b5c87b0b6ce8884779d413..6bab9222d2101a343d9241374e0b7580c850e687 100644
--- a/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
+++ b/source/RobotAPI/components/InteractiveMemoryEditor/InteractiveMemoryEditor.cpp
@@ -31,10 +31,15 @@ namespace armarx
{
{
providerInfo.objectType = objpose::ObjectType::KnownObject;
- std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset("KIT");
- for (const auto &obj: objects)
+
+ for (const auto& dataset : objectFinder.getDatasets())
{
- providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
+ std::vector<ObjectInfo> objects = objectFinder.findAllObjectsOfDataset(dataset);
+
+ for (const auto& obj: objects)
+ {
+ providerInfo.supportedObjects.push_back(armarx::toIce(obj.id()));
+ }
}
}
}
@@ -70,7 +75,9 @@ namespace armarx
{
.providerName = getName(),
.objectScaling = objectScaling,
- .confidenceThreshold = confidenceThreshold
+ .confidenceThreshold = confidenceThreshold,
+ .availableObjects = providerInfo.supportedObjects,
+ .getObjectInfo = [this](armarx::ObjectID const& id) { return objectFinder.findObject(id); }
};
Editor editor(arviz, properties,