diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..5ed95be3ea1206b7c9102a82bef6ccd512ec112d --- /dev/null +++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt @@ -0,0 +1,84 @@ +set(LIB_NAME GraphImportExport) + +armarx_component_set_name("${LIB_NAME}") +armarx_set_target("Library: ${LIB_NAME}") + +# If your component needs a special ice interface, define it here: +# armarx_add_component_interface_lib( +# SLICE_FILES +# GraphImportExport.ice +# ICE_LIBS +# # RobotAPI +#) + + +# Add the component +armarx_add_component( + COMPONENT_LIBS + # ArmarXCore + ArmarXCore + ArmarXCoreComponentPlugins # For DebugObserver plugin. + # ArmarXGui + ArmarXGuiComponentPlugins # For RemoteGui plugin. + # RobotAPI + ## RobotAPICore + ## RobotAPIInterfaces + ## RobotAPIComponentPlugins # For ArViz and other plugins. + armem + + # MemoryX + MemoryXCore + MemoryXMemoryTypes + + # This project. + Navigation::Location + Navigation::Graph + + ## ${PROJECT_NAME}Interfaces # For ice interfaces from this package. + # This component + ## GraphImportExportInterfaces # If you defined a component ice interface above. + + SOURCES + GraphImportExport.cpp + + HEADERS + GraphImportExport.h +) + + + +find_package(MemoryX QUIET) +armarx_build_if(MemoryX_FOUND "MemoryX not available") +if(MemoryX_FOUND) + target_include_directories(${LIB_NAME} PUBLIC ${MemoryX_INCLUDE_DIRS}) +endif() + + +# Add dependencies +#find_package(MyLib QUIET) +#armarx_build_if(MyLib_FOUND "MyLib not available") + +# All target_include_directories must be guarded by if(Xyz_FOUND) +# For multiple libraries write: if(X_FOUND AND Y_FOUND) ... +#if(MyLib_FOUND) +# target_include_directories(GraphImportExport PUBLIC ${MyLib_INCLUDE_DIRS}) +#endif() + + +# Add ARON files +#armarx_enable_aron_file_generation_for_target( +# TARGET_NAME +# ${ARMARX_COMPONENT_NAME} +# ARON_FILES +# aron/ExampleData.xml +#) + + +# Add unit tests +# add_subdirectory(test) + +# Generate the application +armarx_generate_and_add_component_executable( + # If your component is not defined in ::armarx, specify its namespace here: + COMPONENT_NAMESPACE "armarx::nav" +) diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp new file mode 100644 index 0000000000000000000000000000000000000000..25fbac039db17aed7c305175d173e561551f5a85 --- /dev/null +++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp @@ -0,0 +1,359 @@ +/* + * 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 MemoryX::ArmarXObjects::GraphImportExport + * @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 + */ + +#include "GraphImportExport.h" + +#include <armarx/navigation/locations/aron/Location.aron.generated.h> + +#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h> +#include <MemoryX/core/MemoryXCoreObjectFactories.h> + +#include <RobotAPI/libraries/core/FramedPose.h> + +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> + +#include <iomanip> + + +namespace armarx::nav +{ + + armarx::PropertyDefinitionsPtr GraphImportExport::createPropertyDefinitions() + { + armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier()); + + // Publish to a topic (passing the TopicListenerPrx). + // def->topic(myTopicListener); + + // Subscribe to a topic (passing the topic name). + // def->topic<PlatformUnitListener>("MyTopic"); + + // Use (and depend on) another component (passing the ComponentInterfacePrx). + def->component(proxies.priorKnowledge); + def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver"); + + + // Add a required property. (The component won't start without a value being set.) + // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz."); + + // Add an optionalproperty. + // def->optional(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz."); + + + return def; + } + + + void GraphImportExport::onInitComponent() + { + // Topics and properties defined above are automagically registered. + + // Keep debug observer data until calling `sendDebugObserverBatch()`. + setDebugObserverBatchModeEnabled(true); + } + + + void GraphImportExport::onConnectComponent() + { + // Get proxies. + if (proxies.priorKnowledge->hasGraphSegment()) + { + proxies.graphSegment = proxies.priorKnowledge->getGraphSegment(); + } + proxies.navigationWriter = memoryNameSystem.useWriter(armem::MemoryID(properties.memoryName)); + + // Setup and start the remote GUI. + refreshScenes(); + RemoteGui_startRunningTask(); + } + + + void GraphImportExport::onDisconnectComponent() + { + } + + + void GraphImportExport::onExitComponent() + { + } + + + std::string GraphImportExport::getDefaultName() const + { + return "GraphImportExport"; + } + + + void GraphImportExport::createRemoteGuiTab(const std::vector<std::string>& sceneNames) + { + using namespace armarx::RemoteGui::Client; + + tab.sceneComboBox.setOptions(sceneNames); + tab.sceneRefreshButton.setLabel("Refresh"); + + tab.providerSegmentLine.setValue(getName()); + + tab.dryRun.setValue(false); + + tab.locationsMemoryXToArMemButton.setLabel("Locations MemoryX -> ArMem"); + tab.locationsArMemToMemoryXButton.setLabel("Locations ArMem -> MemoryX (WIP)"); + tab.locationsClearArMemButton.setLabel("Clear ArMem Locations"); + + tab.graphMemoryXToArMemButton.setLabel("Graph MemoryX -> ArMem (WIP)"); + tab.graphArMemToMemoryXButton.setLabel("Graph ArMem -> MemoryX (WIP)"); + tab.graphClearArMemButton.setLabel("Clear ArMem Graphs"); + + + GridLayout grid; + int row = 0; + { + grid.add(Label("Scene:"), {row, 0}).add(tab.sceneComboBox, {row, 1}).add(tab.sceneRefreshButton, {row, 2}); + ++row; + + grid.add(Label("Provider Segment:"), {row, 0}).add(tab.providerSegmentLine, {row, 1}); + ++row; + + grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1}); + ++row; + + grid.add(tab.locationsMemoryXToArMemButton, {row, 0}).add(tab.locationsArMemToMemoryXButton, {row, 1}) + .add(tab.locationsClearArMemButton, {row, 2}); + ++row; + + grid.add(tab.graphMemoryXToArMemButton, {row, 0}).add(tab.graphArMemToMemoryXButton, {row, 1}) + .add(tab.graphClearArMemButton, {row, 2}); + ++row; + } + + VBoxLayout root = {grid, VSpacer()}; + RemoteGui_createTab(getName(), root, &tab); + } + + + void GraphImportExport::RemoteGui_update() + { + if (tab.sceneRefreshButton.wasClicked()) + { + refreshScenes(); + } + if (tab.locationsMemoryXToArMemButton.wasClicked()) + { + exportLocations(tab.sceneComboBox.getValue()); + } + if (tab.locationsMemoryXToArMemButton.wasClicked()) + { + proxies.navigationWriter.addSegment(); + } + } + + + void GraphImportExport::refreshScenes() + { + const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes(); + createRemoteGuiTab(sceneNames); + } + + + void GraphImportExport::clearProviderSegment() + { + armem::data::AddSegmentInput input; + input.coreSegmentName = "Locations"; + input.providerSegmentName = tab.providerSegmentLine.getValue(); + input.clearWhenExists = true; + proxies.navigationWriter.addSegment(input); + } + + + void GraphImportExport::exportLocations(const std::string& sceneName) + { + const armem::Time time = armem::Time::now(); + armem::Commit commit; + + memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName); + for (memoryx::GraphNodeBasePtr& node : graphNodes) + { + ARMARX_CHECK_NOT_NULL(node); + + armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose()); + if (pose and not node->isMetaEntity()) + { + // ID is just some random MongoDB hash + const std::string nodeId = node->getId(); + const std::string name = node->getName(); + + FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node)); + ARMARX_CHECK_NOT_NULL(globalNodePose); + + 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() + ; + + nav::locs::arondto::Location data; + data.globalRobotPose = globalNodePose->toEigen(); + + armem::EntityUpdate& update = commit.add(); + update.entityID = armem::MemoryID( + "Navigation", "Locations", tab.providerSegmentLine.getValue(), name + ); + update.timeCreated = time; + update.instancesData = { data.toAron() }; + } + } + + if (not tab.dryRun.getValue()) + { + armem::CommitResult result = proxies.navigationWriter.commit(commit); + if (result.allSuccess()) + { + ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations."; + } + else + { + ARMARX_WARNING << result.allErrorMessages(); + } + } + else + { + ARMARX_VERBOSE << "Dry Run - skipping commit."; + } + } + + + void GraphImportExport::drawScene(const std::string& sceneName) + { + memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName); + for (memoryx::GraphNodeBasePtr& node : graphNodes) + { + ARMARX_CHECK_NOT_NULL(node); + + armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose()); + if (pose and not node->isMetaEntity()) + { + const std::string nodeId = node->getId(); + + const std::string name = node->getName(); + FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node)); + ARMARX_CHECK_NOT_NULL(globalNodePose); + } + } + +#if 0 + // Add edges + for (memoryx::GraphNodeBasePtr& node : graphNodes) + { + std::string nodeId = node->getId(); + for (int i = 0; i < node->getOutdegree(); i++) + { + auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity()); + ARMARX_CHECK_NOT_NULL(adjacent); + auto adjacentId = adjacent->getId(); + + addEdge(nodeId, adjacentId); + } + } +#endif + } + + + void GraphImportExport::addNode(const memoryx::GraphNodeBasePtr& node) + { + (void) node; + + } + + armem::MemoryID GraphImportExport::getLocationsProviderSegmentID() const + { + return armem::MemoryID(properties.memoryName, + properties.locationsCoreSegmentName, + tab.providerSegmentLine.getValue()); + } + + armem::MemoryID GraphImportExport::getGraphProviderSegmentID() const + { + return armem::MemoryID(properties.memoryName, + properties.graphCoreSegmentName, + tab.providerSegmentLine.getValue()); + } + + +#if 0 + void GraphImportExport::addEdge(const std::string& node1Id, const std::string& node2Id) + { + if (!hasNode(node1Id)) + { + ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node1Id << " does not exist."; + return; + } + + if (!hasNode(node2Id)) + { + ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node2Id << " does not exist."; + return; + } + + auto node1dat = nodes.at(node1Id); + auto node2dat = nodes.at(node2Id); + + if (hasEdge(node1Id, node2Id)) + { + //nothing needs to be updated + ARMARX_WARNING << "Edge: '" << node1dat.node->getName() << "' -> '" << node2dat.node->getName() << "' already exists."; + return; + } + + auto edgeId = toEdge(node1Id, node2Id); + + //add + //table + int row = ui.tableWidgetEdges->rowCount(); + ui.tableWidgetEdges->setRowCount(row + 1); + ui.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())}); + ui.tableWidgetEdges->setItem(row, 1, new QTableWidgetItem {QString::fromStdString(node2dat.node->getName())}); + //debug layer will be done later + //scene + QGraphicsLineItem* graphicsItem = dynamic_cast<QGraphicsLineItem*>(new GraphVisualizerGraphicsLineItem + { + *this, edgeId, + node1dat.pose->position->x, -node1dat.pose->position->y, + node2dat.pose->position->x, -node2dat.pose->position->y + }); + //auto graphicsItem= scene->addLine(node1dat.pos->x,-node1dat.pos->y, + // node2dat.pos->x,-node2dat.pos->y); + scene->addItem(graphicsItem); + //setToolTip on graphicsItem does not work + dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Edge:"} +QString::fromStdString(node1dat.node->getName()) + + QString {" <-> "} +QString::fromStdString(node2dat.node->getName())); + //data + EdgeData data {graphicsItem, row, false}; + edges[edgeId] = data; + + updateEdge(edgeId); + } +#endif + + +} diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h new file mode 100644 index 0000000000000000000000000000000000000000..93fd05959fd3d6cb76f6a710bd8377079d0788d0 --- /dev/null +++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h @@ -0,0 +1,157 @@ +/* + * 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 MemoryX::ArmarXObjects::GraphImportExport + * @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 <MemoryX/interface/components/PriorKnowledgeInterface.h> +#include <MemoryX/interface/memorytypes/MemorySegments.h> +#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h> +#include <MemoryX/interface/memorytypes/MemoryEntities.h> + +#include <RobotAPI/libraries/armem/client/ComponentPlugin.h> + +#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h> + +#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h> +#include <ArmarXCore/core/Component.h> + + +namespace armarx::nav +{ + + /** + * @defgroup Component-GraphImportExport GraphImportExport + * @ingroup MemoryX-Components + * A description of the component GraphImportExport. + * + * @class GraphImportExport + * @ingroup Component-GraphImportExport + * @brief Brief description of class GraphImportExport. + * + * Detailed description of class GraphImportExport. + */ + class GraphImportExport : + virtual public armarx::Component + , virtual public armarx::DebugObserverComponentPluginUser + , virtual public armarx::LightweightRemoteGuiComponentPluginUser + , virtual public armarx::armem::client::ComponentPluginUser + { + public: + + /// @see armarx::ManagedIceObject::getDefaultName() + std::string getDefaultName() const override; + + + protected: + + /// @see PropertyUser::createPropertyDefinitions() + armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; + + /// @see armarx::ManagedIceObject::onInitComponent() + void onInitComponent() override; + + /// @see armarx::ManagedIceObject::onConnectComponent() + void onConnectComponent() override; + + /// @see armarx::ManagedIceObject::onDisconnectComponent() + void onDisconnectComponent() override; + + /// @see armarx::ManagedIceObject::onExitComponent() + void onExitComponent() override; + + + /// This function should be called once in onConnect() or when you + /// need to re-create the Remote GUI tab. + void createRemoteGuiTab(const std::vector<std::string>& sceneNames); + + /// After calling `RemoteGui_startRunningTask`, this function is + /// called periodically in a separate thread. If you update variables, + /// make sure to synchronize access to them. + void RemoteGui_update() override; + + + private: + + void refreshScenes(); + + void clearProviderSegment(); + + void exportLocations(const std::string& sceneName); + void importLocations(); + void exportGraph(); + void importGraph(); + + + 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; + + + private: + + struct Proxies + { + memoryx::PriorKnowledgeInterfacePrx priorKnowledge; + memoryx::GraphNodePoseResolverInterfacePrx graphNodePoseResolver; + memoryx::GraphMemorySegmentBasePrx graphSegment; + + armem::client::Writer navigationWriter; + }; + Proxies proxies; + + + /// Properties shown in the Scenario GUI. + struct Properties + { + std::string memoryName = "Navigation"; + std::string locationsCoreSegmentName = "Location"; + std::string graphCoreSegmentName = "Graph"; + }; + Properties properties; + + + /// Tab shown in the Remote GUI. + struct RemoteGuiTab : armarx::RemoteGui::Client::Tab + { + armarx::RemoteGui::Client::ComboBox sceneComboBox; + armarx::RemoteGui::Client::Button sceneRefreshButton; + + armarx::RemoteGui::Client::LineEdit providerSegmentLine; + + armarx::RemoteGui::Client::Button locationsMemoryXToArMemButton; + armarx::RemoteGui::Client::Button locationsArMemToMemoryXButton; + armarx::RemoteGui::Client::Button locationsClearArMemButton; + + armarx::RemoteGui::Client::Button graphMemoryXToArMemButton; + armarx::RemoteGui::Client::Button graphArMemToMemoryXButton; + armarx::RemoteGui::Client::Button graphClearArMemButton; + + armarx::RemoteGui::Client::CheckBox dryRun; + }; + RemoteGuiTab tab; + + }; +} diff --git a/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..1178a8b5b3f0e690748bc8c09f5fd9aeda8fd4a7 --- /dev/null +++ b/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt @@ -0,0 +1,5 @@ + +# Libs required for the tests +SET(LIBS ${LIBS} ArmarXCore GraphImportExport) + +armarx_add_test(GraphImportExportTest GraphImportExportTest.cpp "${LIBS}") diff --git a/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp b/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fa6c535cc4a3ffbba7b52f0feb6a13b3316b3739 --- /dev/null +++ b/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp @@ -0,0 +1,37 @@ +/* + * 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 MemoryX::ArmarXObjects::GraphImportExport + * @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 + */ + +#define BOOST_TEST_MODULE MemoryX::ArmarXObjects::GraphImportExport + +#define ARMARX_BOOST_TEST + +#include <MemoryX/Test.h> +#include "../GraphImportExport.h" + +#include <iostream> + +BOOST_AUTO_TEST_CASE(testExample) +{ + armarx::GraphImportExport instance; + + BOOST_CHECK_EQUAL(true, true); +}