diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 374fae4c45ac36a164b11136d08f6e43384e9933..19d3e19df844731d355a05a95986216d68ead88a 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -214,7 +214,6 @@ namespace armarx
{
throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
}
-
return this->_synchronizedPrx;
}
@@ -357,6 +356,7 @@ namespace armarx
insertPose(time, globalRobotPose.transform);
_synchronized->setGlobalPose(globalRobotPose.transform);
+ _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
if (_sharedRobotServant)
{
@@ -703,12 +703,25 @@ namespace armarx
const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+
+ TransformStamped stamped;
+ stamped.header.frame = armarx::GlobalFrame;
+ stamped.header.agent = _synchronized->getName();
+ stamped.header.timestampInMicroSeconds = time.toMicroSeconds();
+ stamped.header.parentFrame = "";
+ stamped.transform = globalPose;
+
+ this->reportGlobalRobotPose(stamped);
+
+ /*
+ * old:
insertPose(time, globalPose);
if (_sharedRobotServant)
{
_sharedRobotServant->setTimestamp(time);
}
+ */
}
diff --git a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
index d288c472e284872a4baaba971ba728214476d26b..e59bbc11194b5c279e6f1278d9cbf60f57c2b8b1 100644
--- a/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
+++ b/source/RobotAPI/components/RobotToArViz/RobotToArViz.cpp
@@ -64,8 +64,10 @@ namespace armarx
void RobotToArViz::onConnectComponent()
{
// Load robot.
- this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
-
+ if (!RobotState::hasRobot(robotName))
+ {
+ this->robot = RobotState::addRobot(robotName, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ }
// Initialize robot visu element.
this->robotViz = viz::Robot(robot->getName());
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
index 07b09a338d33e9aade018ae890d712ca7c69a819..98788a3a67c1803f64d97d65a101223f0685e810 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.cpp
@@ -27,7 +27,9 @@
#include <SimoxUtility/algorithm/string.h>
#include <RobotAPI/libraries/armem/core/error.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
+#include <RobotAPI/libraries/armem/core/ice_conversions_templates.h>
#include <RobotAPI/components/armem/server/ExampleMemory/aron/ExampleData.aron.generated.h>
@@ -116,6 +118,88 @@ namespace armarx
+ // ACTIONS
+ armem::actions::GetActionsOutputSeq
+ ExampleMemory::getActions(
+ const armem::actions::GetActionsInputSeq& input)
+ {
+ using namespace armem::actions;
+ Action greeting{"hi", "Say hello to " + input[0].id.entityName};
+ Action failure{"fail", "Fail dramatically"};
+ Action nothing{"null", "Do nothing, but deeply nested"};
+
+ SubMenu one{"one", "One", { nothing } };
+ SubMenu two{"two", "Two", { one } };
+ SubMenu three{"three", "Three", { two } };
+ SubMenu four{"four", "Four", { three } };
+
+ Menu menu {
+ greeting,
+ failure,
+ four,
+ SubMenu{"mut", "Mutate", {
+ Action{"copy", "Copy latest instance"}
+ }}
+ };
+
+ return {{ menu.toIce() }};
+ }
+
+ armem::actions::ExecuteActionOutputSeq ExampleMemory::executeActions(
+ const armem::actions::ExecuteActionInputSeq& input)
+ {
+ using namespace armem::actions;
+
+ ExecuteActionOutputSeq output;
+ for (const auto& [id, path] : input)
+ {
+ armem::MemoryID memoryID = armem::fromIce<armem::MemoryID>(id);
+ if (path == ActionPath{"hi"})
+ {
+ ARMARX_INFO << "Hello, " << memoryID.str() << "!";
+ output.emplace_back(true, "");
+ }
+ else if (path == ActionPath{"fail"})
+ {
+ ARMARX_WARNING << "Alas, I am gravely wounded!";
+ output.emplace_back(false, "Why would you do that to him?");
+ }
+ else if (not path.empty() and path.front() == "four" and path.back() == "null")
+ {
+ // Do nothing.
+ ARMARX_INFO << "Nested action (path: " << path << ")";
+ output.emplace_back(true, "");
+ }
+ else if (path == ActionPath{"mut", "copy"})
+ {
+ auto* instance = workingMemory().findLatestInstance(memoryID);
+ if (instance != nullptr)
+ {
+ armem::EntityUpdate update;
+ armem::MemoryID newID = memoryID.getCoreSegmentID()
+ .withProviderSegmentName(memoryID.providerSegmentName)
+ .withEntityName(memoryID.entityName);
+ update.entityID = newID;
+ update.timeCreated = armem::Time::now();
+ update.instancesData = { instance->data() };
+
+ armem::Commit newCommit;
+ this->commit(armem::toIce<armem::data::Commit>(newCommit));
+
+ tab.rebuild = true;
+ ARMARX_INFO << "Duplicated " << memoryID;
+ output.emplace_back(true, "");
+ }
+ else
+ {
+ output.emplace_back(false, "Couldn't duplicate " + memoryID.str());
+ }
+ }
+ }
+
+ return output;
+ }
+
// REMOTE GUI
void ExampleMemory::createRemoteGuiTab()
diff --git a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
index a7dda191aa96c14567899a4f9064edf5f69d2bcf..66a500eeb43043ceb660d9ad9c720d2dc946fb66 100644
--- a/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
+++ b/source/RobotAPI/components/armem/server/ExampleMemory/ExampleMemory.h
@@ -58,7 +58,7 @@ namespace armarx
// WritingInterface interface
public:
armem::data::AddSegmentsResult addSegments(const armem::data::AddSegmentsInput& input, const Ice::Current&) override;
- armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current&) override;
+ armem::data::CommitResult commit(const armem::data::Commit& commit, const Ice::Current& = Ice::emptyCurrent) override;
// LightweightRemoteGuiComponentPluginUser interface
@@ -66,6 +66,11 @@ namespace armarx
void createRemoteGuiTab();
void RemoteGui_update() override;
+ // ActionsInterface interface
+ public:
+ armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& input) override;
+ armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& input) override;
+
protected:
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
index e1b4bfd86c1b66ce9401aa75278e63e5e304e876..9e311c53099e20ef2eeb4153bde2b43ffdf42a89 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.cpp
@@ -9,8 +9,11 @@
#include <RobotAPI/libraries/armem/server/MemoryRemoteGui.h>
#include <RobotAPI/libraries/GraspingUtility/aron/GraspCandidate.aron.generated.h>
+#include <RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h>
+#include <RobotAPI/libraries/GraspingUtility/aron_conversions.h>
#include <RobotAPI/libraries/armem_grasping/aron/KnownGraspCandidate.aron.generated.h>
+
namespace armarx::armem::server::grasp
{
armarx::PropertyDefinitionsPtr GraspMemory::createPropertyDefinitions()
@@ -73,7 +76,7 @@ namespace armarx::armem::server::grasp
{
// This function is overloaded to trigger the remote gui rebuild.
armem::data::CommitResult result = ReadWritePluginUser::commit(commit);
- tab.rebuild = true;
+ gui.tab.rebuild = true;
return result;
}
@@ -88,24 +91,422 @@ namespace armarx::armem::server::grasp
void GraspMemory::createRemoteGuiTab()
{
- using namespace armarx::RemoteGui::Client;
+ //add all instances to map
+ workingMemory().getCoreSegment("GraspCandidate").forEachInstance([this](const auto & instance)
+ {
+ if (gui.candidatesToDraw.find(instance.id().str()) == gui.candidatesToDraw.end()){
+ gui.candidatesToDraw[instance.id().str()] = false;
+ }
+ });
-// {
-//
-// tab.memoryGroup = armem::server::MemoryRemoteGui().makeGroupBox(workingMemory());
-// }
-//
-// VBoxLayout root = {tab.memoryGroup, VSpacer()};
-// RemoteGui_createTab(getName(), root, &tab);
+ using namespace armarx::RemoteGui::Client;
+ GridLayout root;
+ {
+ gui.tab.selectAll.setLabel("Select All");
+ gui.tab.deselectAll.setLabel("Deselect All");
+ int row = 0;
+
+ // bimanual candidates are not available for visualization for now
+// {
+// root.add(Label("CoreSegment"), Pos{row, 0});
+// row++;
+// std::vector<std::string> options = workingMemory().getCoreSegmentNames();
+// if (options.empty())
+// {
+// options.push_back("<None>");
+// }
+// gui.tab.selectCoreSegment.setOptions(options);
+// if (gui.coreSegIndex >= 0 && gui.coreSegIndex < static_cast<int>(options.size()))
+// {
+// gui.tab.selectCoreSegment.setIndex(gui.coreSegIndex);
+// gui.coreSegment = options[gui.coreSegIndex];
+// }
+// root.add(gui.tab.selectCoreSegment, Pos{row,0});
+// row++;
+// }
+ if(workingMemory().hasCoreSegment(gui.coreSegment))
+ {
+ {
+ root.add(Label("Provider"), Pos{row, 0});
+ row++;
+ std::vector<std::string> options = workingMemory().getCoreSegment(gui.coreSegment).getProviderSegmentNames();
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+
+ gui.tab.selectProvider.setOptions(options);
+
+ if (gui.providerIndex >= 0 && gui.providerIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectProvider.setIndex(gui.providerIndex);
+ gui.provider = options[gui.providerIndex];
+ }
+ root.add(gui.tab.selectProvider, Pos{row, 0});
+ row++;
+ }
+
+ if(gui.provider == "<All>"
+ || workingMemory().getCoreSegment(gui.coreSegment).hasProviderSegment(gui.provider))
+ {
+ std::vector<MemoryID> providers;
+ {
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ std::vector<std::string> options;
+
+ if (gui.provider == "<All>")
+ {
+ workingMemory().getCoreSegment(gui.coreSegment).forEachProviderSegment([&providers](const auto & provider)
+ {
+ providers.push_back(provider.id());
+ });
+ }
+ else
+ {
+ providers.push_back(MemoryID(memoryName, gui.coreSegment, gui.provider));
+
+ }
+ for (MemoryID & provider : providers)
+ {
+ for(std::string & entity : workingMemory().getProviderSegment(provider).getEntityNames())
+ {
+ options.push_back(entity);
+ }
+ }
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+ gui.tab.selectEntity.setOptions(options);
+ if (gui.entityIndex >= 0 && gui.entityIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectEntity.setIndex(gui.entityIndex);
+ gui.entity = options[gui.entityIndex];
+ }
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+ }
+
+ if( gui.entity == "<All>"
+ || (gui.provider == "<All>" && workingMemory().getCoreSegment(gui.coreSegment).hasEntity(gui.entity))
+ || workingMemory().getCoreSegment(gui.coreSegment).getProviderSegment(gui.provider).hasEntity(gui.entity))
+ {
+ std::vector<MemoryID> entities;
+ {
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ std::vector<std::string> options;
+ for (MemoryID & provider : providers)
+ {
+ if (gui.entity == "<All>")
+ {
+ workingMemory().getProviderSegment(provider).forEachEntity([&entities](auto & entity)
+ {
+ entities.push_back(entity.id());
+ });
+ }
+ else
+ {
+ if (workingMemory().getProviderSegment(provider).hasEntity(gui.entity))
+ {
+ entities.push_back(MemoryID(memoryName, gui.coreSegment, provider.providerSegmentName, gui.entity));
+ }
+ }
+ }
+
+ for (MemoryID & entity : entities)
+ {
+ workingMemory().getEntity(entity).forEachSnapshot([this, &options](const auto & snapshot)
+ {
+ std::string time = armem::toDateTimeMilliSeconds(snapshot.time());
+ gui.timeMap[time] = snapshot.time();
+ options.push_back(time);
+ });
+ }
+
+ if (options.empty())
+ {
+ options.push_back("<None>");
+ }
+ else
+ {
+ options.insert(options.begin(), "<All>");
+ }
+ gui.tab.selectSnapshot.setOptions(options);
+ if (gui.snapshotIndex >= 0 && gui.snapshotIndex < static_cast<int>(options.size()))
+ {
+ gui.tab.selectSnapshot.setIndex(gui.snapshotIndex);
+ if(options[gui.snapshotIndex] != "<None>")
+ {
+ gui.snapshot = options[gui.snapshotIndex];
+ }
+ }
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+
+
+ }
+ bool comboExists;
+ if (gui.snapshot == "<All>")
+ {
+ comboExists = true;
+ }
+ else
+ {
+ for (MemoryID & entity : entities)
+ {
+ if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ {
+ comboExists = true;
+ }
+ }
+ }
+ if(comboExists)
+ {
+ root.add(Label("Instances"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ gui.tab.checkInstances.clear();
+ std::vector<MemoryID> snapshots;
+ for (MemoryID & entity : entities)
+ {
+ if(gui.snapshot == "<All>")
+ {
+ workingMemory().getEntity(entity).forEachSnapshot([&snapshots](auto & snapshot)
+ {
+ snapshots.push_back(snapshot.id());
+ });
+ }
+ else
+ {
+ if (workingMemory().getEntity(entity).hasSnapshot(gui.timeMap.at(gui.snapshot)))
+ {
+ snapshots.push_back(workingMemory().getEntity(entity).getSnapshot(gui.timeMap.at(gui.snapshot)).id());
+ }
+ }
+ }
+
+ for (MemoryID & snapshot : snapshots)
+ {
+ workingMemory().getSnapshot(snapshot).forEachInstance([this, &row, &root](const auto & instance)
+ {
+ root.add(Label(instance.id().str()) , Pos{row, 0});
+ gui.tab.checkInstances[instance.id().str()] = CheckBox();
+ if(gui.candidatesToDraw.at(instance.id().str()))
+ {
+ gui.tab.checkInstances.at(instance.id().str()).setValue(true);
+ }
+ root.add(gui.tab.checkInstances[instance.id().str()], Pos{row, 1});
+ row++;
+ });
+ }
+ }
+ else
+ {
+ root.add(Label("Instances"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ gui.tab.checkInstances.clear();
+ }
+ }
+ else
+ {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ }
+ }
+ else
+ {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+
+ gui.tab.selectEntity.setOptions(options);
+
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+ }
+ }
+ else {
+ gui.tab.checkInstances.clear();
+
+ std::vector<std::string> options = {"<None>"};
+ gui.tab.selectProvider.setOptions(options);
+
+ root.add(Label("Provider"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectProvider, Pos{row, 0});
+ row++;
+
+ gui.tab.selectEntity.setOptions(options);
+
+ root.add(Label("Entity"), Pos{row,0});
+ row++;
+ root.add(gui.tab.selectEntity, Pos{row, 0});
+ row++;
+
+ gui.tab.selectSnapshot.setOptions(options);
+
+ root.add(Label("Snapshot"), Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectSnapshot, Pos{row, 0});
+ row++;
+ root.add(gui.tab.selectAll, Pos{row, 0});
+ row++;
+ root.add(gui.tab.deselectAll, Pos{row, 0});
+ row++;
+
+ }
+ }
+
+
+ RemoteGui_createTab(getName(), root, &gui.tab);
+ gui.tab.rebuild = false;
}
void GraspMemory::RemoteGui_update()
{
-// if (tab.rebuild.exchange(false))
+ armarx::grasping::GraspCandidateVisu visu;
+
+// if (gui.tab.selectCoreSegment.hasValueChanged())
// {
-//// createRemoteGuiTab();
+// gui.coreSegment = gui.tab.selectCoreSegment.getValue();
+// gui.coreSegIndex = gui.tab.selectCoreSegment.getIndex();
+// gui.providerIndex = 0;
+// gui.entityIndex = 0;
+// gui.snapshotIndex = 0;
+// gui.tab.rebuild = true;
// }
+
+ if (gui.tab.selectProvider.hasValueChanged())
+ {
+ gui.provider = gui.tab.selectProvider.getValue();
+ gui.providerIndex = gui.tab.selectProvider.getIndex();
+ gui.entityIndex = 0;
+ gui.snapshotIndex = 0;
+ gui.tab.rebuild = true;
+ }
+
+ if (gui.tab.selectEntity.hasValueChanged())
+ {
+ gui.entity = gui.tab.selectEntity.getValue();
+ gui.entityIndex = gui.tab.selectEntity.getIndex();
+ gui.snapshotIndex = 0;
+ gui.tab.rebuild = true;
+ }
+
+ if (gui.tab.selectSnapshot.hasValueChanged())
+ {
+ gui.snapshot = gui.tab.selectSnapshot.getValue();
+ gui.snapshotIndex = gui.tab.selectSnapshot.getIndex();
+ gui.tab.rebuild = true;
+ }
+
+
+ if(gui.tab.selectAll.wasClicked())
+ {
+ for(auto & element : gui.tab.checkInstances)
+ {
+ element.second.setValue(true);
+ }
+ }
+
+ if(gui.tab.deselectAll.wasClicked())
+ {
+ for(auto & element : gui.tab.checkInstances)
+ {
+ element.second.setValue(false);
+ }
+ }
+
+ for(auto & element : gui.tab.checkInstances)
+ {
+ gui.candidatesToDraw.at(element.first) = element.second.getValue();
+ }
+
+ std::map<std::string, viz::Layer> reachableLayers;
+ std::map<std::string, viz::Layer> unreachableLayers;
+
+ for(auto & element : gui.candidatesToDraw )
+ {
+ std::string entityName = armem::MemoryID::fromString(element.first).entityName;
+
+ if (reachableLayers.find(entityName) == reachableLayers.end())
+ {
+ reachableLayers[entityName] = arviz.layer(entityName + "_reachable");
+ unreachableLayers[entityName] = arviz.layer(entityName + "_unreachable");
+ }
+
+ //draw candidates
+ if (element.second)
+ {
+ armarx::grasping::GraspCandidate candidate;
+
+ armarx::grasping::arondto::GraspCandidate aronTransform;
+
+ armem::wm::EntityInstance instance = workingMemory().getInstance(armem::MemoryID::fromString(element.first));
+
+ aronTransform.fromAron(workingMemory().getInstance(armem::MemoryID::fromString(element.first)).data());
+
+ fromAron(aronTransform, candidate);
+
+ visu.visualize(element.first, candidate, reachableLayers.at(entityName), unreachableLayers.at(entityName));
+ }
+ }
+
+ std::vector<viz::Layer> layers;
+ for(auto & [entityName, layer] : reachableLayers)
+ {
+ layers.push_back(layer);
+ layers.push_back(unreachableLayers.at(entityName));
+ }
+ arviz.commit(layers);
+
+ if (gui.tab.rebuild)
+ {
+ createRemoteGuiTab();
+ }
}
}
diff --git a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
index ea7dceaddd3af6671ac35b151e3bc803222d65ca..867bf0a62575f2affd51e1842d6631ceb0204e8e 100644
--- a/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
+++ b/source/RobotAPI/components/armem/server/GraspMemory/GraspMemory.h
@@ -70,9 +70,34 @@ namespace armarx::armem::server::grasp
{
std::atomic_bool rebuild = false;
- RemoteGui::Client::GroupBox memoryGroup;
+ // RemoteGui::Client::ComboBox selectCoreSegment;
+ RemoteGui::Client::ComboBox selectProvider;
+ RemoteGui::Client::ComboBox selectEntity;
+ RemoteGui::Client::ComboBox selectSnapshot;
+ RemoteGui::Client::Button selectAll;
+ RemoteGui::Client::Button deselectAll;
+ std::map<std::string, RemoteGui::Client::CheckBox> checkInstances;
};
- RemoteGuiTab tab;
+
+
+ struct GuiInfo
+ {
+ // int coreSegIndex = 0;
+ int providerIndex = 0;
+ int entityIndex = 0;
+ int snapshotIndex = 0;
+
+ RemoteGuiTab tab;
+
+ std::string coreSegment = "GraspCandidate";
+ std::string provider = "";
+ std::string entity = "";
+ std::string snapshot = "";
+
+ std::map<std::string, bool> candidatesToDraw;
+ std::map<std::string, Time> timeMap;
+ };
+ GuiInfo gui;
};
}
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index c605df2efece207f0a0b11337dd96c61ca534470..1d9c4ac147ddc2407952ed6db08c8fd4cfcc279b 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -156,6 +156,62 @@ namespace armarx::armem::server::obj
{
}
+ armem::actions::GetActionsOutputSeq ObjectMemory::getActions(
+ const armem::actions::GetActionsInputSeq& inputs)
+ {
+ using namespace armem::actions;
+ GetActionsOutputSeq outputs;
+ for (const auto& input : inputs)
+ {
+ auto memoryID = fromIce<armem::MemoryID>(input.id);
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
+ and memoryID.hasEntityName() and not memoryID.hasGap())
+ {
+ Menu menu {
+ Action{"vis", "Visualize object"},
+ };
+ // For strange C++ reasons, this works, but emplace_back doesn't.
+ outputs.push_back({ menu.toIce() });
+ }
+ }
+
+ return outputs;
+ }
+
+ armem::actions::ExecuteActionOutputSeq ObjectMemory::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs)
+ {
+ using namespace armem::actions;
+ ExecuteActionOutputSeq outputs;
+
+ for (const auto& input : inputs)
+ {
+ auto memoryID = fromIce<armem::MemoryID>(input.id);
+ if (input.actionPath == ActionPath{"vis"})
+ {
+ if (armem::contains(armem::MemoryID("Object", "Class"), memoryID)
+ and memoryID.hasEntityName() and not memoryID.hasGap())
+ {
+ classSegment.visualizeClass(memoryID);
+ outputs.emplace_back(true, "");
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "MemoryID " << memoryID
+ << " does not refer to a valid object class.";
+ outputs.emplace_back(false, sstream.str());
+ }
+ }
+ else
+ {
+ std::stringstream sstream;
+ sstream << "Action path " << input.actionPath << " is not defined.";
+ outputs.emplace_back(false, sstream.str());
+ }
+ }
+ return outputs;
+ }
void ObjectMemory::createRemoteGuiTab()
{
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
index 91217c90cc0dcb5be302663a0b472cac3327f3c0..82f39db591bd56537fa280e0dd10a56fa9806ca3 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.h
@@ -93,6 +93,14 @@ namespace armarx::armem::server::obj
void onDisconnectComponent() override;
void onExitComponent() override;
+ // Without this, ObjectMemory draws the original
+ // methods from ObjectMemoryInterface and complains
+ // that an overload is being hidden.
+ using ReadWritePluginUser::getActions;
+ using ReadWritePluginUser::executeActions;
+
+ armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs) override;
+ armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs) override;
// Remote GUI
void createRemoteGuiTab();
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 1d5bd4be73c9215365c746f875731042e4a54f48..36442cf0bdaa4238a8a832fe39f6c244569fe830 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -107,6 +107,7 @@ set(SLICE_FILES
armem.ice
+ armem/actions.ice
armem/io.ice
armem/commit.ice
armem/memory.ice
@@ -116,6 +117,7 @@ set(SLICE_FILES
armem/client/MemoryListenerInterface.ice
armem/server.ice
+ armem/server/ActionsInterface.ice
armem/server/StoringMemoryInterface.ice
armem/server/LoadingMemoryInterface.ice
armem/server/MemoryInterface.ice
diff --git a/source/RobotAPI/interface/armem.ice b/source/RobotAPI/interface/armem.ice
index 79c54360ec8cdea2189d6f748cbadc4544379130..f7621099de52b5a2f15b3f0f6367ec5c19471fb3 100644
--- a/source/RobotAPI/interface/armem.ice
+++ b/source/RobotAPI/interface/armem.ice
@@ -1,5 +1,6 @@
#pragma once
+#include <RobotAPI/interface/armem/actions.ice>
#include <RobotAPI/interface/armem/client.ice>
#include <RobotAPI/interface/armem/commit.ice>
#include <RobotAPI/interface/armem/memory.ice>
diff --git a/source/RobotAPI/interface/armem/actions.ice b/source/RobotAPI/interface/armem/actions.ice
new file mode 100644
index 0000000000000000000000000000000000000000..5bb06fc4c9ec815dba87fef49a31bc13498b66fc
--- /dev/null
+++ b/source/RobotAPI/interface/armem/actions.ice
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/memory.ice>
+
+module armarx
+{
+ module armem
+ {
+ module actions
+ {
+ module data
+ {
+
+ class MenuEntry
+ {
+ /// Item in the created action path.
+ string id;
+ /// Human-readable text displayed to the user.
+ string text;
+ };
+ sequence<MenuEntry> MenuEntrySeq;
+
+ /**
+ * Clickable / executable action.
+ */
+ class Action extends MenuEntry
+ {
+ };
+
+ /**
+ * Expandable sub-menu, but cannot be executed itself.
+ */
+ class SubMenu extends MenuEntry
+ {
+ MenuEntrySeq entries;
+ };
+
+
+ class Menu
+ {
+ MenuEntrySeq entries;
+ };
+
+ sequence<string> ActionPath;
+
+ struct GetActionsInput
+ {
+ armem::data::MemoryID id;
+ };
+ sequence<GetActionsInput> GetActionsInputSeq;
+
+ struct GetActionsOutput
+ {
+ Menu menu;
+ };
+ sequence<GetActionsOutput> GetActionsOutputSeq;
+
+ struct ExecuteActionInput
+ {
+ armem::data::MemoryID id;
+ ActionPath actionPath;
+ };
+ sequence<ExecuteActionInput> ExecuteActionInputSeq;
+
+ struct ExecuteActionOutput
+ {
+ bool success = false;
+ string errorMessage;
+ };
+ sequence<ExecuteActionOutput> ExecuteActionOutputSeq;
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
index 95b6a9fa6b03d8f7d4644d2538ecfd64d5258229..2d6b220a2050001960fcc4b564d54b3526f4ed44 100644
--- a/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
+++ b/source/RobotAPI/interface/armem/mns/MemoryNameSystemInterface.ice
@@ -1,5 +1,6 @@
#pragma once
+#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
@@ -15,11 +16,14 @@ module armarx
/**
* A memory server can implement the reading and/or writing
* memory interface. They should be handled individually.
+ * Additionally, it can implement an actions interface,
+ * which is currently the case for all reading and writing memories.
*/
struct MemoryServerInterfaces
{
server::ReadingMemoryInterface* reading;
server::WritingMemoryInterface* writing;
+ server::actions::ActionsInterface* actions;
};
dictionary<string, MemoryServerInterfaces> MemoryServerInterfacesMap;
diff --git a/source/RobotAPI/interface/armem/server/ActionsInterface.ice b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..8fa6754f7e1e0c961c4a5ff9c9bb317dc9fc3aef
--- /dev/null
+++ b/source/RobotAPI/interface/armem/server/ActionsInterface.ice
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <RobotAPI/interface/armem/actions.ice>
+
+module armarx
+{
+ module armem
+ {
+ module server
+ {
+ module actions
+ {
+ interface ActionsInterface
+ {
+ actions::data::GetActionsOutputSeq getActions(actions::data::GetActionsInputSeq inputs);
+ actions::data::ExecuteActionOutputSeq executeActions(actions::data::ExecuteActionInputSeq inputs);
+ };
+ }
+ }
+ }
+}
diff --git a/source/RobotAPI/interface/armem/server/MemoryInterface.ice b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
index ec83f439d8a91e8e57fe4c1767a8ba27bfab7c5d..05b2ea8c54207f6bcfa7a2d708f8fd7e59df416b 100644
--- a/source/RobotAPI/interface/armem/server/MemoryInterface.ice
+++ b/source/RobotAPI/interface/armem/server/MemoryInterface.ice
@@ -5,6 +5,7 @@
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.ice>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.ice>
+#include <RobotAPI/interface/armem/server/ActionsInterface.ice>
#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice>
@@ -26,6 +27,7 @@ module armarx
interface MemoryInterface extends
WorkingMemoryInterface,
LongTermMemoryInterface,
+ actions::ActionsInterface,
client::MemoryListenerInterface
{
};
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 702e9667ab047b4144008175e9bd4a02020d34f6..6ae5638174ec0293a4a3ab73fea68e869a397cf9 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -144,7 +144,6 @@ module armarx
StringVariantBaseMap currentConfig;
};
-
dictionary<string, ProviderInfo> InfoMap;
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index da1d938b96fc8e7fe3d2a9cb2c0f25df386a06af..20f30486fd3249168f6f80a9353d6dd78cc7b2ea 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
@@ -20,6 +20,7 @@ set(LIB_FILES
ObjectPoseClient.cpp
PoseManifoldGaussian.cpp
ProvidedObjectPose.cpp
+ Scene.cpp
json_conversions.cpp
ice_conversions.cpp
@@ -40,6 +41,7 @@ set(LIB_HEADERS
ObjectPoseClient.h
PoseManifoldGaussian.h
ProvidedObjectPose.h
+ Scene.h
forward_declarations.h
json_conversions.h
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index a534e7df3a8aec94c251a0dc7600d87c8ef76b44..11e583902b8fbb0963e8f4c1b00746de01356f1b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -347,6 +347,31 @@ namespace armarx
}
return nullptr;
}
+
+ objpose::data::ObjectPose* objpose::findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ {
+ for (objpose::data::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
+
+ const objpose::data::ObjectPose* objpose::findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id)
+ {
+ for (const objpose::data::ObjectPose& pose : objectPoses)
+ {
+ if (pose.objectID == id)
+ {
+ return &pose;
+ }
+ }
+ return nullptr;
+ }
+
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
index 74b1b3e6dd6632ca7c11dcc060f5934a24725262..66c87f22f5f369c154043e83a72146f0d5bbea54 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.h
@@ -150,5 +150,7 @@ namespace armarx::objpose
ObjectPose* findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id);
const ObjectPose* findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id);
+ data::ObjectPose* findObjectPoseByID(data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
+ const data::ObjectPose* findObjectPoseByID(const data::ObjectPoseSeq& objectPoses, const armarx::data::ObjectID& id);
}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp b/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1437da55faff8976d7c87ac9f76f9a2adf11432f
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.cpp
@@ -0,0 +1,44 @@
+#include "Scene.h"
+
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+
+
+namespace armarx::objects
+{
+
+ ObjectID SceneObject::getClassID() const
+ {
+ return ObjectID(className);
+ }
+
+
+ ObjectID SceneObject::getClassID(ObjectFinder& finder) const
+ {
+ ObjectID id = getClassID();
+ if (id.dataset().empty())
+ {
+ if (std::optional<ObjectInfo> info = finder.findObject(id.className()))
+ {
+ return info->id();
+ }
+ }
+ return id;
+ }
+
+
+ ObjectID SceneObject::getObjectID() const
+ {
+ return getClassID().withInstanceName(instanceName);
+ }
+
+
+ ObjectID SceneObject::getObjectID(ObjectFinder& finder) const
+ {
+ return getClassID(finder).withInstanceName(instanceName);
+ }
+
+}
+
+
+
diff --git a/source/RobotAPI/libraries/ArmarXObjects/Scene.h b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
new file mode 100644
index 0000000000000000000000000000000000000000..a4cc41d80b5fccc87b1e9d871808c685ce332401
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/Scene.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ObjectMemory
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <map>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "forward_declarations.h"
+
+
+namespace armarx::objects
+{
+
+ struct SceneObject
+ {
+ std::string className;
+ std::string instanceName;
+ std::string collection;
+
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
+
+ std::map<std::string, float> jointValues;
+
+ ObjectID getClassID() const;
+ ObjectID getClassID(ObjectFinder& finder) const;
+ ObjectID getObjectID() const;
+ ObjectID getObjectID(ObjectFinder& finder) const;
+ };
+
+ struct Scene
+ {
+ std::vector<SceneObject> objects;
+ };
+
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
index b7175785af42fce4284769d47290b3aae3616614..320305ba22474d6eca649966c7988ad7b3048c0b 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h
@@ -27,6 +27,12 @@ namespace armarx::objpose
class ObjectPoseClient;
}
+namespace armarx::objects
+{
+ struct Scene;
+ struct SceneObject;
+}
+
// Ice Types
namespace armarx::objpose::data
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
index 3888855892a95cecb6ea5fecc55e63763c894f6a..617baca941ebb86be7e3b05ecac1e79efc23490c 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.cpp
@@ -3,8 +3,12 @@
#include <SimoxUtility/json/eigen_conversion.h>
#include <SimoxUtility/shapes/json_conversions.h>
+#include "ObjectID.h"
+#include "ObjectPose.h"
+#include "Scene.h"
#include "ice_conversions.h"
+
void armarx::to_json(nlohmann::json& j, const ObjectID& id)
{
j["dataset"] = id.dataset();
@@ -71,3 +75,46 @@ void armarx::objpose::from_json(const nlohmann::json& j, ObjectPose& op)
op.localOOBB = j.at("localOOBB").get<simox::OrientedBoxf>();
}
}
+
+
+void armarx::objects::to_json(nlohmann::json& j, const SceneObject& rhs)
+{
+ // j["instanceID"] = rhs.instanceID;
+ j["class"] = rhs.className;
+ j["instanceName"] = rhs.instanceName;
+ j["collection"] = rhs.collection;
+ j["position"] = rhs.position;
+ j["orientation"] = rhs.orientation;
+ j["jointValues"] = rhs.jointValues;
+}
+
+
+void armarx::objects::from_json(const nlohmann::json& j, SceneObject& rhs)
+{
+ // j.at("instanceID").get_to(rhs.instanceID);
+ j.at("class").get_to(rhs.className);
+ if (j.count("instanceName"))
+ {
+ j["instanceName"].get_to(rhs.instanceName);
+ }
+ j.at("collection").get_to(rhs.collection);
+ j.at("position").get_to(rhs.position);
+ j.at("orientation").get_to(rhs.orientation);
+ if (j.count("jointValues"))
+ {
+ j.at("jointValues").get_to(rhs.jointValues);
+ }
+}
+
+
+
+void armarx::objects::to_json(nlohmann::json& j, const Scene& rhs)
+{
+ j["objects"] = rhs.objects;
+}
+
+
+void armarx::objects::from_json(const nlohmann::json& j, Scene& rhs)
+{
+ j.at("objects").get_to(rhs.objects);
+}
diff --git a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
index 048e7d933af5913678f17abd7eacbcf76e4e83c1..608ad14424234ef4120f117d511310a42be71bcc 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
+++ b/source/RobotAPI/libraries/ArmarXObjects/json_conversions.h
@@ -2,8 +2,8 @@
#include <SimoxUtility/json/json.hpp>
-#include "ObjectID.h"
-#include "ObjectPose.h"
+#include "forward_declarations.h"
+
namespace armarx
{
@@ -16,3 +16,13 @@ namespace armarx::objpose
void to_json(nlohmann::json& j, const ObjectPose& op);
void from_json(const nlohmann::json& j, ObjectPose& op);
}
+
+
+namespace armarx::objects
+{
+ void to_json(nlohmann::json& j, const SceneObject& rhs);
+ void from_json(const nlohmann::json& j, SceneObject& rhs);
+
+ void to_json(nlohmann::json& j, const Scene& rhs);
+ void from_json(const nlohmann::json& j, Scene& rhs);
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index 7ec81af63d843443f25008f486a87880aeb05c65..1ae8862c277d13836d015473953c605db92c9349 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
@@ -10,6 +10,7 @@ armarx_add_library(
RobotAPI::ArmarXObjects
RobotAPI::ArViz
RobotAPI::armem_objects
+
SOURCES box_to_grasp_candidates.cpp
grasp_candidate_drawer.cpp
GraspCandidateHelper.cpp
@@ -17,14 +18,18 @@ armarx_add_library(
aron_conversions.cpp
GraspCandidateWriter.cpp
GraspCandidateReader.cpp
+ GraspCandidateVisu.cpp
+
HEADERS box_to_grasp_candidates.h
box_to_grasp_candidates.ipp
grasp_candidate_drawer.h
GraspCandidateHelper.h
BimanualGraspCandidateHelper.h
+
aron_conversions.h
GraspCandidateWriter.h
GraspCandidateReader.h
+ GraspCandidateVisu.h
)
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
index 4e742c96f69db5d5abeabe58f7cbc69684e740bb..8f875542bce9f7a5ed3fe91bc697eb3cd76e1edf 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.cpp
@@ -14,14 +14,16 @@ namespace armarx::armem
{
}
- void GraspCandidateReader::connect()
+
+ void GraspCandidateReader::connect(bool use)
{
// Wait for the memory to become available and add it as dependency.
ARMARX_IMPORTANT << "GraspCandidateReader: Waiting for memory '"
<< properties.memoryName << "' ...";
try
{
- memoryReader = memoryNameSystem.useReader(properties.memoryName);
+ memoryReader = use ? memoryNameSystem.useReader(properties.memoryName)
+ : memoryNameSystem.getReader(MemoryID(properties.memoryName));
ARMARX_IMPORTANT << "GraspCandidateReader: Connected to memory '"
<< properties.memoryName;
}
@@ -30,13 +32,9 @@ namespace armarx::armem
ARMARX_ERROR << e.what();
return;
}
-
-
}
-
-
armarx::grasping::GraspCandidate asGraspCandidate(const armem::wm::EntityInstance& instance)
{
armarx::grasping::GraspCandidate candidate;
@@ -64,12 +62,26 @@ namespace armarx::armem
grasping::GraspCandidatePtr GraspCandidateReader::queryGraspCandidateInstanceByID(const armem::MemoryID& id) const
+ {
+ auto dict = queryGraspCandidateInstancesByID({id});
+ if (auto it = dict.find(id.str()); it != dict.end())
+ {
+ return it->second;
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+ grasping::GraspCandidateDict GraspCandidateReader::queryGraspCandidateInstancesByID(const std::vector<MemoryID>& ids) const
{
armem::client::query::Builder qb;
ARMARX_DEBUG << "Query for memory name: " << properties.memoryName;
- qb.singleEntitySnapshot(id.getEntitySnapshotID());
+ qb.multipleEntitySnapshots(ids);
const armem::client::QueryResult qResult =
memoryReader.query(qb.buildQueryInput());
@@ -80,24 +92,19 @@ namespace armarx::armem
throw armem::error::QueryFailed(properties.memoryName, qResult.errorMessage);
}
-
- armarx::grasping::GraspCandidatePtr candidate;
-
- armem::wm::FunctionalVisitor visitor;
- visitor.instanceConstFn = [id, &candidate](armem::wm::EntityInstance const & instance)
+ armarx::grasping::GraspCandidateDict candidates;
+ for (const MemoryID& id : ids)
{
- if (instance.id() == id)
+ if (const armem::wm::EntityInstance* instance = qResult.memory.findInstance(id))
{
- candidate = new grasping::GraspCandidate(asGraspCandidate(instance));
+ candidates[id.str()] = new grasping::GraspCandidate(asGraspCandidate(*instance));
}
- return true;
- };
-
- visitor.applyTo(qResult.memory);
+ }
- return candidate;
+ return candidates;
}
+
grasping::BimanualGraspCandidatePtr GraspCandidateReader::queryBimanualGraspCandidateInstanceByID(
const armem::MemoryID& id) const
{
@@ -134,7 +141,7 @@ namespace armarx::armem
return candidate;
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidateEntity(
+ grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidateEntity(
const std::string& provider, const std::string& entity) const
{
armem::client::query::Builder qb;
@@ -172,7 +179,7 @@ namespace armarx::armem
return getBimanualGraspCandidatesFromResultSet(qResult);
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::queryLatestGraspCandidates(
+ grasping::GraspCandidateDict GraspCandidateReader::queryLatestGraspCandidates(
const std::string& provider) const
{
@@ -249,7 +256,7 @@ namespace armarx::armem
def->optional(properties.memoryName, prefix + "MemoryName");
}
- std::map<std::string, grasping::GraspCandidatePtr> GraspCandidateReader::getGraspCandidatesFromResultSet(
+ grasping::GraspCandidateDict GraspCandidateReader::getGraspCandidatesFromResultSet(
const armem::client::QueryResult& qResult) const
{
if (!qResult.success)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
index 0a1604afdb9ca58c93ce52628cee61e7ee647dcc..5de5c25e335e5544170f18f838df2e5e23d213fa 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h
@@ -1,9 +1,12 @@
#pragma once
+#include <vector>
+
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client.h>
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
namespace armarx::armem
{
@@ -12,19 +15,20 @@ namespace armarx::armem
public:
GraspCandidateReader(armem::client::MemoryNameSystem& memoryNameSystem);
- void connect();
+ void connect(bool use = true);
grasping::GraspCandidatePtr queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
+ grasping::GraspCandidateDict queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
grasping::BimanualGraspCandidatePtr queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
- std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidateEntity(
+ grasping::GraspCandidateDict queryLatestGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidateEntity(
std::string const& provider, std::string const& entity) const;
- std::map<std::string, grasping::GraspCandidatePtr> queryLatestGraspCandidates(
+ grasping::GraspCandidateDict queryLatestGraspCandidates(
std::string const& provider = "") const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> queryLatestBimanualGraspCandidates(
@@ -37,7 +41,7 @@ namespace armarx::armem
private:
- std::map<std::string, grasping::GraspCandidatePtr> getGraspCandidatesFromResultSet(
+ grasping::GraspCandidateDict getGraspCandidatesFromResultSet(
armem::client::QueryResult const& qResult) const;
std::map<std::string, grasping::BimanualGraspCandidatePtr> getBimanualGraspCandidatesFromResultSet(
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1c5e3bc4781d504f8f467ce2b8584d5358b09a06
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.cpp
@@ -0,0 +1,89 @@
+#include "GraspCandidateVisu.h"
+
+#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/components/ArViz/Client/elements/RobotHand.h>
+
+
+namespace armarx::grasping
+{
+ GraspCandidateVisu::GraspCandidateVisu()
+ {
+ }
+
+
+ void GraspCandidateVisu::visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Client& arviz)
+ {
+ viz::Layer graspsLayer = arviz.layer("Grasps");
+ viz::Layer graspsNonReachableLayer = arviz.layer("Grasps Non-Reachable");
+
+ visualize(candidates, graspsLayer, graspsNonReachableLayer);
+ arviz.commit({graspsLayer, graspsNonReachableLayer});
+ }
+
+
+ void GraspCandidateVisu::visualize(const std::string &id, const GraspCandidate &candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
+ {
+ int alpha = alpha_default;
+ if (auto it = alphasByKey.find(id); it != alphasByKey.end())
+ {
+ alpha = it->second;
+ }
+
+ viz::Robot hand = visualize("Grasp_" + id, candidate, alpha);
+
+ if (candidate.reachabilityInfo->reachable)
+ {
+ layerReachable.add(hand);
+ }
+ else
+ {
+ layerNonReachable.add(hand);
+ }
+ }
+
+ void GraspCandidateVisu::visualize(const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable)
+ {
+ for (auto& [id, candidate] : candidates)
+ {
+ visualize(id, *candidate, layerReachable, layerNonReachable);
+ }
+ }
+
+
+ viz::Robot GraspCandidateVisu::visualize(
+ const std::string& name,
+ const GraspCandidate& candidate)
+ {
+ return visualize(name, candidate, alpha_default);
+ }
+
+
+ viz::Robot
+ GraspCandidateVisu::visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate,
+ int alpha)
+ {
+ bool isReachable = candidate.reachabilityInfo->reachable;
+ viz::Color color = isReachable ? viz::Color::green() : viz::Color::red();
+ color.a = alpha;
+
+ Eigen::Matrix4f tcp2handRoot = fromIce(candidate.tcpPoseInHandRoot).inverse();
+ Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate.graspPose)->toEigen();
+ std::string modelFile = "rt/robotmodel/Armar6-SH/Armar6-" + candidate.side + "Hand-v3.xml";
+
+ viz::Robot hand = viz::RobotHand(name)
+ .file("Armar6RT", modelFile)
+ .pose(fromIce(candidate.robotPose) * graspPose * tcp2handRoot)
+ .overrideColor(color);
+
+ return hand;
+ }
+}
+
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
new file mode 100644
index 0000000000000000000000000000000000000000..8eb298e4183d0daef8d004d4d3fc3cacc8cd30e7
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateVisu.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <RobotAPI/components/ArViz/Client/Layer.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+
+namespace armarx::grasping
+{
+ class GraspCandidateVisu
+ {
+ public:
+ GraspCandidateVisu();
+
+
+ void visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Client& arviz);
+
+ void visualize(
+ const grasping::GraspCandidateDict& candidates,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
+
+ void visualize(
+ const std::string& id,
+ const grasping::GraspCandidate& candidate,
+ viz::Layer& layerReachable,
+ viz::Layer& layerNonReachable);
+
+ viz::Robot visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate);
+ viz::Robot visualize(
+ const std::string& name,
+ const grasping::GraspCandidate& candidate,
+ int alpha);
+
+
+ public:
+
+ int alpha_default = 255;
+ std::map<std::string, int> alphasByKey = {};
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index ba231f670b3342011fd228da611d2e9d1b8d87cc..5feda749b6fc3eb3e1f601812b778e71b20c099b 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -12,6 +12,7 @@ set(LIBS
)
set(LIB_FILES
+ core/actions.cpp
core/Commit.cpp
core/MemoryID.cpp
core/MemoryID_operators.cpp
@@ -77,6 +78,7 @@ set(LIB_HEADERS
core.h
core/forward_declarations.h
+ core/actions.h
core/Commit.h
core/DataMode.h
core/MemoryID.h
diff --git a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
index 01bf3aecfdc821d41f2902efd8329fa9e509152a..9422d575cd9b32d58c64110d6d3ef83a8deabe56 100644
--- a/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
+++ b/source/RobotAPI/libraries/armem/client/MemoryNameSystem.h
@@ -181,7 +181,15 @@ namespace armarx::armem::client
std::map<MemoryID, wm::EntityInstance> resolveEntityInstances(const std::vector<MemoryID>& ids);
-
+ /**
+ * @brief Resolve the given memory server for the given memory ID.
+ *
+ * @param memoryID The memory ID.
+ * @return The memory server proxy.
+ *
+ * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
+ */
+ mns::dto::MemoryServerInterfaces resolveServer(const MemoryID& memoryID);
// Registration - only for memory servers
@@ -217,16 +225,6 @@ namespace armarx::armem::client
private:
- /**
- * @brief Resolve the given memory server for the given memory ID.
- *
- * @param memoryID The memory ID.
- * @return The memory server proxy.
- *
- * @throw `error::CouldNotResolveMemoryServer` If the memory name could not be resolved.
- */
- mns::dto::MemoryServerInterfaces resolveServer(const MemoryID& memoryID);
-
/**
* @brief Wait for the given memory server.
*
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.cpp b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
index 8637055e5b850494f15c10255b29de94785cec08..2a31828717f34906875bdbb8a17c62719b675331 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.cpp
@@ -120,4 +120,12 @@ namespace armarx::armem::client::query
.snapshots().atTime(snapshotID.timestamp);
}
+ void Builder::multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs)
+ {
+ for (const MemoryID& snapshotID : snapshotIDs)
+ {
+ singleEntitySnapshot(snapshotID);
+ }
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/Builder.h b/source/RobotAPI/libraries/armem/client/query/Builder.h
index 3b52aa4b1945d6a2bef62932403013670843f94a..90d3e211ac9e776f061695420392b765bac1aac2 100644
--- a/source/RobotAPI/libraries/armem/client/query/Builder.h
+++ b/source/RobotAPI/libraries/armem/client/query/Builder.h
@@ -61,6 +61,7 @@ namespace armarx::armem::client::query
void latestEntitySnapshot(const MemoryID& entityID);
void singleEntitySnapshot(const MemoryID& snapshotID);
+ void multipleEntitySnapshots(const std::vector<MemoryID>& snapshotIDs);
QueryInput buildQueryInput() const;
diff --git a/source/RobotAPI/libraries/armem/core/actions.cpp b/source/RobotAPI/libraries/armem/core/actions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9eb28f7187d9ac51295dfa3be351f14fc97ef2cd
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/actions.cpp
@@ -0,0 +1,112 @@
+#include "actions.h"
+
+#include <RobotAPI/interface/armem/actions.h>
+
+namespace armarx::armem::actions
+{
+
+ MenuEntry::MenuEntry(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries) :
+ id(id), text(text), entries(entries)
+ {
+ }
+
+ MenuEntry&
+ MenuEntry::add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+ data::MenuEntryPtr
+ MenuEntry::toIce() const // NOLINT (recursive call chain)
+ {
+ if (entries.empty())
+ {
+ return new data::Action{this->id, this->text};
+ }
+
+ data::SubMenuPtr ice = new data::SubMenu{this->id, this->text, {}};
+ for (const MenuEntry& entry : entries)
+ {
+ ice->entries.push_back(entry.toIce());
+ }
+ return ice;
+ }
+
+ MenuEntry
+ MenuEntry::fromIce(const data::MenuEntryPtr& ice) // NOLINT (recursive call chain)
+ {
+ if (ice->ice_isA(data::SubMenu::ice_staticId()))
+ {
+ auto ptr = IceUtil::Handle<data::SubMenu>::dynamicCast(ice);
+ MenuEntry subMenu{ptr->id, ptr->text, {}};
+ for (const auto& entry : ptr->entries)
+ {
+ subMenu.entries.push_back(MenuEntry::fromIce(entry));
+ }
+ return subMenu;
+ }
+
+ return MenuEntry{ice->id, ice->text, {}};
+ }
+
+ Action::Action(const std::string& id, const std::string& text) : MenuEntry(id, text)
+ {
+ }
+
+ SubMenu::SubMenu(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries) :
+ MenuEntry(id, text, entries)
+ {
+ }
+
+ MenuEntry&
+ SubMenu::add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+
+ Menu::Menu(std::initializer_list<MenuEntry> entries) : entries(entries)
+ {
+ }
+
+ Menu::Menu(const std::vector<MenuEntry>& entries) : entries(entries)
+ {
+ }
+
+
+ MenuEntry&
+ Menu::add(const std::string& id, const std::string& text, const std::vector<MenuEntry>& entries)
+ {
+ return this->entries.emplace_back(id, text, entries);
+ }
+
+ data::MenuPtr
+ Menu::toIce() const
+ {
+ data::MenuPtr ice{new data::Menu};
+ for (const MenuEntry& entry : entries)
+ {
+ ice->entries.push_back(entry.toIce());
+ }
+ return ice;
+ }
+
+ Menu
+ Menu::fromIce(const data::MenuPtr& ice)
+ {
+ Menu menu;
+ for (const auto& entry : ice->entries)
+ {
+ menu.entries.push_back(MenuEntry::fromIce(entry));
+ }
+ return menu;
+ }
+} // namespace armarx::armem::actions
diff --git a/source/RobotAPI/libraries/armem/core/actions.h b/source/RobotAPI/libraries/armem/core/actions.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d96dbbac3f5b8f614772a881b504793d78b40a9
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/core/actions.h
@@ -0,0 +1,77 @@
+#pragma once
+
+#include <vector>
+
+#include <RobotAPI/interface/armem/actions.h>
+
+namespace armarx::armem::actions
+{
+ // Business objects to make working with the Armem
+ // action interface easier. To see an example usage,
+ // check the ExampleMemory.
+
+ struct MenuEntry
+ {
+ MenuEntry(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ data::MenuEntryPtr toIce() const;
+
+ static MenuEntry fromIce(const data::MenuEntryPtr& ice);
+
+ public:
+ std::string id;
+ std::string text;
+ std::vector<MenuEntry> entries;
+ };
+
+
+ struct Action : public MenuEntry
+ {
+ Action(const std::string& id, const std::string& text);
+ };
+
+ struct SubMenu : public MenuEntry
+ {
+ SubMenu(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+ };
+
+
+ class Menu
+ {
+
+ public:
+ Menu(std::initializer_list<MenuEntry> entries);
+
+ Menu(const std::vector<MenuEntry>& entries = {});
+
+ MenuEntry& add(const std::string& id,
+ const std::string& text,
+ const std::vector<MenuEntry>& entries = {});
+
+ data::MenuPtr toIce() const;
+
+ static Menu fromIce(const data::MenuPtr& ice);
+
+ public:
+ std::vector<MenuEntry> entries;
+ };
+
+ using data::ExecuteActionInputSeq;
+ using data::ExecuteActionOutputSeq;
+ using data::GetActionsInputSeq;
+ using data::GetActionsOutputSeq;
+ using data::ActionPath;
+
+} // namespace armarx::armem::actions
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
index 29f57ca33e82e865e52c49f08241581a632e0fca..20df9176bb562ab9025c001451d6fef23793d504 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.cpp
@@ -138,5 +138,14 @@ namespace armarx
update.timeArrived = timeArrived;
}
+ void armem::fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu)
+ {
+ menu = actions::Menu::fromIce(ice);
+ }
+
+ void armem::toIce(actions::data::MenuPtr& ice, const actions::Menu& menu)
+ {
+ ice = menu.toIce();
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/ice_conversions.h b/source/RobotAPI/libraries/armem/core/ice_conversions.h
index e8f104fe04ebcb721b0ffbf4b70b18ada0af234c..a8ef35e7938f07f14a81b8ecb731e2470b395c97 100644
--- a/source/RobotAPI/libraries/armem/core/ice_conversions.h
+++ b/source/RobotAPI/libraries/armem/core/ice_conversions.h
@@ -1,10 +1,12 @@
#pragma once
+#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/commit.h>
#include <RobotAPI/interface/armem/memory.h>
#include "ice_conversions_templates.h"
+#include "actions.h"
#include "Commit.h"
#include "MemoryID.h"
#include "Time.h"
@@ -43,5 +45,8 @@ namespace armarx::armem
void fromIce(const data::Commit& ice, Commit& commit, Time timeArrived);
void fromIce(const data::EntityUpdate& ice, EntityUpdate& update, Time timeArrived);
+
+ void fromIce(const actions::data::MenuPtr& ice, actions::Menu& menu);
+ void toIce(actions::data::MenuPtr& ice, const actions::Menu& menu);
}
diff --git a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
index a0c606a990fd4635a5fa2cded1c0e5be8f7566a8..566cf7e0bcbdba70e8e16ca5c527120628c89f32 100644
--- a/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
+++ b/source/RobotAPI/libraries/armem/mns/MemoryNameSystem.cpp
@@ -59,7 +59,7 @@ namespace armarx::armem::mns
int row = 0;
grid.add(Label("Memory Name"), {row, 0})
.add(Label("Component Name"), {row, 1})
- .add(Label("R/W"), {row, 2})
+ .add(Label("R/W/A"), {row, 2})
.add(Label("Registration Time"), {row, 3})
;
row++;
@@ -79,6 +79,11 @@ namespace armarx::armem::mns
componentName = info.server.writing->ice_getIdentity().name;
mode += "W";
}
+ if (info.server.actions)
+ {
+ componentName = info.server.actions->ice_getIdentity().name;
+ mode += "A";
+ }
int col = 0;
grid.add(Label(name), {row, col});
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.cpp b/source/RobotAPI/libraries/armem/mns/Registry.cpp
index 260cfcd818a426ae7b4f18c6cde5a4180a459f0b..b8b2d02c807750aad2cb36c28cd55013b2a0b496 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.cpp
+++ b/source/RobotAPI/libraries/armem/mns/Registry.cpp
@@ -1,6 +1,7 @@
#include "Registry.h"
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/interface/armem/server/ActionsInterface.h>
namespace armarx::armem::mns
@@ -158,5 +159,11 @@ namespace armarx::armem::mns
return server.writing;
}
+
+ server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server)
+ {
+ return server.actions;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/mns/Registry.h b/source/RobotAPI/libraries/armem/mns/Registry.h
index c1965bdac24fcdf943ad2b7bc945698358dd3f0c..7fcd1c04b95b5b0fe938ae4fdc383ada5bd147d5 100644
--- a/source/RobotAPI/libraries/armem/mns/Registry.h
+++ b/source/RobotAPI/libraries/armem/mns/Registry.h
@@ -3,6 +3,7 @@
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ActionsInterface.h>
#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
#include <RobotAPI/interface/armem/server/WritingMemoryInterface.h>
@@ -68,4 +69,5 @@ namespace armarx::armem::mns
server::ReadingMemoryInterfacePrx getReadingInterface(const dto::MemoryServerInterfaces& server);
server::WritingMemoryInterfacePrx getWritingInterface(const dto::MemoryServerInterfaces& server);
+ server::actions::ActionsInterfacePrx getActionsInterface(const dto::MemoryServerInterfaces& server);
}
diff --git a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
index 5c8387726e6eb65307e66173bcb7e7a0984cdd6b..7ae0bc4bac3f4e3166f47fd4777d44d68f41a86f 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/Plugin.cpp
@@ -77,6 +77,7 @@ namespace armarx::armem::server::plugins
mns::dto::MemoryServerInterfaces server;
server.reading = ReadingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
server.writing = WritingMemoryInterfacePrx::uncheckedCast(parent.getProxy());
+ server.actions = actions::ActionsInterfacePrx::uncheckedCast(parent.getProxy());
mns::dto::RegisterServerResult result;
try
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
index 285c65e0b02895bbb59663a461db1249330daf13..41e09c13be1cbc68f0281023596be9c275f16dc5 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.cpp
@@ -91,4 +91,30 @@ namespace armarx::armem::server::plugins
return plugin->longtermMemory;
}
-}
+ // ACTIONS
+ armem::actions::GetActionsOutputSeq ReadWritePluginUser::getActions(
+ const armem::actions::GetActionsInputSeq& inputs, const ::Ice::Current& /*unused*/)
+ {
+ return getActions(inputs);
+ }
+
+ armem::actions::GetActionsOutputSeq ReadWritePluginUser::getActions(
+ const armem::actions::GetActionsInputSeq& inputs)
+ {
+ (void) inputs;
+ return {};
+ }
+
+ armem::actions::ExecuteActionOutputSeq ReadWritePluginUser::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs, const ::Ice::Current& /*unused*/)
+ {
+ return executeActions(inputs);
+ }
+
+ armem::actions::ExecuteActionOutputSeq ReadWritePluginUser::executeActions(
+ const armem::actions::ExecuteActionInputSeq& inputs)
+ {
+ return {};
+ }
+
+} // namespace armarx::armem::server::plugins
diff --git a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
index 8d3dad4c98d3f11592b250ce11a10fc96dc8cd81..83e761281bf064a111a90efe15a9148689daa2ff 100644
--- a/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
+++ b/source/RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h
@@ -3,6 +3,7 @@
#include <RobotAPI/libraries/armem/server/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
+#include <RobotAPI/libraries/armem/core/actions.h>
#include <RobotAPI/interface/armem/server/MemoryInterface.h>
#include <ArmarXCore/core/ManagedIceObject.h>
@@ -47,6 +48,12 @@ namespace armarx::armem::server::plugins
// StoringInterface interface
virtual data::StoreResult store(const data::StoreInput&, const Ice::Current& = Ice::emptyCurrent) override;
+ // ActionsInterface interface
+ virtual armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs);
+ virtual armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs);
+
+ virtual armem::actions::GetActionsOutputSeq getActions(const armem::actions::GetActionsInputSeq& inputs, const ::Ice::Current&) override;
+ virtual armem::actions::ExecuteActionOutputSeq executeActions(const armem::actions::ExecuteActionInputSeq& inputs, const ::Ice::Current&) override;
public:
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8bf2121617d084537b456fb741fad34fcbf8e26
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.cpp
@@ -0,0 +1,52 @@
+#include "ActionsMenuBuilder.h"
+
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+
+
+namespace armarx::armem::gui
+{
+ ActionsMenuBuilder::ActionsMenuBuilder(
+ MemoryID memoryID,
+ QWidget* parent,
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func) :
+ memoryID(std::move(memoryID)), parent(parent), func(std::move(func))
+ {
+ }
+
+ QMenu*
+ ActionsMenuBuilder::buildActionsMenu(actions::data::GetActionsOutput& actionsOutput)
+ {
+ auto* menu = new QMenu("Actions", parent);
+
+ for (const auto& entry : actions::Menu::fromIce(actionsOutput.menu).entries)
+ {
+ addMenuEntry(menu, {}, entry);
+ }
+
+ return menu;
+ }
+
+ void
+ ActionsMenuBuilder::addMenuEntry( // NOLINT (clangd complains about the recursion here)
+ QMenu* menu,
+ actions::ActionPath path,
+ const actions::MenuEntry& entry)
+ {
+ path.push_back(entry.id);
+ if (entry.entries.empty())
+ {
+ menu->addAction(QString::fromStdString(entry.text),
+ parent,
+ [this, path]() { func(memoryID, path); });
+ }
+ else
+ {
+ QMenu* qSubmenu = menu->addMenu(QString::fromStdString(entry.text));
+ for (const auto& subEntry : entry.entries)
+ {
+ addMenuEntry(qSubmenu, path, subEntry);
+ }
+ }
+ }
+
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d1261f65cecf4cfd4afd77867ad4e2b9af7ebac
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <QMenu>
+
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/libraries/armem/core/actions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+namespace armarx::armem::gui
+{
+ class ActionsMenuBuilder
+ {
+ public:
+ ActionsMenuBuilder(MemoryID memoryID,
+ QWidget* parent,
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func);
+
+ QMenu* buildActionsMenu(actions::data::GetActionsOutput& actionsOutput);
+
+ private:
+ void
+ addMenuEntry(QMenu* menu, actions::ActionPath path, const actions::MenuEntry& entry);
+
+ MemoryID memoryID;
+ QWidget* parent;
+ std::function<void(const MemoryID&, const actions::ActionPath&)> func;
+ };
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
index 5a774f498c13b372fa2b188bba2d6b1508fc3094..b92bcbfbce86f7bc65bee7a9f186c73d14d0c9b5 100644
--- a/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_gui/CMakeLists.txt
@@ -21,6 +21,7 @@ set(SOURCES
gui_utils.cpp
lifecycle.cpp
+ ActionsMenuBuilder.cpp
MemoryViewer.cpp
PeriodicUpdateWidget.cpp
@@ -60,6 +61,7 @@ set(HEADERS
gui_utils.h
lifecycle.h
+ ActionsMenuBuilder.h
MemoryViewer.h
PeriodicUpdateWidget.h
TreeWidgetBuilder.h
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index ddb264ec45b8b36ea0867e453c68a93a136e88bb..318329d8eb16a348338132ad276e819e631b1cdb 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -1,45 +1,47 @@
#include "MemoryViewer.h"
-#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
-
-#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
-#include <RobotAPI/libraries/armem_gui/gui_utils.h>
-
-#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
-#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
-
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/observers/variant/Variant.h>
-
-#include <SimoxUtility/algorithm/get_map_keys_values.h>
-
#include <QApplication>
#include <QBoxLayout>
#include <QCheckBox>
#include <QClipboard>
#include <QDialog>
#include <QGroupBox>
-#include <QMenu>
#include <QLabel>
#include <QLayout>
+#include <QMenu>
#include <QSettings>
#include <QTimer>
-#include <iomanip>
+#include <Ice/Exception.h>
+
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
+#include <RobotAPI/interface/armem/actions.h>
+#include <RobotAPI/interface/armem/memory.h>
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/libraries/armem/core/wm/ice_conversions.h>
+#include <RobotAPI/libraries/armem/server/query_proc/ltm/disk/ltm.h>
+#include <RobotAPI/libraries/armem/server/query_proc/wm/wm.h>
+#include <RobotAPI/libraries/armem_gui/ActionsMenuBuilder.h>
+#include <RobotAPI/libraries/armem_gui/gui_utils.h>
namespace armarx::armem::gui
{
- MemoryViewer::MemoryViewer(
- QBoxLayout* updateWidgetLayout,
- QGroupBox* memoryGroupBox, QLayout* memoryGroupBoxParentLayout,
- QGroupBox* instanceGroupBox, QLayout* instanceGroupBoxParentLayout,
- QBoxLayout* diskControlWidgetLayout,
- QLabel* statusLabel
- )
+ MemoryViewer::MemoryViewer(QBoxLayout* updateWidgetLayout,
+ QGroupBox* memoryGroupBox,
+ QLayout* memoryGroupBoxParentLayout,
+ QGroupBox* instanceGroupBox,
+ QLayout* instanceGroupBoxParentLayout,
+ QBoxLayout* diskControlWidgetLayout,
+ QLabel* statusLabel)
{
Logging::setTag("MemoryViewer");
@@ -47,19 +49,17 @@ namespace armarx::armem::gui
this->statusLabel->clear();
statusLabel->setContextMenuPolicy(Qt::CustomContextMenu);
- connect(statusLabel, &QLabel::customContextMenuRequested, [statusLabel](const QPoint & pos)
- {
- QMenu menu(statusLabel);
- menu.addAction("Copy to clipboard", [statusLabel]()
- {
- QApplication::clipboard()->setText(statusLabel->text());
- });
- menu.addAction("Clear status", [statusLabel]()
- {
- statusLabel->clear();
- });
- menu.exec(statusLabel->mapToGlobal(pos));
- });
+ connect(statusLabel,
+ &QLabel::customContextMenuRequested,
+ [statusLabel](const QPoint& pos)
+ {
+ QMenu menu(statusLabel);
+ menu.addAction("Copy to clipboard",
+ [statusLabel]()
+ { QApplication::clipboard()->setText(statusLabel->text()); });
+ menu.addAction("Clear status", [statusLabel]() { statusLabel->clear(); });
+ menu.exec(statusLabel->mapToGlobal(pos));
+ });
// Update timer
@@ -68,7 +68,7 @@ namespace armarx::armem::gui
updateWidgetLayout->insertWidget(0, updateWidget);
processQueryResultTimer = new QTimer(this);
- processQueryResultTimer->setInterval(1000 / 60); // Keep this stable.
+ processQueryResultTimer->setInterval(1000 / 60); // Keep this stable.
processQueryResultTimer->start();
// Memory View
@@ -91,8 +91,14 @@ namespace armarx::armem::gui
}
// Connections
- connect(diskControl, &armem::gui::disk::ControlWidget::requestedStoreOnDisk, this, &This::storeOnDisk);
- connect(diskControl, &armem::gui::disk::ControlWidget::requestedLoadFromDisk, this, &This::loadFromDisk);
+ connect(diskControl,
+ &armem::gui::disk::ControlWidget::requestedStoreOnDisk,
+ this,
+ &This::storeOnDisk);
+ connect(diskControl,
+ &armem::gui::disk::ControlWidget::requestedLoadFromDisk,
+ this,
+ &This::loadFromDisk);
connect(this, &This::connected, this, &This::startQueries);
connect(updateWidget, &armem::gui::PeriodicUpdateWidget::update, this, &This::startQueries);
@@ -102,21 +108,43 @@ namespace armarx::armem::gui
connect(memoryGroup->commitWidget(), &armem::gui::CommitWidget::commit, this, &This::commit);
connect(this, &This::memoryDataChanged, this, &This::updateMemoryTree);
- connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::selectedItemChanged, this, &This::updateInstanceTree);
-
- connect(memoryGroup->tree(), &armem::gui::MemoryTreeWidget::updated, this, &This::memoryTreeUpdated);
- connect(instanceGroup, &armem::gui::InstanceGroupBox::viewUpdated, this, &This::instanceTreeUpdated);
- connect(instanceGroup->view, &armem::gui::InstanceView::memoryIdResolutionRequested, this, &This::resolveMemoryID);
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::selectedItemChanged,
+ this,
+ &This::updateInstanceTree);
+
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::updated,
+ this,
+ &This::memoryTreeUpdated);
+ connect(memoryGroup->tree(),
+ &armem::gui::MemoryTreeWidget::actionsMenuRequested,
+ this,
+ &This::showActionsMenu);
+ connect(instanceGroup,
+ &armem::gui::InstanceGroupBox::viewUpdated,
+ this,
+ &This::instanceTreeUpdated);
+ connect(instanceGroup->view,
+ &armem::gui::InstanceView::memoryIdResolutionRequested,
+ this,
+ &This::resolveMemoryID);
+ connect(instanceGroup->view,
+ &armem::gui::InstanceView::actionsMenuRequested,
+ this,
+ &This::showActionsMenu);
}
- void MemoryViewer::setLogTag(const std::string& _tag) // Leading _ silences a warning
+ void
+ MemoryViewer::setLogTag(const std::string& _tag) // Leading _ silences a warning
{
Logging::setTag(_tag);
}
- void MemoryViewer::onInit(ManagedIceObject& component)
+ void
+ MemoryViewer::onInit(ManagedIceObject& component)
{
if (mnsName.size() > 0)
{
@@ -131,7 +159,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::onConnect(ManagedIceObject& component)
+ void
+ MemoryViewer::onConnect(ManagedIceObject& component)
{
if (not mnsName.empty())
{
@@ -154,7 +183,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::onDisconnect(ManagedIceObject&)
+ void
+ MemoryViewer::onDisconnect(ManagedIceObject&)
{
updateWidget->stopTimer();
@@ -162,14 +192,15 @@ namespace armarx::armem::gui
}
- const armem::wm::Memory* MemoryViewer::getSingleMemoryData(const std::string& memoryName)
+ const armem::wm::Memory*
+ MemoryViewer::getSingleMemoryData(const std::string& memoryName)
{
auto it = memoryData.find(memoryName);
if (it == memoryData.end())
{
std::stringstream ss;
- ss << "Memory name '" << memoryName << "' is unknown. Known are: "
- << simox::alg::get_keys(memoryData);
+ ss << "Memory name '" << memoryName
+ << "' is unknown. Known are: " << simox::alg::get_keys(memoryData);
statusLabel->setText(QString::fromStdString(ss.str()));
return nullptr;
}
@@ -185,7 +216,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::storeInLTM()
+ void
+ MemoryViewer::storeInLTM()
{
TIMING_START(MemoryStore);
@@ -244,7 +276,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::storeOnDisk(QString directory)
+ void
+ MemoryViewer::storeOnDisk(QString directory)
{
TIMING_START(MemoryExport)
@@ -256,10 +289,12 @@ namespace armarx::armem::gui
}
- void MemoryViewer::loadFromDisk(QString directory)
+ void
+ MemoryViewer::loadFromDisk(QString directory)
{
std::string status;
- std::map<std::string, wm::Memory> data = diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
+ std::map<std::string, wm::Memory> data =
+ diskControl->loadFromDisk(directory, memoryGroup->queryWidget()->queryInput(), &status);
memoryWriters = mns.getAllWriters(true);
for (auto& [name, memory] : data)
@@ -281,16 +316,19 @@ namespace armarx::armem::gui
}
- void MemoryViewer::startQueries()
+ void
+ MemoryViewer::startQueries()
{
memoryReaders = mns.getAllReaders(true);
startDueQueries(runningQueries, memoryReaders);
}
- void MemoryViewer::processQueryResults()
+ void
+ MemoryViewer::processQueryResults()
{
- const std::map<std::string, client::QueryResult> results = collectQueryResults(runningQueries, memoryReaders);
+ const std::map<std::string, client::QueryResult> results =
+ collectQueryResults(runningQueries, memoryReaders);
int errorCount = 0;
applyQueryResults(results, &errorCount);
@@ -300,7 +338,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::updateStatusLabel(int errorCount)
+ void
+ MemoryViewer::updateStatusLabel(int errorCount)
{
// Code to output status label information
if (statusLabel and errorCount > 0)
@@ -317,9 +356,9 @@ namespace armarx::armem::gui
}
- void MemoryViewer::startDueQueries(
- std::map<std::string, Ice::AsyncResultPtr>& queries,
- const std::map<std::string, armem::client::Reader>& readers)
+ void
+ MemoryViewer::startDueQueries(std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, armem::client::Reader>& readers)
{
armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
@@ -334,9 +373,8 @@ namespace armarx::armem::gui
std::map<std::string, client::QueryResult>
- MemoryViewer::collectQueryResults(
- std::map<std::string, Ice::AsyncResultPtr>& queries,
- const std::map<std::string, client::Reader>& readers)
+ MemoryViewer::collectQueryResults(std::map<std::string, Ice::AsyncResultPtr>& queries,
+ const std::map<std::string, client::Reader>& readers)
{
TIMING_START(tCollectQueryResults)
@@ -352,7 +390,8 @@ namespace armarx::armem::gui
{
try
{
- results[name] = client::QueryResult::fromIce(jt->second.memoryPrx->end_query(queryPromise));
+ results[name] = client::QueryResult::fromIce(
+ jt->second.memoryPrx->end_query(queryPromise));
}
catch (const Ice::ConnectionRefusedException&)
{
@@ -366,27 +405,29 @@ namespace armarx::armem::gui
}
else
{
- ++it; // Uncompleted => Keep.
+ ++it; // Uncompleted => Keep.
}
}
TIMING_END_STREAM(tCollectQueryResults, ARMARX_VERBOSE)
if (debugObserver)
{
- debugObserver->begin_setDebugChannel(Logging::tag.tagName,
- {
- { "t Collect Query Results [ms]", new Variant(tCollectQueryResults.toMilliSecondsDouble()) },
- { "# Collected Query Results", new Variant(static_cast<int>(results.size())) },
- });
+ debugObserver->begin_setDebugChannel(
+ Logging::tag.tagName,
+ {
+ {"t Collect Query Results [ms]",
+ new Variant(tCollectQueryResults.toMilliSecondsDouble())},
+ {"# Collected Query Results", new Variant(static_cast<int>(results.size()))},
+ });
}
return results;
}
- void MemoryViewer::applyQueryResults(
- const std::map<std::string, client::QueryResult>& results,
- int* outErrorCount)
+ void
+ MemoryViewer::applyQueryResults(const std::map<std::string, client::QueryResult>& results,
+ int* outErrorCount)
{
TIMING_START(tProcessQueryResults)
for (const auto& [name, result] : results)
@@ -397,7 +438,8 @@ namespace armarx::armem::gui
}
else
{
- ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n" << result.errorMessage;
+ ARMARX_WARNING << "Querying memory server '" << name << "' produced an error: \n"
+ << result.errorMessage;
if (outErrorCount)
{
outErrorCount++;
@@ -422,16 +464,19 @@ namespace armarx::armem::gui
TIMING_END_STREAM(tProcessQueryResults, ARMARX_VERBOSE)
if (debugObserver)
{
- debugObserver->begin_setDebugChannel(Logging::tag.tagName,
- {
- { "t Process Query Results [ms]", new Variant(tProcessQueryResults.toMilliSecondsDouble()) },
- { "# Processed Query Results", new Variant(static_cast<int>(results.size())) },
- });
+ debugObserver->begin_setDebugChannel(
+ Logging::tag.tagName,
+ {
+ {"t Process Query Results [ms]",
+ new Variant(tProcessQueryResults.toMilliSecondsDouble())},
+ {"# Processed Query Results", new Variant(static_cast<int>(results.size()))},
+ });
}
}
- void MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
+ void
+ MemoryViewer::updateInstanceTree(const armem::MemoryID& selectedID)
{
const armem::wm::Memory* data = getSingleMemoryData(selectedID.memoryName);
if (data)
@@ -481,11 +526,12 @@ namespace armarx::armem::gui
}
- void MemoryViewer::resolveMemoryID(const MemoryID& id)
+ void
+ MemoryViewer::resolveMemoryID(const MemoryID& id)
{
// ARMARX_IMPORTANT << "Resolving memory ID: " << id;
- auto handleError = [this](const std::string & msg)
+ auto handleError = [this](const std::string& msg)
{
statusLabel->setText(QString::fromStdString(msg));
ARMARX_WARNING << msg;
@@ -548,7 +594,8 @@ namespace armarx::armem::gui
}
- void MemoryViewer::updateMemoryTree()
+ void
+ MemoryViewer::updateMemoryTree()
{
std::map<std::string, const armem::wm::Memory*> convMap;
for (auto& [name, data] : memoryData)
@@ -564,7 +611,9 @@ namespace armarx::armem::gui
{
try
{
- debugObserver->setDebugDatafield(Logging::tag.tagName, "GUI Update [ms]", new Variant(GuiUpdate.toMilliSecondsDouble()));
+ debugObserver->setDebugDatafield(Logging::tag.tagName,
+ "GUI Update [ms]",
+ new Variant(GuiUpdate.toMilliSecondsDouble()));
}
catch (const Ice::Exception&)
{
@@ -574,28 +623,140 @@ namespace armarx::armem::gui
}
+ void
+ MemoryViewer::showActionsMenu(const MemoryID& memoryID,
+ QWidget* parent,
+ const QPoint& pos,
+ QMenu* menu)
+ {
+ // Called if we have to stop because of an error.
+ auto showMenu = [menu, pos]()
+ {
+ if (menu)
+ menu->exec(pos);
+ };
+
+ mns::dto::MemoryServerInterfaces prx;
+ try
+ {
+ prx = mns.resolveServer(memoryID);
+ }
+ catch (const error::CouldNotResolveMemoryServer& e)
+ {
+ statusLabel->setText(
+ QString::fromStdString(
+ e.makeMsg(memoryID, "Could not resolve memory server.")));
+ showMenu();
+ return;
+ }
+
+ if (!prx.actions)
+ {
+ std::stringstream ss;
+ ss << "Memory server " << memoryID
+ << " does not support actions or is offline.";
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ showMenu();
+ return;
+ }
+
+ actions::GetActionsOutputSeq result;
+ try
+ {
+ result = prx.actions->getActions({{toIce<data::MemoryID>(memoryID)}});
+ }
+ catch (const Ice::LocalException& e)
+ {
+ std::stringstream ss;
+ ss << "Could not get actions for " << memoryID << ".";
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ showMenu();
+ return;
+ }
+
+ if (result.size() == 0)
+ {
+ showMenu();
+ return;
+ }
+ auto builder = ActionsMenuBuilder(
+ memoryID,
+ parent,
+ [this, prx](const MemoryID& memoryID, const actions::ActionPath& path)
+ {
+ actions::data::ExecuteActionOutputSeq result;
+ try
+ {
+ result = prx.actions->executeActions(
+ {{toIce<armem::data::MemoryID>(memoryID), path}});
+ }
+ catch (const Ice::LocalException& e)
+ {
+ std::stringstream ss;
+ ss << "Failed to execute action: " << e.what();
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+
+ for (const auto& [success, errorMessage] : result)
+ {
+ if (not success)
+ {
+ std::stringstream ss;
+ ss << "Failed to execute action: " << errorMessage;
+ statusLabel->setText(QString::fromStdString(ss.str()));
+ ARMARX_WARNING << ss.str();
+ }
+ }
+ });
+
+ QMenu* actionsMenu = builder.buildActionsMenu(result[0]);
+ if (menu == nullptr)
+ {
+ actionsMenu->exec(pos);
+ }
+ else
+ {
+ menu->addMenu(actionsMenu);
+ menu->exec(pos);
+ }
+ }
+
+
const static std::string CONFIG_KEY_MEMORY = "MemoryViewer.MemoryNameSystem";
const static std::string CONFIG_KEY_DEBUG_OBSERVER = "MemoryViewer.DebugObserverName";
- void MemoryViewer::loadSettings(QSettings* settings)
+ void
+ MemoryViewer::loadSettings(QSettings* settings)
{
- mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem").toString().toStdString();
- debugObserverName = settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver").toString().toStdString();
+ mnsName = settings->value(QString::fromStdString(CONFIG_KEY_MEMORY), "MemoryNameSystem")
+ .toString()
+ .toStdString();
+ debugObserverName =
+ settings->value(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), "DebugObserver")
+ .toString()
+ .toStdString();
}
- void MemoryViewer::saveSettings(QSettings* settings)
+ void
+ MemoryViewer::saveSettings(QSettings* settings)
{
- settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY), QString::fromStdString(mnsName));
- settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER), QString::fromStdString(debugObserverName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_MEMORY),
+ QString::fromStdString(mnsName));
+ settings->setValue(QString::fromStdString(CONFIG_KEY_DEBUG_OBSERVER),
+ QString::fromStdString(debugObserverName));
}
- void MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
+ void
+ MemoryViewer::writeConfigDialog(SimpleConfigDialog* dialog)
{
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>({CONFIG_KEY_MEMORY, "MemoryNameSystem", "MemoryNameSystem"});
- dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>({CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>(
+ {CONFIG_KEY_MEMORY, "MemoryNameSystem", "MemoryNameSystem"});
+ dialog->addProxyFinder<armarx::DebugObserverInterfacePrx>(
+ {CONFIG_KEY_DEBUG_OBSERVER, "Debug Observer", "DebugObserver"});
}
- void MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
+ void
+ MemoryViewer::readConfigDialog(SimpleConfigDialog* dialog)
{
mnsName = dialog->getProxyName(CONFIG_KEY_MEMORY);
if (mnsName.empty())
@@ -605,4 +766,4 @@ namespace armarx::armem::gui
debugObserverName = dialog->getProxyName(CONFIG_KEY_DEBUG_OBSERVER);
}
-}
+} // namespace armarx::armem::gui
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
index b4ea2897516a60e01ff13709ab41d7e5df413964..51cda4cb797911c3afcd1afaf43657b145b9a140 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.h
@@ -7,6 +7,7 @@
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <RobotAPI/interface/armem/actions.h>
#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
@@ -71,6 +72,9 @@ namespace armarx::armem::gui
void resolveMemoryID(const MemoryID& id);
+ void showActionsMenu(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
+
// Disk Control
void storeOnDisk(QString directory);
void loadFromDisk(QString directory);
@@ -98,7 +102,6 @@ namespace armarx::armem::gui
void updateMemoryTree();
-
signals:
void memoryDataChanged();
@@ -130,7 +133,6 @@ namespace armarx::armem::gui
int* outErrorCount = nullptr);
-
public:
std::string mnsName;
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
index 6cdbcab8351b7597bd5dc0dab8e1ed07408f5ad9..3bbb2ee55a87be3216386a0f847b40829b4bb785 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.cpp
@@ -253,13 +253,13 @@ namespace armarx::armem::gui::instance
return; // No item => no context menu.
}
- QMenu menu(this);
+ QMenu* menu = new QMenu(this);
if (item == this->treeItemInstanceID && currentInstance.has_value())
{
if (QAction* action = makeActionCopyMemoryID(currentInstance->id()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
}
else if (item == this->treeItemData && currentInstance.has_value())
@@ -267,11 +267,10 @@ namespace armarx::armem::gui::instance
auto actions = makeActionsCopyDataToClipboard();
for (const auto& action : actions)
{
- if (!action)
+ if (action)
{
- continue;
+ menu->addAction(action);
}
- menu.addAction(action);
}
}
else if (item->parent() == nullptr)
@@ -288,7 +287,7 @@ namespace armarx::armem::gui::instance
if (const std::optional<aron::Path> path = getElementPath(item))
{
QAction* viewAction = new QAction("Show image");
- menu.addAction(viewAction);
+ menu->addAction(viewAction);
connect(viewAction, &QAction::triggered, [this, path]()
{
this->showImageView(path.value());
@@ -330,11 +329,11 @@ namespace armarx::armem::gui::instance
{
if (QAction* action = makeActionCopyMemoryID(id.value()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
if (QAction* action = makeActionResolveMemoryID(id.value()))
{
- menu.addAction(action);
+ menu->addAction(action);
}
}
}
@@ -346,17 +345,20 @@ namespace armarx::armem::gui::instance
auto actions = makeActionsCopyDataToClipboard(elementPath.value());
for (const auto& action : actions)
{
- if (!action)
+ if (action)
{
- continue;
+ menu->addAction(action);
}
- menu.addAction(action);
}
}
- if (menu.actions().size() > 0)
+ if (menu->actions().size() > 0)
{
- menu.exec(tree->mapToGlobal(pos));
+ emit actionsMenuRequested(currentInstance->id(), this, tree->mapToGlobal(pos), menu);
+ }
+ else
+ {
+ emit actionsMenuRequested(currentInstance->id(), this, tree->mapToGlobal(pos), nullptr);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
index 289e67caff9749d8a3e8012ea2c2798507b255aa..982d277db1febb68070290d3c86a3f06c602b990 100644
--- a/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
+++ b/source/RobotAPI/libraries/armem_gui/instance/InstanceView.h
@@ -3,6 +3,7 @@
#include <optional>
#include <deque>
+#include <QMenu>
#include <QWidget>
#include <QGroupBox>
@@ -55,6 +56,8 @@ namespace armarx::armem::gui::instance
void updated();
void instanceSelected(const MemoryID& id);
void memoryIdResolutionRequested(const MemoryID& id);
+ void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
private slots:
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
index 0ece75cfb419df323a3269a7baacb8d3e4fbba6e..858c932bdba6825696a99de538bbdf02284c159c 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.cpp
@@ -3,10 +3,12 @@
#include <RobotAPI/libraries/armem_gui/instance/sanitize_typename.h>
#include <RobotAPI/libraries/aron/core/type/variant/container/Object.h>
+#include <RobotAPI/interface/armem/actions.h>
#include <SimoxUtility/algorithm/string.h>
#include <QHeaderView>
+#include <QMenu>
namespace armarx::armem::gui::memory
@@ -45,6 +47,9 @@ namespace armarx::armem::gui::memory
connect(this, &This::entitySelected, this, &This::itemSelected);
connect(this, &This::snapshotSelected, this, &This::itemSelected);
connect(this, &This::instanceSelected, this, &This::itemSelected);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(this, &QTreeWidget::customContextMenuRequested, this, &This::prepareTreeContextMenu);
}
void TreeWidget::initBuilders()
@@ -269,6 +274,18 @@ namespace armarx::armem::gui::memory
(void) data, (void) dataItem;
}
+ void TreeWidget::prepareTreeContextMenu(const QPoint& pos)
+ {
+ const QTreeWidgetItem* item = this->itemAt(pos);
+ if (item == nullptr)
+ {
+ return;
+ }
+
+ MemoryID memoryID(item->data(int(Columns::ID), Qt::UserRole).toString().toStdString());
+ emit actionsMenuRequested(memoryID, this, mapToGlobal(pos), nullptr);
+ }
+
template <class MemoryItemT>
QTreeWidgetItem* TreeWidget::makeItem(const std::string& key, const MemoryItemT& memoryItem)
{
diff --git a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
index 75f56c9ace248e58daf3f7ee3ac69baa12b0edfe..1ad975e5f29e741df801c55906d0ff364e607996 100644
--- a/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/memory/TreeWidget.h
@@ -44,6 +44,9 @@ namespace armarx::armem::gui::memory
void snapshotSelected(const MemoryID& id);
void instanceSelected(const MemoryID& id);
+ void actionsMenuRequested(const MemoryID& memoryID, QWidget* parent,
+ const QPoint& pos, QMenu* menu);
+
private slots:
@@ -65,6 +68,7 @@ namespace armarx::armem::gui::memory
void updateChildren(const armem::wm::EntitySnapshot& snapshot, QTreeWidgetItem* snapshotItem);
void updateChildren(const armem::wm::EntityInstance& data, QTreeWidgetItem* parent);
+ void prepareTreeContextMenu(const QPoint& pos);
template <class MemoryItemT>
QTreeWidgetItem* makeItem(const std::string& key, const MemoryItemT& memoryItem);
diff --git a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
index 74928ae14dc40cc72b49c6a81570525336c48bfa..b0663babe7059ddbc2915869dc49d06a128f03e5 100644
--- a/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_objects/CMakeLists.txt
@@ -21,8 +21,6 @@ armarx_add_library(
aron_conversions.h
aron_forward_declarations.h
- SceneSnapshot.h
-
server/class/FloorVis.h
server/class/Segment.h
@@ -50,8 +48,6 @@ armarx_add_library(
SOURCES
aron_conversions.cpp
- SceneSnapshot.cpp
-
client/articulated_object/Reader.cpp
client/articulated_object/Writer.cpp
client/articulated_object/ArticulatedObjectReader.cpp
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
deleted file mode 100644
index d3ece87d9746198af2f5b9acc5b81cce38d189aa..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-#include "SceneSnapshot.h"
-
-#include <SimoxUtility/json.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-// #include <RobotAPI/libraries/armem/core/json_conversions.h>
-
-
-namespace armarx::armem::obj
-{
-
- ObjectID SceneSnapshot::Object::getClassID() const
- {
- return ObjectID(className);
- }
-
-
- ObjectID SceneSnapshot::Object::getClassID(ObjectFinder& finder) const
- {
- ObjectID id = getClassID();
- if (id.dataset().empty())
- {
- if (std::optional<ObjectInfo> info = finder.findObject(id.className()))
- {
- return info->id();
- }
- }
- return id;
- }
-
-
- ObjectID SceneSnapshot::Object::getObjectID() const
- {
- return getClassID().withInstanceName(instanceName);
- }
-
-
- ObjectID SceneSnapshot::Object::getObjectID(ObjectFinder& finder) const
- {
- return getClassID(finder).withInstanceName(instanceName);
- }
-
-}
-
-
-void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs)
-{
- // j["instanceID"] = rhs.instanceID;
- j["class"] = rhs.className;
- j["instanceName"] = rhs.instanceName;
- j["collection"] = rhs.collection;
- j["position"] = rhs.position;
- j["orientation"] = rhs.orientation;
- j["jointValues"] = rhs.jointValues;
-}
-
-
-void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs)
-{
- // j.at("instanceID").get_to(rhs.instanceID);
- j.at("class").get_to(rhs.className);
- if (j.count("instanceName"))
- {
- j["instanceName"].get_to(rhs.instanceName);
- }
- j.at("collection").get_to(rhs.collection);
- j.at("position").get_to(rhs.position);
- j.at("orientation").get_to(rhs.orientation);
- if (j.count("jointValues"))
- {
- j.at("jointValues").get_to(rhs.jointValues);
- }
-}
-
-
-
-void armarx::armem::obj::to_json(nlohmann::json& j, const SceneSnapshot& rhs)
-{
- j["objects"] = rhs.objects;
-}
-
-
-void armarx::armem::obj::from_json(const nlohmann::json& j, SceneSnapshot& rhs)
-{
- j.at("objects").get_to(rhs.objects);
-}
-
-
diff --git a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h b/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
deleted file mode 100644
index 3e24156490544ebc537372a0d204c052ae8b2428..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_objects/SceneSnapshot.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @package RobotAPI::ArmarXObjects::ObjectMemory
- * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
- * @date 2021
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-#pragma once
-
-#include <SimoxUtility/json/json.hpp>
-
-#include <Eigen/Geometry>
-
-
-namespace armarx
-{
- class ObjectID;
- class ObjectFinder;
-}
-namespace armarx::armem::obj
-{
-
- struct SceneSnapshot
- {
- struct Object
- {
- std::string className;
- std::string instanceName;
- std::string collection;
-
- Eigen::Vector3f position = Eigen::Vector3f::Zero();
- Eigen::Quaternionf orientation = Eigen::Quaternionf::Identity();
-
- std::map<std::string, float> jointValues;
-
- ObjectID getClassID() const;
- ObjectID getClassID(ObjectFinder& finder) const;
- ObjectID getObjectID() const;
- ObjectID getObjectID(ObjectFinder& finder) const;
- };
- std::vector<Object> objects;
- };
-
- void to_json(nlohmann::json& j, const SceneSnapshot::Object& rhs);
- void from_json(const nlohmann::json& j, SceneSnapshot::Object& rhs);
-
- void to_json(nlohmann::json& j, const SceneSnapshot& rhs);
- void from_json(const nlohmann::json& j, SceneSnapshot& rhs);
-
-}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index f28cd81e1767d5e2ba6b983b8bbe9efed4958ef4..485715690f3fdbfe089f3557582bae86ef7210ad 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -1,9 +1,11 @@
#include "Segment.h"
+#include <RobotAPI/libraries/ArmarXObjects/Scene.h>
+#include <RobotAPI/libraries/ArmarXObjects/json_conversions.h>
+
#include <RobotAPI/libraries/armem_objects/aron/ObjectClass.aron.generated.h>
#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.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>
@@ -630,7 +632,8 @@ namespace armarx::armem::server::obj::instance
// Store attachment in new entity snapshot.
{
- armem::EntityUpdate update;
+ armem::Commit commit;
+ armem::EntityUpdate & update = commit.add();
update.entityID = objectEntity->id();
update.timeCreated = now;
{
@@ -639,7 +642,7 @@ namespace armarx::armem::server::obj::instance
updated.pose.attachmentValid = true;
update.instancesData = { updated.toAron() };
}
- objectEntity->update(update);
+ iceMemory.commit(commit);
}
ARMARX_INFO << "Attached object " << objectID << " by provider '" << data.pose.providerName << "' "
@@ -737,7 +740,8 @@ namespace armarx::armem::server::obj::instance
Time now,
bool commitAttachedPose)
{
- armem::EntityUpdate update;
+ armem::Commit commit;
+ armem::EntityUpdate & update = commit.add();
update.entityID = entity.id();
update.timeCreated = now;
{
@@ -762,7 +766,7 @@ namespace armarx::armem::server::obj::instance
update.instancesData = { updated.toAron() };
}
- entity.update(update);
+ iceMemory.commit(commit);
}
@@ -800,7 +804,7 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::storeScene(const std::string& filename, const armem::obj::SceneSnapshot& scene)
+ void Segment::storeScene(const std::string& filename, const armarx::objects::Scene& scene)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
{
@@ -818,7 +822,7 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<armem::obj::SceneSnapshot>
+ std::optional<armarx::objects::Scene>
Segment::loadScene(const std::string& filename)
{
if (const std::optional<std::filesystem::path> path = resolveSceneFilename(filename))
@@ -832,7 +836,7 @@ namespace armarx::armem::server::obj::instance
}
- std::optional<armem::obj::SceneSnapshot>
+ std::optional<armarx::objects::Scene>
Segment::loadScene(const std::filesystem::path& path)
{
ARMARX_INFO << "Loading scene snapshot from: \n" << path;
@@ -847,7 +851,7 @@ namespace armarx::armem::server::obj::instance
return std::nullopt;
}
- armem::obj::SceneSnapshot snapshot = j;
+ armarx::objects::Scene snapshot = j;
return snapshot;
}
@@ -888,9 +892,9 @@ namespace armarx::armem::server::obj::instance
}
- armem::obj::SceneSnapshot Segment::getSceneSnapshot() const
+ armarx::objects::Scene Segment::getSceneSnapshot() const
{
- armem::obj::SceneSnapshot scene;
+ armarx::objects::Scene scene;
segmentPtr->forEachEntity([&scene](wm::Entity & entity)
{
try
@@ -899,7 +903,7 @@ namespace armarx::armem::server::obj::instance
std::optional<arondto::ObjectInstance> objectInstance = tryCast<arondto::ObjectInstance>(entityInstance);
if (objectInstance)
{
- armem::obj::SceneSnapshot::Object& object = scene.objects.emplace_back();
+ armarx::objects::SceneObject& object = scene.objects.emplace_back();
// object.instanceID = entityInstance.id();
object.className = ObjectID(objectInstance->classID.entityName).getClassID().str();
object.collection = "";
@@ -917,7 +921,7 @@ namespace armarx::armem::server::obj::instance
}
- void Segment::commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName)
+ void Segment::commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName)
{
const Time now = TimeUtil::GetTime();
std::map<ObjectID, int> idCounters;
@@ -1032,7 +1036,7 @@ namespace armarx::armem::server::obj::instance
if (storeButton.wasClicked())
{
- armem::obj::SceneSnapshot scene;
+ armarx::objects::Scene scene;
segment.doLocked([&scene, &segment]()
{
scene = segment.getSceneSnapshot();
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index 51972e678155c56e5dade67cc1a7076d773c569b..24f80704f8580c386bfaf346502cccd9fa86c1c6 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -24,9 +24,9 @@
#include "Decay.h"
-namespace armarx::armem::obj
+namespace armarx::objects
{
- struct SceneSnapshot;
+ struct Scene;
}
namespace armarx::armem::arondto
{
@@ -148,13 +148,13 @@ namespace armarx::armem::server::obj::instance
private:
- 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);
+ 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);
- armem::obj::SceneSnapshot getSceneSnapshot() const;
- void commitSceneSnapshot(const armem::obj::SceneSnapshot& scene, const std::string& sceneName);
+ armarx::objects::Scene getSceneSnapshot() const;
+ void commitSceneSnapshot(const armarx::objects::Scene& scene, const std::string& sceneName);
void commitSceneSnapshotFromFilename(const std::string& filename, bool lockMemory);