diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp index 25fbac039db17aed7c305175d173e561551f5a85..f90f4834d0dedeeb54a6a93cee25f388a618a682 100644 --- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp +++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp @@ -22,7 +22,10 @@ #include "GraphImportExport.h" -#include <armarx/navigation/locations/aron/Location.aron.generated.h> +#include <armarx/navigation/location/aron/Location.aron.generated.h> +#include <armarx/navigation/location/constants.h> +#include <armarx/navigation/graph/aron/Graph.aron.generated.h> +#include <armarx/navigation/graph/constants.h> #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h> #include <MemoryX/core/MemoryXCoreObjectFactories.h> @@ -37,6 +40,13 @@ namespace armarx::nav { + GraphImportExport::GraphImportExport() + { + properties.memoryName = "Navigation"; + properties.locationCoreSegmentName = loc::coreSegmentName; + properties.graphCoreSegmentName = graph::coreSegmentName; + } + armarx::PropertyDefinitionsPtr GraphImportExport::createPropertyDefinitions() { armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier()); @@ -159,9 +169,14 @@ namespace armarx::nav { exportLocations(tab.sceneComboBox.getValue()); } + if (tab.locationsMemoryXToArMemButton.wasClicked()) { - proxies.navigationWriter.addSegment(); + clearArMemProviderSegment(getLocationProviderSegmentID()); + } + if (tab.graphMemoryXToArMemButton.wasClicked()) + { + clearArMemProviderSegment(getGraphProviderSegmentID()); } } @@ -173,13 +188,18 @@ namespace armarx::nav } - void GraphImportExport::clearProviderSegment() + void GraphImportExport::clearArMemProviderSegment(const armem::MemoryID& providerSegmentID) { - armem::data::AddSegmentInput input; - input.coreSegmentName = "Locations"; - input.providerSegmentName = tab.providerSegmentLine.getValue(); - input.clearWhenExists = true; - proxies.navigationWriter.addSegment(input); + const bool clearWhenExists = true; + auto result = proxies.navigationWriter.addSegment(providerSegmentID, clearWhenExists); + if (result.success) + { + ARMARX_IMPORTANT << "Cleared ArMem provider segment " << providerSegmentID << "."; + } + else + { + ARMARX_WARNING << result.errorMessage; + } } @@ -192,33 +212,36 @@ namespace armarx::nav for (memoryx::GraphNodeBasePtr& node : graphNodes) { ARMARX_CHECK_NOT_NULL(node); - - armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose()); - if (pose and not node->isMetaEntity()) + if (not node->isMetaEntity()) { // ID is just some random MongoDB hash const std::string nodeId = node->getId(); + // This is the readable name entered in the GUI. const std::string name = node->getName(); + armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose()); + ARMARX_CHECK_NOT_NULL(pose); + FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node)); ARMARX_CHECK_NOT_NULL(globalNodePose); + // `pose` and `globalNodePose` seem to be identical. Is the last step necessary? + // Maybe `pose` could be non-global. + ARMARX_VERBOSE << std::setprecision(2) << std::fixed << "Processing node " << (commit.updates.size() + 1) << "\n- ID: \t" << nodeId << "\n- Name: \t" << name - << "\n- pose: \n" << pose->toEigen() - << "\n- resolved global pose: \n" << globalNodePose->toEigen() + << "\n- Pose: \n" << pose->toEigen() + << "\n- Resolved global pose: \n" << globalNodePose->toEigen() ; - nav::locs::arondto::Location data; + nav::loc::arondto::Location data; data.globalRobotPose = globalNodePose->toEigen(); armem::EntityUpdate& update = commit.add(); - update.entityID = armem::MemoryID( - "Navigation", "Locations", tab.providerSegmentLine.getValue(), name - ); + update.entityID = getLocationProviderSegmentID().withEntityName(name); update.timeCreated = time; update.instancesData = { data.toAron() }; } @@ -285,14 +308,14 @@ namespace armarx::nav } - armem::MemoryID GraphImportExport::getLocationsProviderSegmentID() const + armem::MemoryID GraphImportExport::getLocationProviderSegmentID() { return armem::MemoryID(properties.memoryName, - properties.locationsCoreSegmentName, + properties.locationCoreSegmentName, tab.providerSegmentLine.getValue()); } - armem::MemoryID GraphImportExport::getGraphProviderSegmentID() const + armem::MemoryID GraphImportExport::getGraphProviderSegmentID() { return armem::MemoryID(properties.memoryName, properties.graphCoreSegmentName, diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h index 93fd05959fd3d6cb76f6a710bd8377079d0788d0..06215c7d62240d6556e61083323bbcbb31ab4350 100644 --- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h +++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h @@ -57,6 +57,9 @@ namespace armarx::nav { public: + GraphImportExport(); + + /// @see armarx::ManagedIceObject::getDefaultName() std::string getDefaultName() const override; @@ -93,21 +96,21 @@ namespace armarx::nav void refreshScenes(); - void clearProviderSegment(); - void exportLocations(const std::string& sceneName); void importLocations(); void exportGraph(); void importGraph(); + void clearArMemProviderSegment(const armem::MemoryID& providerSegmentID); + void drawScene(const std::string& sceneName); void addEdge(const std::string& node1Id, const std::string& node2Id); void addNode(const memoryx::GraphNodeBasePtr& node); - armem::MemoryID getLocationsProviderSegmentID() const; - armem::MemoryID getGraphProviderSegmentID() const; + armem::MemoryID getLocationProviderSegmentID(); + armem::MemoryID getGraphProviderSegmentID(); private: @@ -126,9 +129,9 @@ namespace armarx::nav /// Properties shown in the Scenario GUI. struct Properties { - std::string memoryName = "Navigation"; - std::string locationsCoreSegmentName = "Location"; - std::string graphCoreSegmentName = "Graph"; + std::string memoryName; + std::string locationCoreSegmentName; + std::string graphCoreSegmentName; }; Properties properties;