From 75fc6610b30ac898b0e366de59914da96d55520f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 17 Aug 2021 18:12:13 +0200
Subject: [PATCH 01/33] Update to changes in armem
---
.../components/NavigationMemory/NavigationMemory.cpp | 3 +--
.../libraries/memory/client/parameterization/Writer.cpp | 7 +++++--
.../libraries/memory/client/parameterization/Writer.h | 9 +++++----
3 files changed, 11 insertions(+), 8 deletions(-)
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
index e3b6f0b7..7b4bb34f 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -73,8 +73,7 @@ namespace armarx
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
- workingMemory.name() = p.memoryName;
- longtermMemory.name() = p.memoryName;
+ setMemoryName(p.memoryName);
workingMemory.addCoreSegment("Parameterization");
diff --git a/source/Navigation/libraries/memory/client/parameterization/Writer.cpp b/source/Navigation/libraries/memory/client/parameterization/Writer.cpp
index 19053f78..dc6a0348 100644
--- a/source/Navigation/libraries/memory/client/parameterization/Writer.cpp
+++ b/source/Navigation/libraries/memory/client/parameterization/Writer.cpp
@@ -1,12 +1,15 @@
#include "Writer.h"
-#include "Navigation/libraries/core/constants.h"
+#include <Navigation/libraries/core/constants.h>
+
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+
namespace armarx::nav::mem::client::param
{
bool
Writer::store(const std::unordered_map<core::StackLayer,
- aron::datanavigator::DictNavigator::PointerType>& stack,
+ aron::datanavigator::DictNavigatorPtr>& stack,
const std::string& clientID,
const core::TimestampUs& timestamp)
{
diff --git a/source/Navigation/libraries/memory/client/parameterization/Writer.h b/source/Navigation/libraries/memory/client/parameterization/Writer.h
index ca69dab0..b98b7daf 100644
--- a/source/Navigation/libraries/memory/client/parameterization/Writer.h
+++ b/source/Navigation/libraries/memory/client/parameterization/Writer.h
@@ -21,10 +21,11 @@
#pragma once
-#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
-#include "Navigation/libraries/core/constants.h"
-#include "Navigation/libraries/core/types.h"
+#include <Navigation/libraries/core/constants.h>
+#include <Navigation/libraries/core/types.h>
namespace armarx::nav::mem::client::param
@@ -37,7 +38,7 @@ namespace armarx::nav::mem::client::param
bool store(const std::unordered_map<core::StackLayer,
- aron::datanavigator::DictNavigator::PointerType>& stack,
+ aron::datanavigator::DictNavigatorPtr>& stack,
const std::string& clientID,
const core::TimestampUs& timestamp);
--
GitLab
From d5db819c1608c90e1737382181b7b5a1c6da80a4 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 17 Aug 2021 18:12:55 +0200
Subject: [PATCH 02/33] Add libraries locations and graphs
---
source/armarx/navigation/graph/CMakeLists.txt | 30 +++++++++++++
source/armarx/navigation/graph/aron/Graph.xml | 21 +++++++++
source/armarx/navigation/graph/constants.cpp | 11 +++++
source/armarx/navigation/graph/constants.h | 30 +++++++++++++
.../armarx/navigation/location/CMakeLists.txt | 29 ++++++++++++
.../navigation/location/aron/Location.xml | 44 +++++++++++++++++++
.../armarx/navigation/location/constants.cpp | 11 +++++
source/armarx/navigation/location/constants.h | 30 +++++++++++++
8 files changed, 206 insertions(+)
create mode 100644 source/armarx/navigation/graph/CMakeLists.txt
create mode 100644 source/armarx/navigation/graph/aron/Graph.xml
create mode 100644 source/armarx/navigation/graph/constants.cpp
create mode 100644 source/armarx/navigation/graph/constants.h
create mode 100644 source/armarx/navigation/location/CMakeLists.txt
create mode 100644 source/armarx/navigation/location/aron/Location.xml
create mode 100644 source/armarx/navigation/location/constants.cpp
create mode 100644 source/armarx/navigation/location/constants.h
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
new file mode 100644
index 00000000..ae1cf290
--- /dev/null
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -0,0 +1,30 @@
+set(LIB_NAME ${PROJECT_NAME}Graph)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ # ${PROJECT_NAME}Core
+ SOURCES
+ constants.cpp
+ HEADERS
+ constants.h
+)
+
+
+add_library(
+ ${PROJECT_NAME}::Graph
+ ALIAS
+ "${LIB_NAME}"
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Graph.xml
+)
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
new file mode 100644
index 00000000..38d9fe74
--- /dev/null
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ <!--Include include="armarx/navigation/locations/aron/Location.aron.generated.h" /-->
+ </CodeIncludes>
+ <AronIncludes>
+ <!-- <Include include="<armarx/navigation/locations/aron/Location.xml>" /> -->
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <Object name='armarx::nav::graphs::arondto::Graph'>
+
+ <!--ObjectChild key='location'>
+ <armarx::nav::locs::arondto::Location />
+ </ObjectChild-->
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/graph/constants.cpp b/source/armarx/navigation/graph/constants.cpp
new file mode 100644
index 00000000..e1b0ee56
--- /dev/null
+++ b/source/armarx/navigation/graph/constants.cpp
@@ -0,0 +1,11 @@
+#include "aron_conversions.h"
+
+
+namespace armarx::nav
+{
+
+ void core::test()
+ {
+
+ }
+}
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
new file mode 100644
index 00000000..0897a62a
--- /dev/null
+++ b/source/armarx/navigation/graph/constants.h
@@ -0,0 +1,30 @@
+/**
+ * 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/>.
+ *
+ * @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
+
+
+namespace armarx::nav::core
+{
+
+ void test();
+
+}
diff --git a/source/armarx/navigation/location/CMakeLists.txt b/source/armarx/navigation/location/CMakeLists.txt
new file mode 100644
index 00000000..a23eb4a2
--- /dev/null
+++ b/source/armarx/navigation/location/CMakeLists.txt
@@ -0,0 +1,29 @@
+set(LIB_NAME ${PROJECT_NAME}Location)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+
+armarx_add_library(
+ LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ # ${ProjectName}Libraries
+ SOURCES
+ constants.cpp
+ HEADERS
+ constants.h
+)
+
+add_library(
+ ${PROJECT_NAME}::Location
+ ALIAS
+ ${LIB_NAME}
+)
+
+armarx_enable_aron_file_generation_for_target(
+ TARGET_NAME
+ "${LIB_NAME}"
+ ARON_FILES
+ aron/Location.xml
+)
diff --git a/source/armarx/navigation/location/aron/Location.xml b/source/armarx/navigation/location/aron/Location.xml
new file mode 100644
index 00000000..eb3cc696
--- /dev/null
+++ b/source/armarx/navigation/location/aron/Location.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true"/>
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <!--
+ ToDo: Model regions. Ideas:
+ - Polygon (convex, non-convex)
+ -
+ -->
+
+
+ <Object name='armarx::nav::locs::arondto::ObjectRelativeLocation'>
+
+ <ObjectChild key='objectInstanceID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='relativeRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::nav::locs::arondto::Location'>
+
+ <ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='relativeToObject'>
+ <armarx::nav::locs::arondto::ObjectRelativeLocation optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/navigation/location/constants.cpp b/source/armarx/navigation/location/constants.cpp
new file mode 100644
index 00000000..d35921fd
--- /dev/null
+++ b/source/armarx/navigation/location/constants.cpp
@@ -0,0 +1,11 @@
+#include "aron_conversions.h"
+
+
+namespace armarx::nav
+{
+
+ void coree::test()
+ {
+
+ }
+}
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
new file mode 100644
index 00000000..44692727
--- /dev/null
+++ b/source/armarx/navigation/location/constants.h
@@ -0,0 +1,30 @@
+/**
+ * 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/>.
+ *
+ * @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
+
+
+namespace armarx::nav::coree
+{
+
+void test();
+
+}
--
GitLab
From dcfb4bc735373b100030bdd82a5c536b3669d58f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 10:36:08 +0200
Subject: [PATCH 03/33] Add component GraphImportExport
---
.../GraphImportExport/CMakeLists.txt | 84 ++++
.../GraphImportExport/GraphImportExport.cpp | 359 ++++++++++++++++++
.../GraphImportExport/GraphImportExport.h | 157 ++++++++
.../GraphImportExport/test/CMakeLists.txt | 5 +
.../test/GraphImportExportTest.cpp | 37 ++
5 files changed, 642 insertions(+)
create mode 100644 source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
create mode 100644 source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
create mode 100644 source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt
create mode 100644 source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
new file mode 100644
index 00000000..5ed95be3
--- /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 00000000..25fbac03
--- /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 00000000..93fd0595
--- /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 00000000..1178a8b5
--- /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 00000000..fa6c535c
--- /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);
+}
--
GitLab
From 9a0b76ba2a774eff19fabe5f39ec770d5f2bae7f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 11:32:05 +0200
Subject: [PATCH 04/33] Add and use constants for core segment names
---
.../components/NavigationMemory/CMakeLists.txt | 2 ++
.../components/NavigationMemory/NavigationMemory.cpp | 10 ++++++++++
source/armarx/navigation/graph/aron/Graph.xml | 8 ++++----
source/armarx/navigation/graph/constants.cpp | 6 ++----
source/armarx/navigation/graph/constants.h | 6 ++++--
source/armarx/navigation/location/aron/Location.xml | 6 +++---
source/armarx/navigation/location/constants.cpp | 6 ++----
source/armarx/navigation/location/constants.h | 6 ++++--
8 files changed, 31 insertions(+), 19 deletions(-)
diff --git a/source/Navigation/components/NavigationMemory/CMakeLists.txt b/source/Navigation/components/NavigationMemory/CMakeLists.txt
index 19721ffb..ae5f287c 100644
--- a/source/Navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/Navigation/components/NavigationMemory/CMakeLists.txt
@@ -31,6 +31,8 @@ armarx_add_component(
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## NavigationMemoryInterfaces # If you defined a component ice interface above.
+ Navigation::Graph
+ Navigation::Location
SOURCES
NavigationMemory.cpp
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
index 7b4bb34f..c914b239 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -24,6 +24,10 @@
#include <Navigation/libraries/core/aron/Trajectory.aron.generated.h>
#include <Navigation/libraries/core/aron/Twist.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 headers you only need in function definitions in the .cpp.
@@ -88,6 +92,12 @@ namespace armarx
workingMemory.addCoreSegment("Events"); //, armem::example::ExampleData::toAronType());
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::toAronType());
+
+
+ workingMemory.addCoreSegment(nav::loc::coreSegmentName,
+ nav::loc::arondto::Location::toAronType());
+ workingMemory.addCoreSegment(nav::graph::coreSegmentName,
+ nav::graph::arondto::Graph::toAronType());
}
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
index 38d9fe74..69d0f78a 100644
--- a/source/armarx/navigation/graph/aron/Graph.xml
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -1,18 +1,18 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <!--Include include="armarx/navigation/locations/aron/Location.aron.generated.h" /-->
+ <!--Include include="armarx/navigation/location/aron/Location.aron.generated.h" /-->
</CodeIncludes>
<AronIncludes>
- <!-- <Include include="<armarx/navigation/locations/aron/Location.xml>" /> -->
+ <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" /> -->
</AronIncludes>
<GenerateTypes>
- <Object name='armarx::nav::graphs::arondto::Graph'>
+ <Object name='armarx::nav::graph::arondto::Graph'>
<!--ObjectChild key='location'>
- <armarx::nav::locs::arondto::Location />
+ <armarx::nav::loc::arondto::Location />
</ObjectChild-->
</Object>
diff --git a/source/armarx/navigation/graph/constants.cpp b/source/armarx/navigation/graph/constants.cpp
index e1b0ee56..543952ee 100644
--- a/source/armarx/navigation/graph/constants.cpp
+++ b/source/armarx/navigation/graph/constants.cpp
@@ -1,11 +1,9 @@
-#include "aron_conversions.h"
+#include "constants.h"
namespace armarx::nav
{
- void core::test()
- {
+ const std::string graph::coreSegmentName = "Graph";
- }
}
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
index 0897a62a..7e518c7e 100644
--- a/source/armarx/navigation/graph/constants.h
+++ b/source/armarx/navigation/graph/constants.h
@@ -21,10 +21,12 @@
#pragma once
+#include <string>
-namespace armarx::nav::core
+
+namespace armarx::nav::graph
{
- void test();
+ extern const std::string coreSegmentName;
}
diff --git a/source/armarx/navigation/location/aron/Location.xml b/source/armarx/navigation/location/aron/Location.xml
index eb3cc696..4bc0d14f 100644
--- a/source/armarx/navigation/location/aron/Location.xml
+++ b/source/armarx/navigation/location/aron/Location.xml
@@ -15,7 +15,7 @@
-->
- <Object name='armarx::nav::locs::arondto::ObjectRelativeLocation'>
+ <Object name='armarx::nav::loc::arondto::ObjectRelativeLocation'>
<ObjectChild key='objectInstanceID'>
<armarx::armem::arondto::MemoryID />
@@ -28,14 +28,14 @@
</Object>
- <Object name='armarx::nav::locs::arondto::Location'>
+ <Object name='armarx::nav::loc::arondto::Location'>
<ObjectChild key='globalRobotPose'>
<Pose />
</ObjectChild>
<ObjectChild key='relativeToObject'>
- <armarx::nav::locs::arondto::ObjectRelativeLocation optional="true" />
+ <armarx::nav::loc::arondto::ObjectRelativeLocation optional="true" />
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/location/constants.cpp b/source/armarx/navigation/location/constants.cpp
index d35921fd..4a9a4ca4 100644
--- a/source/armarx/navigation/location/constants.cpp
+++ b/source/armarx/navigation/location/constants.cpp
@@ -1,11 +1,9 @@
-#include "aron_conversions.h"
+#include "constants.h"
namespace armarx::nav
{
- void coree::test()
- {
+ const std::string loc::coreSegmentName = "Location";
- }
}
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
index 44692727..31d558d0 100644
--- a/source/armarx/navigation/location/constants.h
+++ b/source/armarx/navigation/location/constants.h
@@ -21,10 +21,12 @@
#pragma once
+#include <string>
-namespace armarx::nav::coree
+
+namespace armarx::nav::loc
{
-void test();
+ extern const std::string coreSegmentName;
}
--
GitLab
From 62945e17099f296eaba5689cc45cea7fed1c9142 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 11:33:14 +0200
Subject: [PATCH 05/33] Implement Locations MemoryX -> ArMem
---
.../GraphImportExport/GraphImportExport.cpp | 63 +++++++++++++------
.../GraphImportExport/GraphImportExport.h | 17 ++---
2 files changed, 53 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 25fbac03..f90f4834 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 93fd0595..06215c7d 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;
--
GitLab
From 031fe9cb19d8e88042d98b24c680ecf23181d910 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 15:24:46 +0200
Subject: [PATCH 06/33] Implement graph export
---
.../GraphImportExport/CMakeLists.txt | 9 +
.../GraphImportExport/GraphImportExport.cpp | 203 +++++++++---------
.../GraphImportExport/GraphImportExport.h | 22 +-
source/armarx/navigation/graph/CMakeLists.txt | 21 +-
source/armarx/navigation/graph/Graph.cpp | 30 +++
source/armarx/navigation/graph/Graph.h | 48 +++++
source/armarx/navigation/graph/aron/Graph.xml | 48 ++++-
7 files changed, 263 insertions(+), 118 deletions(-)
create mode 100644 source/armarx/navigation/graph/Graph.cpp
create mode 100644 source/armarx/navigation/graph/Graph.h
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index 5ed95be3..16d673fe 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -54,6 +54,15 @@ if(MemoryX_FOUND)
endif()
+
+#find_package(SemanticObjectRelations QUIET)
+#armarx_build_if(SemanticObjectRelations_FOUND "SemanticObjectRelations not available")
+#if(SemanticObjectRelations_FOUND)
+# target_link_libraries(${LIB_NAME} PRIVATE SemanticObjectRelations)
+# target_include_directories(${LIB_NAME} PRIVATE SemanticObjectRelations)
+#endif()
+
+
# Add dependencies
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index f90f4834..bf9394bc 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -20,17 +20,22 @@
* GNU General Public License
*/
+#include <VirtualRobot/VirtualRobot.h>
+
#include "GraphImportExport.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 <armarx/navigation/graph/Graph.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -47,28 +52,14 @@ namespace armarx::nav
properties.graphCoreSegmentName = graph::coreSegmentName;
}
+
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;
}
@@ -124,12 +115,12 @@ namespace armarx::nav
tab.dryRun.setValue(false);
- tab.locationsMemoryXToArMemButton.setLabel("Locations MemoryX -> ArMem");
- tab.locationsArMemToMemoryXButton.setLabel("Locations ArMem -> MemoryX (WIP)");
+ 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.graphMemoryxToArmemButton.setLabel("Graph MemoryX -> ArMem");
+ tab.graphArmemToMemoryxButton.setLabel("Graph ArMem -> MemoryX (WIP)");
tab.graphClearArMemButton.setLabel("Clear ArMem Graphs");
@@ -145,11 +136,11 @@ namespace armarx::nav
grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
++row;
- grid.add(tab.locationsMemoryXToArMemButton, {row, 0}).add(tab.locationsArMemToMemoryXButton, {row, 1})
+ 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})
+ grid.add(tab.graphMemoryxToArmemButton, {row, 0}).add(tab.graphArmemToMemoryxButton, {row, 1})
.add(tab.graphClearArMemButton, {row, 2});
++row;
}
@@ -165,16 +156,29 @@ namespace armarx::nav
{
refreshScenes();
}
- if (tab.locationsMemoryXToArMemButton.wasClicked())
+
+ if (tab.locationsMemoryxToArmemButton.wasClicked())
+ {
+ locationsMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.locationsArmemToMemoryxButton.wasClicked())
{
- exportLocations(tab.sceneComboBox.getValue());
+ locationsArmemToMemoryx(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphMemoryxToArmemButton.wasClicked())
+ {
+ graphMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphArmemToMemoryxButton.wasClicked())
+ {
+ graphArmemToMemoryx(tab.sceneComboBox.getValue());
}
- if (tab.locationsMemoryXToArMemButton.wasClicked())
+ if (tab.locationsClearArMemButton.wasClicked())
{
clearArMemProviderSegment(getLocationProviderSegmentID());
}
- if (tab.graphMemoryXToArMemButton.wasClicked())
+ if (tab.graphClearArMemButton.wasClicked())
{
clearArMemProviderSegment(getGraphProviderSegmentID());
}
@@ -203,7 +207,7 @@ namespace armarx::nav
}
- void GraphImportExport::exportLocations(const std::string& sceneName)
+ void GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
{
const armem::Time time = armem::Time::now();
armem::Commit commit;
@@ -252,7 +256,7 @@ namespace armarx::nav
armem::CommitResult result = proxies.navigationWriter.commit(commit);
if (result.allSuccess())
{
- ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations.";
+ ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations from MemoryX to ArMem.";
}
else
{
@@ -266,48 +270,108 @@ namespace armarx::nav
}
- void GraphImportExport::drawScene(const std::string& sceneName)
+ void GraphImportExport::locationsArmemToMemoryx(const std::string& sceneName)
+ {
+ ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
+ (void) sceneName;
+ }
+
+
+ void GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
{
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
+
+ nav::graph::Graph graph;
+ std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
+
+ // Add nodes
+ semrel::ShapeID nextVertexID { 0 };
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())
{
- const std::string nodeId = node->getId();
-
+ // This is the readable name entered in the GUI.
const std::string name = node->getName();
+
FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
ARMARX_CHECK_NOT_NULL(globalNodePose);
+
+ ARMARX_VERBOSE << "\n- Adding node: \t" << name;
+
+ nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
+ vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
+ toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
+ vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
+
+ nextVertexID++;
}
}
-#if 0
// Add edges
for (memoryx::GraphNodeBasePtr& node : graphNodes)
{
- std::string nodeId = node->getId();
+ const auto& sourceVertex = vertexMap.at(node->getName());
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);
+ const auto& targetVertex = vertexMap.at(adjacent->getName());
+
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
+
+ nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
+ edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
+ edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
}
}
-#endif
+
+ // Build ARON Graph
+ nav::graph::arondto::Graph aron;
+ for (auto vertex : graph.vertices())
+ {
+ aron.vertices.push_back(vertex.attrib().aron);
+ }
+ for (auto edge : graph.edges())
+ {
+ aron.edges.push_back(edge.attrib().aron);
+ }
+
+
+ // Build commit
+ const armem::Time time = armem::Time::now();
+ armem::EntityUpdate update;
+ update.entityID = getGraphProviderSegmentID().withEntityName(sceneName);
+ update.timeCreated = time;
+ update.instancesData = { aron.toAron() };
+
+ if (not tab.dryRun.getValue())
+ {
+ armem::EntityUpdateResult result = proxies.navigationWriter.commit(update);
+ if (result.success)
+ {
+ ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName << "' from MemoryX to ArMem.";
+ }
+ else
+ {
+ ARMARX_WARNING << result.errorMessage;
+ }
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Dry Run - skipping commit.";
+ }
}
- void GraphImportExport::addNode(const memoryx::GraphNodeBasePtr& node)
+ void GraphImportExport::graphArmemToMemoryx(const std::string& sceneName)
{
- (void) node;
-
+ ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
+ (void) sceneName;
}
+
armem::MemoryID GraphImportExport::getLocationProviderSegmentID()
{
return armem::MemoryID(properties.memoryName,
@@ -322,61 +386,4 @@ namespace armarx::nav
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
index 06215c7d..f653a43c 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -96,19 +96,13 @@ namespace armarx::nav
void refreshScenes();
- void exportLocations(const std::string& sceneName);
- void importLocations();
- void exportGraph();
- void importGraph();
+ void locationsMemoryxToArmem(const std::string& sceneName);
+ void locationsArmemToMemoryx(const std::string& sceneName);
+ void graphMemoryxToArmem(const std::string& sceneName);
+ void graphArmemToMemoryx(const std::string& sceneName);
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 getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
@@ -144,12 +138,12 @@ namespace armarx::nav
armarx::RemoteGui::Client::LineEdit providerSegmentLine;
- armarx::RemoteGui::Client::Button locationsMemoryXToArMemButton;
- armarx::RemoteGui::Client::Button locationsArMemToMemoryXButton;
+ 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 graphMemoryxToArmemButton;
+ armarx::RemoteGui::Client::Button graphArmemToMemoryxButton;
armarx::RemoteGui::Client::Button graphClearArMemButton;
armarx::RemoteGui::Client::CheckBox dryRun;
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index ae1cf290..aec5e4a1 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -6,13 +6,20 @@ armarx_set_target("Library: ${LIB_NAME}")
armarx_add_library(
LIBS
+ # ArmarXCore
ArmarXCoreInterfaces
ArmarXCore
- # ${PROJECT_NAME}Core
+
+ # RobotAPI
+ aron
+
SOURCES
constants.cpp
+ Graph.cpp
+
HEADERS
constants.h
+ Graph.h
)
@@ -22,9 +29,21 @@ add_library(
"${LIB_NAME}"
)
+
armarx_enable_aron_file_generation_for_target(
TARGET_NAME
"${LIB_NAME}"
ARON_FILES
aron/Graph.xml
)
+
+
+find_package(VTK QUIET)
+armarx_build_if(VTK_FOUND "VTK not available")
+
+find_package(SemanticObjectRelations QUIET)
+armarx_build_if(SemanticObjectRelations_FOUND "SemanticObjectRelations not available")
+if(SemanticObjectRelations_FOUND)
+ target_link_libraries(${LIB_NAME} PUBLIC SemanticObjectRelations)
+ # target_include_directories(${LIB_NAME} PUBLIC SemanticObjectRelations)
+endif()
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
new file mode 100644
index 00000000..2f4c4e49
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -0,0 +1,30 @@
+/*
+ * 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 "Graph.h"
+
+
+namespace armarx::nav::graph
+{
+
+
+}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
new file mode 100644
index 00000000..e3d61432
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.h
@@ -0,0 +1,48 @@
+/*
+ * 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 <armarx/navigation/graph/aron/Graph.aron.generated.h>
+
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+
+
+namespace armarx::nav::graph
+{
+
+ struct VertexAttribs : public semrel::ShapeVertex
+ {
+ armarx::nav::graph::arondto::Vertex aron;
+ };
+ struct EdgeAttribs
+ {
+ armarx::nav::graph::arondto::Edge aron;
+ };
+ struct GraphAttribs
+ {
+ };
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+
+}
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
index 69d0f78a..18e1c847 100644
--- a/source/armarx/navigation/graph/aron/Graph.xml
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -1,19 +1,57 @@
<?xml version="1.0" encoding="UTF-8" ?>
<AronTypeDefinition>
<CodeIncludes>
- <!--Include include="armarx/navigation/location/aron/Location.aron.generated.h" /-->
</CodeIncludes>
<AronIncludes>
- <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" /> -->
+ <!-- <Include include="<armarx/navigation/location/aron/Location.xml>" autoinclude="true" /> -->
+ <Include include="<RobotAPI/libraries/armem/aron/MemoryID.xml>" autoinclude="true" />
</AronIncludes>
<GenerateTypes>
+ <Object name='armarx::nav::graph::arondto::Vertex'>
+
+ <ObjectChild key='vertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='locationID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::nav::graph::arondto::Edge'>
+
+ <ObjectChild key='sourceVertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='targetVertexID'>
+ <Long />
+ </ObjectChild>
+
+ </Object>
+
+
<Object name='armarx::nav::graph::arondto::Graph'>
- <!--ObjectChild key='location'>
- <armarx::nav::loc::arondto::Location />
- </ObjectChild-->
+ <ObjectChild key='vertices'>
+ <List>
+ <armarx::nav::graph::arondto::Vertex />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='edges'>
+ <List>
+ <armarx::nav::graph::arondto::Edge />
+ </List>
+ </ObjectChild>
</Object>
--
GitLab
From d7905a239d2b04055156d1eb8aad4984b98a4322 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 18 Aug 2021 15:51:12 +0200
Subject: [PATCH 07/33] Move code to library
---
.../GraphImportExport/GraphImportExport.cpp | 108 +++++++++---------
.../GraphImportExport/GraphImportExport.h | 4 +
source/armarx/navigation/graph/CMakeLists.txt | 1 +
source/armarx/navigation/graph/Graph.cpp | 34 ++++++
source/armarx/navigation/graph/Graph.h | 3 +
5 files changed, 95 insertions(+), 55 deletions(-)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index bf9394bc..3804695b 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -280,64 +280,11 @@ namespace armarx::nav
void GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
{
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
-
- nav::graph::Graph graph;
- std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
-
- // Add nodes
- semrel::ShapeID nextVertexID { 0 };
- for (memoryx::GraphNodeBasePtr& node : graphNodes)
- {
- ARMARX_CHECK_NOT_NULL(node);
- if (not node->isMetaEntity())
- {
- // This is the readable name entered in the GUI.
- const std::string name = node->getName();
-
- FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
- ARMARX_CHECK_NOT_NULL(globalNodePose);
-
- ARMARX_VERBOSE << "\n- Adding node: \t" << name;
-
- nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
- vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
- toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
- vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
-
- nextVertexID++;
- }
- }
-
- // Add edges
- for (memoryx::GraphNodeBasePtr& node : graphNodes)
- {
- const auto& sourceVertex = vertexMap.at(node->getName());
- for (int i = 0; i < node->getOutdegree(); i++)
- {
- auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
- ARMARX_CHECK_NOT_NULL(adjacent);
-
- const auto& targetVertex = vertexMap.at(adjacent->getName());
-
- ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
-
- nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
- edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
- edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
- }
- }
+ nav::graph::Graph graph = toArmemGraph(graphNodes);
// Build ARON Graph
nav::graph::arondto::Graph aron;
- for (auto vertex : graph.vertices())
- {
- aron.vertices.push_back(vertex.attrib().aron);
- }
- for (auto edge : graph.edges())
- {
- aron.edges.push_back(edge.attrib().aron);
- }
-
+ toAron(aron, graph);
// Build commit
const armem::Time time = armem::Time::now();
@@ -386,4 +333,55 @@ namespace armarx::nav
tab.providerSegmentLine.getValue());
}
+ nav::graph::Graph GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
+ {
+ nav::graph::Graph graph;
+ std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
+
+ // Add nodes
+ semrel::ShapeID nextVertexID { 0 };
+ for (memoryx::GraphNodeBasePtr node : graphNodes)
+ {
+ ARMARX_CHECK_NOT_NULL(node);
+ if (not node->isMetaEntity())
+ {
+ // This is the readable name entered in the GUI.
+ const std::string name = node->getName();
+
+ FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
+ ARMARX_CHECK_NOT_NULL(globalNodePose);
+
+ ARMARX_VERBOSE << "\n- Adding node: \t" << name;
+
+ nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
+ vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
+ toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
+ vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
+
+ nextVertexID++;
+ }
+ }
+
+ // Add edges
+ for (memoryx::GraphNodeBasePtr node : graphNodes)
+ {
+ const auto& sourceVertex = vertexMap.at(node->getName());
+ for (int i = 0; i < node->getOutdegree(); i++)
+ {
+ auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
+ ARMARX_CHECK_NOT_NULL(adjacent);
+
+ const auto& targetVertex = vertexMap.at(adjacent->getName());
+
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
+
+ nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
+ edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
+ edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
+ }
+ }
+
+ return graph;
+ }
+
}
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index f653a43c..90878834 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -22,6 +22,8 @@
#pragma once
+#include <armarx/navigation/graph/forward_declarations.h>
+
#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
@@ -106,6 +108,8 @@ namespace armarx::nav
armem::MemoryID getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
+ nav::graph::Graph toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes);
+
private:
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index aec5e4a1..064597f7 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -19,6 +19,7 @@ armarx_add_library(
HEADERS
constants.h
+ forward_declarations.h
Graph.h
)
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index 2f4c4e49..d8350099 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -26,5 +26,39 @@
namespace armarx::nav::graph
{
+}
+
+
+namespace armarx::nav
+{
+
+ void graph::toAron(arondto::Graph& dto, const Graph& bo)
+ {
+ dto = {};
+ for (auto vertex : bo.vertices())
+ {
+ dto.vertices.push_back(vertex.attrib().aron);
+ }
+ for (auto edge : bo.edges())
+ {
+ dto.edges.push_back(edge.attrib().aron);
+ }
+ }
+
+
+ void graph::fromAron(const arondto::Graph& dto, Graph& bo)
+ {
+ bo = {};
+ for (const arondto::Vertex& vertex : dto.vertices)
+ {
+ auto v = bo.addVertex(semrel::ShapeID(vertex.vertexID));
+ v.attrib().aron = vertex;
+ }
+ for (const arondto::Edge& edge : dto.edges)
+ {
+ auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID), semrel::ShapeID(edge.targetVertexID));
+ e.attrib().aron = edge;
+ }
+ }
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index e3d61432..10ec0e29 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -45,4 +45,7 @@ namespace armarx::nav::graph
using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+ void toAron(arondto::Graph& dto, const Graph& bo);
+ void fromAron(const arondto::Graph& dto, Graph& bo);
+
}
--
GitLab
From 80d0e49a33a273ec6eb83772e0f59861929e184f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 19 Aug 2021 08:28:27 +0200
Subject: [PATCH 08/33] Implement graph visu
---
.../GraphImportExport/CMakeLists.txt | 2 +-
.../GraphImportExport/GraphImportExport.cpp | 23 +++-
.../GraphImportExport/GraphImportExport.h | 3 +
source/armarx/navigation/graph/CMakeLists.txt | 2 +
source/armarx/navigation/graph/Visu.cpp | 108 ++++++++++++++++++
source/armarx/navigation/graph/Visu.h | 92 +++++++++++++++
.../navigation/graph/forward_declarations.h | 39 +++++++
7 files changed, 265 insertions(+), 4 deletions(-)
create mode 100644 source/armarx/navigation/graph/Visu.cpp
create mode 100644 source/armarx/navigation/graph/Visu.h
create mode 100644 source/armarx/navigation/graph/forward_declarations.h
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index 16d673fe..e5d7c37d 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -23,7 +23,7 @@ armarx_add_component(
# RobotAPI
## RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
armem
# MemoryX
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 3804695b..14b6704b 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -26,9 +26,12 @@
#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 <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/graph/Visu.h>
+
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
@@ -114,6 +117,7 @@ namespace armarx::nav
tab.providerSegmentLine.setValue(getName());
tab.dryRun.setValue(false);
+ tab.visuEnabled.setValue(true);
tab.locationsMemoryxToArmemButton.setLabel("Locations MemoryX -> ArMem");
tab.locationsArmemToMemoryxButton.setLabel("Locations ArMem -> MemoryX (WIP)");
@@ -136,6 +140,9 @@ namespace armarx::nav
grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
++row;
+ grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
+ ++row;
+
grid.add(tab.locationsMemoryxToArmemButton, {row, 0}).add(tab.locationsArmemToMemoryxButton, {row, 1})
.add(tab.locationsClearArMemButton, {row, 2});
++row;
@@ -286,6 +293,18 @@ namespace armarx::nav
nav::graph::arondto::Graph aron;
toAron(aron, graph);
+
+ if (tab.visuEnabled.getValue())
+ {
+ nav::graph::GraphVisu visu;
+ viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
+ visu.draw(layer, graph);
+
+ ARMARX_VERBOSE << "Visualize graph '" << sceneName << "' ...";
+ arviz.commit(layer);
+ }
+
+
// Build commit
const armem::Time time = armem::Time::now();
armem::EntityUpdate update;
@@ -370,11 +389,9 @@ namespace armarx::nav
{
auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
ARMARX_CHECK_NOT_NULL(adjacent);
-
const auto& targetVertex = vertexMap.at(adjacent->getName());
- ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
-
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> " << adjacent->getName();
nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index 90878834..efd4aed1 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -30,6 +30,7 @@
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -55,6 +56,7 @@ namespace armarx::nav
virtual public armarx::Component
, virtual public armarx::DebugObserverComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
, virtual public armarx::armem::client::ComponentPluginUser
{
public:
@@ -151,6 +153,7 @@ namespace armarx::nav
armarx::RemoteGui::Client::Button graphClearArMemButton;
armarx::RemoteGui::Client::CheckBox dryRun;
+ armarx::RemoteGui::Client::CheckBox visuEnabled;
};
RemoteGuiTab tab;
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index 064597f7..530fe93e 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -16,11 +16,13 @@ armarx_add_library(
SOURCES
constants.cpp
Graph.cpp
+ Visu.cpp
HEADERS
constants.h
forward_declarations.h
Graph.h
+ Visu.h
)
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
new file mode 100644
index 00000000..62ede64f
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -0,0 +1,108 @@
+/*
+ * 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 <VirtualRobot/VirtualRobot.h>
+
+#include "Visu.h"
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+
+
+namespace armarx::nav::graph
+{
+
+ viz::Pose VertexVisu::Pose::draw(Graph::ConstVertex vertex) const
+ {
+ const arondto::Vertex& aron = vertex.attrib().aron;
+ return viz::Pose(aron.locationID.entityName)
+ .pose(aron.globalRobotPose)
+ .scale(scale);
+ }
+
+
+ viz::Arrow VertexVisu::ForwardArrow::draw(Graph::ConstVertex vertex) const
+ {
+ const arondto::Vertex& aron = vertex.attrib().aron;
+ return viz::Arrow(aron.locationID.entityName + " forward")
+ .fromTo(simox::math::position(aron.globalRobotPose),
+ simox::math::transform_position(aron.globalRobotPose, length * Eigen::Vector3f::UnitY()))
+ .color(color)
+ .width(width);
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, Graph::ConstVertex vertex) const
+ {
+ if (pose.has_value())
+ {
+ layer.add(pose->draw(vertex));
+ }
+ if (forwardArrow.has_value())
+ {
+ layer.add(forwardArrow->draw(vertex));
+ }
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(Graph::ConstEdge edge) const
+ {
+ const auto& sourceAron = edge.source().attrib().aron;
+ const auto& targetAron = edge.target().attrib().aron;
+ return viz::Arrow(sourceAron.locationID.entityName + " -> " +
+ targetAron.locationID.entityName)
+ .fromTo(simox::math::position(sourceAron.globalRobotPose),
+ simox::math::position(targetAron.globalRobotPose))
+ .width(width)
+ .color(color);
+ }
+
+
+ void EdgeVisu::draw(viz::Layer& layer, Graph::ConstEdge edge) const
+ {
+ if (arrow.has_value())
+ {
+ layer.add(arrow->draw(edge));
+ }
+ }
+
+
+ void GraphVisu::draw(viz::Layer& layer, const Graph& graph)
+ {
+ if (vertex.has_value())
+ {
+ for (Graph::ConstVertex v : graph.vertices())
+ {
+ vertex->draw(layer, v);
+ }
+ }
+ if (edge.has_value())
+ {
+ for (Graph::ConstEdge e : graph.edges())
+ {
+ edge->draw(layer, e);
+ }
+ }
+ }
+}
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
new file mode 100644
index 00000000..6e2516bc
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.h
@@ -0,0 +1,92 @@
+/*
+ * 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 <armarx/navigation/graph/Graph.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include <optional>
+
+
+namespace armarx::viz
+{
+ class Arrow;
+ class Layer;
+ class Pose;
+}
+
+namespace armarx::nav::graph
+{
+
+ struct VertexVisu
+ {
+ struct Pose
+ {
+ float scale = 1.0;
+
+ viz::Pose draw(Graph::ConstVertex vertex) const;
+ };
+ std::optional<Pose> pose = Pose {};
+
+ struct ForwardArrow
+ {
+ float width = 7.5;
+ float length = 100.0;
+ simox::Color color = simox::Color::cyan(220);
+
+ viz::Arrow draw(Graph::ConstVertex vertex) const;
+ };
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+
+
+ void draw(viz::Layer& layer, Graph::ConstVertex vertex) const;
+ };
+
+
+ struct EdgeVisu
+ {
+ struct Arrow
+ {
+ float width = 5.0;
+ simox::Color color = simox::Color::azure(196);
+
+ viz::Arrow draw(Graph::ConstEdge edge) const;
+ };
+ std::optional<Arrow> arrow = Arrow {};
+
+
+ void draw(viz::Layer& layer, Graph::ConstEdge edge) const;
+ };
+
+
+ struct GraphVisu
+ {
+ std::optional<VertexVisu> vertex = VertexVisu {};
+ std::optional<EdgeVisu> edge = EdgeVisu {};
+
+
+ void draw(viz::Layer& layer, const Graph& graph);
+ };
+
+}
diff --git a/source/armarx/navigation/graph/forward_declarations.h b/source/armarx/navigation/graph/forward_declarations.h
new file mode 100644
index 00000000..fad17f2b
--- /dev/null
+++ b/source/armarx/navigation/graph/forward_declarations.h
@@ -0,0 +1,39 @@
+/**
+ * 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/>.
+ *
+ * @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
+
+
+namespace semrel
+{
+ template <class VertexAttribT, class EdgeAttribT, class GraphAttribT>
+ class RelationGraph;
+}
+namespace armarx::nav::graph
+{
+
+ struct VertexAttribs;
+ struct EdgeAttribs;
+ struct GraphAttribs;
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+}
--
GitLab
From 04bce89fb74fb29c6e00f0fc49589cbd4b85dd00 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 19 Aug 2021 11:35:44 +0200
Subject: [PATCH 09/33] Use MemoryIDs instead of strings
---
.../GraphImportExport/GraphImportExport.cpp | 66 +++++++++----------
.../GraphImportExport/GraphImportExport.h | 19 +++---
source/armarx/navigation/graph/CMakeLists.txt | 1 +
source/armarx/navigation/graph/constants.cpp | 4 +-
source/armarx/navigation/graph/constants.h | 4 +-
.../armarx/navigation/location/CMakeLists.txt | 3 +
.../armarx/navigation/location/constants.cpp | 4 +-
source/armarx/navigation/location/constants.h | 4 +-
8 files changed, 56 insertions(+), 49 deletions(-)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 14b6704b..96a69c23 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -26,6 +26,7 @@
#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/graph/Writer.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
@@ -50,9 +51,8 @@ namespace armarx::nav
GraphImportExport::GraphImportExport()
{
- properties.memoryName = "Navigation";
- properties.locationCoreSegmentName = loc::coreSegmentName;
- properties.graphCoreSegmentName = graph::coreSegmentName;
+ properties.locationCoreSegmentID = loc::coreSegmentID;
+ properties.graphCoreSegmentID = graph::coreSegmentID;
}
@@ -83,7 +83,9 @@ namespace armarx::nav
{
proxies.graphSegment = proxies.priorKnowledge->getGraphSegment();
}
- proxies.navigationWriter = memoryNameSystem.useWriter(armem::MemoryID(properties.memoryName));
+ proxies.locationWriter = memoryNameSystem.useWriter(properties.locationCoreSegmentID);
+ proxies.graphWriter = memoryNameSystem.useWriter(properties.graphCoreSegmentID);
+
// Setup and start the remote GUI.
refreshScenes();
@@ -183,11 +185,11 @@ namespace armarx::nav
if (tab.locationsClearArMemButton.wasClicked())
{
- clearArMemProviderSegment(getLocationProviderSegmentID());
+ clearArMemProviderSegment(proxies.locationWriter, getLocationProviderSegmentID());
}
if (tab.graphClearArMemButton.wasClicked())
{
- clearArMemProviderSegment(getGraphProviderSegmentID());
+ clearArMemProviderSegment(proxies.graphWriter, getGraphProviderSegmentID());
}
}
@@ -199,21 +201,6 @@ namespace armarx::nav
}
- void GraphImportExport::clearArMemProviderSegment(const armem::MemoryID& providerSegmentID)
- {
- 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;
- }
- }
-
-
void GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
{
const armem::Time time = armem::Time::now();
@@ -260,7 +247,7 @@ namespace armarx::nav
if (not tab.dryRun.getValue())
{
- armem::CommitResult result = proxies.navigationWriter.commit(commit);
+ armem::CommitResult result = proxies.locationWriter.commit(commit);
if (result.allSuccess())
{
ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations from MemoryX to ArMem.";
@@ -289,11 +276,6 @@ namespace armarx::nav
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
nav::graph::Graph graph = toArmemGraph(graphNodes);
- // Build ARON Graph
- nav::graph::arondto::Graph aron;
- toAron(aron, graph);
-
-
if (tab.visuEnabled.getValue())
{
nav::graph::GraphVisu visu;
@@ -304,6 +286,9 @@ namespace armarx::nav
arviz.commit(layer);
}
+ // Build ARON Graph
+ nav::graph::arondto::Graph aron;
+ toAron(aron, graph);
// Build commit
const armem::Time time = armem::Time::now();
@@ -314,7 +299,7 @@ namespace armarx::nav
if (not tab.dryRun.getValue())
{
- armem::EntityUpdateResult result = proxies.navigationWriter.commit(update);
+ armem::EntityUpdateResult result = proxies.graphWriter.commit(update);
if (result.success)
{
ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName << "' from MemoryX to ArMem.";
@@ -338,20 +323,33 @@ namespace armarx::nav
}
+ void GraphImportExport::clearArMemProviderSegment(armem::client::Writer& writer, const armem::MemoryID& providerSegmentID)
+ {
+ const bool clearWhenExists = true;
+ auto result = writer.addSegment(providerSegmentID, clearWhenExists);
+ if (result.success)
+ {
+ ARMARX_IMPORTANT << "Cleared ArMem provider segment " << providerSegmentID << ".";
+ }
+ else
+ {
+ ARMARX_WARNING << result.errorMessage;
+ }
+ }
+
+
armem::MemoryID GraphImportExport::getLocationProviderSegmentID()
{
- return armem::MemoryID(properties.memoryName,
- properties.locationCoreSegmentName,
- tab.providerSegmentLine.getValue());
+ return properties.locationCoreSegmentID.withProviderSegmentName(tab.providerSegmentLine.getValue());
}
+
armem::MemoryID GraphImportExport::getGraphProviderSegmentID()
{
- return armem::MemoryID(properties.memoryName,
- properties.graphCoreSegmentName,
- tab.providerSegmentLine.getValue());
+ return properties.graphCoreSegmentID.withProviderSegmentName(tab.providerSegmentLine.getValue());
}
+
nav::graph::Graph GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
{
nav::graph::Graph graph;
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index efd4aed1..949ad673 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -24,12 +24,13 @@
#include <armarx/navigation/graph/forward_declarations.h>
-#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
+#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
+#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
-#include <MemoryX/interface/memorytypes/MemoryEntities.h>
-#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -57,7 +58,7 @@ namespace armarx::nav
, virtual public armarx::DebugObserverComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
- , virtual public armarx::armem::client::ComponentPluginUser
+ , virtual public armarx::armem::client::MemoryNameSystemComponentPluginUser
{
public:
@@ -105,7 +106,7 @@ namespace armarx::nav
void graphMemoryxToArmem(const std::string& sceneName);
void graphArmemToMemoryx(const std::string& sceneName);
- void clearArMemProviderSegment(const armem::MemoryID& providerSegmentID);
+ void clearArMemProviderSegment(armem::client::Writer& writer, const armem::MemoryID& providerSegmentID);
armem::MemoryID getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
@@ -121,7 +122,8 @@ namespace armarx::nav
memoryx::GraphNodePoseResolverInterfacePrx graphNodePoseResolver;
memoryx::GraphMemorySegmentBasePrx graphSegment;
- armem::client::Writer navigationWriter;
+ armem::client::Writer locationWriter;
+ armem::client::Writer graphWriter;
};
Proxies proxies;
@@ -129,9 +131,8 @@ namespace armarx::nav
/// Properties shown in the Scenario GUI.
struct Properties
{
- std::string memoryName;
- std::string locationCoreSegmentName;
- std::string graphCoreSegmentName;
+ armem::MemoryID locationCoreSegmentID;
+ armem::MemoryID graphCoreSegmentID;
};
Properties properties;
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index 530fe93e..a53eb52b 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -12,6 +12,7 @@ armarx_add_library(
# RobotAPI
aron
+ armem
SOURCES
constants.cpp
diff --git a/source/armarx/navigation/graph/constants.cpp b/source/armarx/navigation/graph/constants.cpp
index 543952ee..f862be9c 100644
--- a/source/armarx/navigation/graph/constants.cpp
+++ b/source/armarx/navigation/graph/constants.cpp
@@ -1,9 +1,11 @@
#include "constants.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
namespace armarx::nav
{
- const std::string graph::coreSegmentName = "Graph";
+ const armem::MemoryID graph::coreSegmentID { "Navigation", "Graph" };
}
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
index 7e518c7e..ccb1cc7e 100644
--- a/source/armarx/navigation/graph/constants.h
+++ b/source/armarx/navigation/graph/constants.h
@@ -21,12 +21,12 @@
#pragma once
-#include <string>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
namespace armarx::nav::graph
{
- extern const std::string coreSegmentName;
+ extern const armem::MemoryID coreSegmentID;
}
diff --git a/source/armarx/navigation/location/CMakeLists.txt b/source/armarx/navigation/location/CMakeLists.txt
index a23eb4a2..675377bd 100644
--- a/source/armarx/navigation/location/CMakeLists.txt
+++ b/source/armarx/navigation/location/CMakeLists.txt
@@ -9,6 +9,9 @@ armarx_add_library(
ArmarXCoreInterfaces
ArmarXCore
# ${ProjectName}Libraries
+
+ armem
+
SOURCES
constants.cpp
HEADERS
diff --git a/source/armarx/navigation/location/constants.cpp b/source/armarx/navigation/location/constants.cpp
index 4a9a4ca4..33b2bc98 100644
--- a/source/armarx/navigation/location/constants.cpp
+++ b/source/armarx/navigation/location/constants.cpp
@@ -1,9 +1,11 @@
#include "constants.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
namespace armarx::nav
{
- const std::string loc::coreSegmentName = "Location";
+ const armem::MemoryID loc::coreSegmentID { "Navigation", "Location" };
}
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
index 31d558d0..ac5f34ef 100644
--- a/source/armarx/navigation/location/constants.h
+++ b/source/armarx/navigation/location/constants.h
@@ -21,12 +21,12 @@
#pragma once
-#include <string>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
namespace armarx::nav::loc
{
- extern const std::string coreSegmentName;
+ extern const armem::MemoryID coreSegmentID;
}
--
GitLab
From d18a7fcb8040ca165b9645a4a2fe01570280eb6c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 19 Aug 2021 13:54:46 +0200
Subject: [PATCH 10/33] Update usage
---
.../components/NavigationMemory/NavigationMemory.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
index c914b239..73f3fcfd 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -94,9 +94,9 @@ namespace armarx
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::toAronType());
- workingMemory.addCoreSegment(nav::loc::coreSegmentName,
+ workingMemory.addCoreSegment(nav::loc::coreSegmentID.coreSegmentName,
nav::loc::arondto::Location::toAronType());
- workingMemory.addCoreSegment(nav::graph::coreSegmentName,
+ workingMemory.addCoreSegment(nav::graph::coreSegmentID.coreSegmentName,
nav::graph::arondto::Graph::toAronType());
}
--
GitLab
From 3e8ca7ede7c17ff61024fdf1bc3d2e0bf0a57ea0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 19 Aug 2021 13:55:59 +0200
Subject: [PATCH 11/33] Add gui plugin LocationGraphEditor (copy from MemoryX
Graph Visualizer)
---
.../navigation/gui-plugins/CMakeLists.txt | 2 +
.../LocationGraphEditor/CMakeLists.txt | 57 +
.../LocationGraphEditorWidget.ui | 572 ++++++++
.../LocationGraphEditorWidgetController.cpp | 1162 +++++++++++++++++
.../LocationGraphEditorWidgetController.h | 487 +++++++
5 files changed, 2280 insertions(+)
create mode 100644 source/armarx/navigation/gui-plugins/CMakeLists.txt
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
diff --git a/source/armarx/navigation/gui-plugins/CMakeLists.txt b/source/armarx/navigation/gui-plugins/CMakeLists.txt
new file mode 100644
index 00000000..0b821f03
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/CMakeLists.txt
@@ -0,0 +1,2 @@
+
+add_subdirectory(LocationGraphEditor)
\ No newline at end of file
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
new file mode 100644
index 00000000..1c9c13a4
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -0,0 +1,57 @@
+set(LIB_NAME "LocationGraphEditorGuiPlugin")
+armarx_set_target("${LIB_NAME}")
+
+# most qt components will be linked against in the call armarx_gui_library
+#armarx_find_qt(QtCore QtGui QtDesigner)
+
+# ArmarXGui gets included through depends_on_armarx_package(ArmarXGui "OPTIONAL")
+# in the toplevel CMakeLists.txt
+armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
+
+
+# do not rename this variable, it is used in armarx_gui_library()...
+set(SOURCES
+ LocationGraphEditorWidgetController.cpp
+)
+set(HEADERS
+ LocationGraphEditorWidgetController.h
+)
+set(GUI_UIS
+ LocationGraphEditorWidget.ui
+)
+
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS
+ # ArmarXGui
+ SimpleConfigDialog
+
+ # RobotAPI
+ armem
+
+ # MemoryX
+ MemoryXCore
+ MemoryXMemoryTypes
+)
+
+
+if(ArmarXGui_FOUND)
+ armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+
+
+ # ToDo: Remove
+ 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()
+
+
+ #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(LocationGraphEditorGuiPlugi PUBLIC ${MyLib_INCLUDE_DIRS})
+ #endif()
+endif()
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
new file mode 100644
index 00000000..4e2a1782
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -0,0 +1,572 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>LocationGraphEditorWidget</class>
+ <widget class="QWidget" name="LocationGraphEditorWidget">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>783</width>
+ <height>664</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>LocationGraphEditorWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <widget class="QGroupBox" name="sceneGroupBox">
+ <property name="title">
+ <string>Navigation Graphs from Navigation Graph Segment</string>
+ </property>
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <item>
+ <widget class="QPushButton" name="refreshScenesButton">
+ <property name="toolTip">
+ <string>Reloads the list of scenes from the memory</string>
+ </property>
+ <property name="text">
+ <string>Refresh List</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QComboBox" name="scenesComboBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="drawSceneButton">
+ <property name="toolTip">
+ <string>Draws the selected scene</string>
+ </property>
+ <property name="text">
+ <string>Load</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSplitter" name="splitter_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <widget class="QFrame" name="graphFrame">
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QWidget" name="graphHead" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QLabel" name="label_graph">
+ <property name="text">
+ <string>Graph</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="buttonRedraw">
+ <property name="text">
+ <string>Repaint</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="buttonClear">
+ <property name="text">
+ <string>Clear graph</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonRotateClock">
+ <property name="text">
+ <string>↻</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonRotateCounterClock">
+ <property name="text">
+ <string>↺</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="Line" name="line_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_zoom">
+ <property name="text">
+ <string>Zoom</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QDoubleSpinBox" name="viewZoomFactor">
+ <property name="decimals">
+ <number>5</number>
+ </property>
+ <property name="singleStep">
+ <double>0.001000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.100000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QToolButton" name="buttonAutoAdjust">
+ <property name="text">
+ <string>Auto</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </item>
+ <item>
+ <widget class="QGraphicsView" name="graphicsViewGraph"/>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tables" native="true">
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QSplitter" name="splitter_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QSplitter" name="splitter">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QWidget" name="layoutWidget">
+ <layout class="QVBoxLayout" name="verticalLayout_nodes">
+ <item>
+ <widget class="QLabel" name="label_nodes">
+ <property name="text">
+ <string>Nodes</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QTableWidget" name="tableWidgetNodes">
+ <property name="editTriggers">
+ <set>QAbstractItemView::NoEditTriggers</set>
+ </property>
+ <column>
+ <property name="text">
+ <string>Name</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>X</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Yaw Angle</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="layoutWidget2">
+ <layout class="QVBoxLayout" name="verticalLayout_edges">
+ <item>
+ <widget class="QLabel" name="label_edges">
+ <property name="text">
+ <string>Edges</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QTableWidget" name="tableWidgetEdges">
+ <property name="editTriggers">
+ <set>QAbstractItemView::NoEditTriggers</set>
+ </property>
+ <column>
+ <property name="text">
+ <string>Node 1</string>
+ </property>
+ </column>
+ <column>
+ <property name="text">
+ <string>Node 2</string>
+ </property>
+ </column>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ <widget class="QTabWidget" name="tabWidget">
+ <property name="currentIndex">
+ <number>0</number>
+ </property>
+ <widget class="QWidget" name="tabWidgetPage1">
+ <attribute name="title">
+ <string>Add Node</string>
+ </attribute>
+ <layout class="QFormLayout" name="formLayout">
+ <property name="fieldGrowthPolicy">
+ <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+ </property>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_12">
+ <property name="text">
+ <string>NodeId</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="editNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Name</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="editNodeName"/>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>Frame</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="editFrameName"/>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="text">
+ <string>Agent</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="editAgentName"/>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="text">
+ <string>X</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxX">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Y</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxY">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Z</string>
+ </property>
+ </widget>
+ </item>
+ <item row="9" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxZ">
+ <property name="decimals">
+ <number>0</number>
+ </property>
+ <property name="minimum">
+ <double>-1000000.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>1000000.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="text">
+ <string>R in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxRoll">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>P in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="11" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxPitch">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>Y in deg</string>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="1">
+ <widget class="QDoubleSpinBox" name="spinBoxYaw">
+ <property name="minimum">
+ <double>-360.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>360.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="0">
+ <widget class="QPushButton" name="btnAdd">
+ <property name="text">
+ <string>Add</string>
+ </property>
+ </widget>
+ </item>
+ <item row="13" column="1">
+ <widget class="QPushButton" name="btnEdit">
+ <property name="enabled">
+ <bool>true</bool>
+ </property>
+ <property name="text">
+ <string>Edit</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="editSceneName"/>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_13">
+ <property name="text">
+ <string>Scene</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab">
+ <attribute name="title">
+ <string>Add Edge</string>
+ </attribute>
+ <layout class="QFormLayout" name="gridLayout">
+ <property name="fieldGrowthPolicy">
+ <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+ </property>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="text">
+ <string>Start Node Id</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLineEdit" name="editStartNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="labelStartNode">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Start Node</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="editStartNodeName">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="text">
+ <string>End Node Id</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="editEndNodeId">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="labelEndNode">
+ <property name="text">
+ <string>End Node</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="editEndNodeName">
+ <property name="enabled">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdge">
+ <property name="text">
+ <string>Add Double-sided Edge</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <spacer name="verticalSpacer">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="5" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdgeStartEnd">
+ <property name="text">
+ <string>Add Edge Start -> End</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0" colspan="2">
+ <widget class="QPushButton" name="btnAddEdgeEndStart">
+ <property name="text">
+ <string>Add Edge End -> Start</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0" colspan="2">
+ <widget class="QLabel" name="labelAddEdgeStatus">
+ <property name="text">
+ <string/>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
new file mode 100644
index 00000000..cb576778
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -0,0 +1,1162 @@
+/*
+ * 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 Navigation::gui-plugins::LocationGraphEditorWidgetController
+ * \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 "LocationGraphEditorWidgetController.h"
+
+#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
+
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+
+#include <VirtualRobot/MathTools.h>
+
+
+// Qt headers
+#include <Qt>
+#include <QtGlobal>
+#include <QPushButton>
+#include <QLabel>
+#include <QLineEdit>
+#include <QHBoxLayout>
+#include <QMenu>
+
+// std
+#include <memory>
+#include <sstream>
+#include <unordered_set>
+#include <tuple>
+#include <filesystem>
+#include <fstream>
+
+
+// static values and helper functions
+#define DEFAULT_PRIOR_KNOWLEDGE_NAME "PriorKnowledge"
+#define DEFAULT_DEBUG_DRAWER_NAME "DebugDrawerUpdates"
+#define DEFAULT_DEBUG_DRAWER_LAYER_NAME "DebugDrawerUpdates_Graph"
+
+/**
+ * @brief The default width of lines drawn onto the debug layer and scene.
+ */
+static const ::Ice::Float LINE_WIDTH_DEFAULT = 5;
+/**
+ * @brief The width of selected lines drawn onto the debug layer and scene.
+ */
+static const ::Ice::Float LINE_WIDTH_SELECTED = 10;
+
+/**
+ * @brief The default color of lines drawn onto the debug layer and scene.
+ */
+static const ::armarx::DrawColor COLOR_DEFAULT = {0.5f, 0.5f, 1.f, 0.2f};
+/**
+ * @brief The color of highlighted lines drawn onto the debug layer and scene.
+ */
+static const ::armarx::DrawColor COLOR_HIGHLIGHT = {1.f, 0.0f, 0.f, 1.f};
+
+/**
+ * @brief The scale factor for elements drawn onto the scene.
+ */
+static const float SCENE_SCALE_FACTOR = 2;
+/**
+ * @brief The scale factor for nodes drawn onto the scene.
+ */
+static const float SCENE_NODES_SCALE_FACTOR = 3 * SCENE_SCALE_FACTOR;
+/**
+ * @brief The scale factor for edges drawn onto the scene.
+ */
+static const float SCENE_LINE_SCALE_FACTOR = SCENE_SCALE_FACTOR;
+
+/**
+ * @brief The increment used when a rotation button is pressed.
+ *
+ * A positive rotation is counter clockwise.
+ * This value should be positive.
+ *
+ * rotation buttons: LocationGraphEditorWidgetController::widget.buttonRotateClock and
+ * LocationGraphEditorWidgetController::widget.buttonRotateCounterClock
+ */
+static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
+
+static const QString SETTING_LAST_SCENE = "lastScene";
+
+
+namespace armarx
+{
+ /**
+ * @brief Returns the name used on the debug layer.
+ * @param edge The edge.
+ * @return The name used on the debug layer.
+ */
+ inline std::string iceName(const LocationGraphEditorWidgetController::EdgeId& edge)
+ {
+ std::stringstream s;
+ s << "edge_" << edge.first << "_" << edge.second;
+ return s.str();
+ }
+
+ /**
+ * @brief iceName Returns the name used on the debug layer.
+ * @param nodeName The node.
+ * @return The name used on the debug layer.
+ */
+ inline std::string iceName(const LocationGraphEditorWidgetController::NodeId& nodeName)
+ {
+ return nodeName;
+ }
+
+
+ QString LocationGraphEditorWidgetController::GetWidgetName()
+ {
+ return "Navigation.LocationGraphEditor";
+ }
+ QIcon LocationGraphEditorWidgetController::GetWidgetIcon()
+ {
+ return QIcon {":// icons/graph_visu.svg"};
+ }
+
+
+ LocationGraphEditorWidgetController::LocationGraphEditorWidgetController() :
+ debugDrawerTopicName {DEFAULT_DEBUG_DRAWER_NAME},
+ viewAngle {0},
+ debugDrawerLayerName {DEFAULT_DEBUG_DRAWER_LAYER_NAME},
+ priorKnowledgeProxyName {DEFAULT_PRIOR_KNOWLEDGE_NAME},
+ settings {"KIT", "LocationGraphEditorWidgetController"}
+ {
+ widget.setupUi(getWidget());
+
+ loadAutomaticSettings();
+ editStartNodeNext = true;
+
+ // Add scene
+ std::unique_ptr<QGraphicsScene> scenePtr{new QGraphicsScene};
+ scene = scenePtr.get();
+ widget.graphicsViewGraph->setScene(scenePtr.release());
+ MouseEventProcessor* mep = new MouseEventProcessor(this);
+ widget.graphicsViewGraph->installEventFilter(mep);
+
+ // Transform view
+ transformView();
+ }
+
+
+ LocationGraphEditorWidgetController::~LocationGraphEditorWidgetController()
+ {
+ saveAutomaticSettings();
+ }
+
+
+ QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (!dialog)
+ {
+ dialog = new SimpleConfigDialog(parent);
+ }
+
+ dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", memoryNameSystemName);
+ dialog->addProxyFinder<memoryx::PriorKnowledgeInterfacePrx>("PriorKnowledge", "Prior Knowledge", priorKnowledgeProxyName);
+
+ return qobject_cast<SimpleConfigDialog*>(dialog);
+ }
+
+
+ void LocationGraphEditorWidgetController::configured()
+ {
+ memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
+ priorKnowledgeProxyName = dialog->getProxyName("PriorKnowledge");
+ }
+
+
+ void LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
+ {
+ memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(memoryNameSystemName)).toString().toStdString();
+ priorKnowledgeProxyName = settings->value("priorKnowledgeProxyName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
+ {
+ settings->setValue("memoryNameSystemName", QString::fromStdString(memoryNameSystemName));
+ settings->setValue("priorKnowledgeProxyName", QString::fromStdString(priorKnowledgeProxyName));
+ }
+
+
+ void LocationGraphEditorWidgetController::onInitComponent()
+ {
+ usingProxy(priorKnowledgeProxyName);
+ usingProxy("GraphNodePoseResolver");
+ offeringTopic(debugDrawerTopicName);
+ }
+
+
+ void LocationGraphEditorWidgetController::onConnectComponent()
+ {
+ debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
+
+ debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
+ priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
+ getProxy(gnpr, "GraphNodePoseResolver");
+
+ if (priorKnowledgePrx->hasGraphSegment())
+ {
+ ARMARX_VERBOSE << "get Proxy to graph segment";
+ graphSeg = priorKnowledgePrx->getGraphSegment();
+ widget.sceneGroupBox->setEnabled(true);
+ widget.sceneGroupBox->setTitle("Scenes from graph memory segment");
+ }
+ else
+ {
+ widget.sceneGroupBox->setEnabled(false);
+ widget.sceneGroupBox->setTitle("Scenes from graph memory segment (No graph memory segment available)");
+ }
+
+ widget.tableWidgetNodes->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
+ widget.tableWidgetEdges->horizontalHeader()->setResizeMode(QHeaderView::Stretch);
+
+ widget.tableWidgetNodes->setContextMenuPolicy(Qt::CustomContextMenu);
+ widget.tableWidgetEdges->setContextMenuPolicy(Qt::CustomContextMenu);
+
+
+ // tables
+ QObject::connect(widget.tableWidgetNodes, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(nodeTableDoubleClicked(int, int)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetNodes, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetNodesCustomContextMenu(QPoint)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetEdges, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(edgeTableDoubleClicked(int, int)), Qt::UniqueConnection);
+ QObject::connect(widget.tableWidgetEdges, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetEdgesCustomContextMenu(QPoint)), Qt::UniqueConnection);
+
+ QObject::connect(widget.btnAdd, SIGNAL(clicked()), this, SLOT(addNewGraphNode()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdge, SIGNAL(clicked()), this, SLOT(addNewEdgeBoth()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdgeStartEnd, SIGNAL(clicked()), this, SLOT(addNewEdgeStartEnd()), Qt::UniqueConnection);
+ QObject::connect(widget.btnAddEdgeEndStart, SIGNAL(clicked()), this, SLOT(addNewEdgeEndStart()), Qt::UniqueConnection);
+ QObject::connect(widget.btnEdit, SIGNAL(clicked()), this, SLOT(editGraphNode()), Qt::UniqueConnection);
+
+ // zoom
+ QObject::connect(widget.viewZoomFactor, SIGNAL(valueChanged(double)), this, SLOT(transformView()), Qt::UniqueConnection);
+ // rota
+ QObject::connect(widget.buttonRotateClock, SIGNAL(clicked()), this, SLOT(viewRotatedClock()), Qt::UniqueConnection);
+ QObject::connect(widget.buttonRotateCounterClock, SIGNAL(clicked()), this, SLOT(viewRotatedCounterClock()), Qt::UniqueConnection);
+ // redraw+clear
+ QObject::connect(widget.buttonRedraw, SIGNAL(clicked()), this, SLOT(redraw()), Qt::UniqueConnection);
+ QObject::connect(widget.buttonClear, SIGNAL(clicked()), this, SLOT(clearGraph()), Qt::UniqueConnection);
+ // auto adjust
+ QObject::connect(widget.buttonAutoAdjust, SIGNAL(clicked()), this, SLOT(adjustView()), Qt::UniqueConnection);
+ // memory
+ QObject::connect(widget.refreshScenesButton, SIGNAL(clicked()), this, SLOT(updateSceneList()), Qt::UniqueConnection);
+ QObject::connect(widget.drawSceneButton, SIGNAL(clicked()), this, SLOT(drawScene()), Qt::UniqueConnection); // BUTTON LOAD
+ QObject::connect(widget.scenesComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectedSceneChanged(int)), Qt::UniqueConnection);
+
+ ARMARX_VERBOSE << "connected";
+
+ updateSceneList();
+ }
+
+
+ void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ {
+ lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveAutomaticSettings()
+ {
+ settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
+ }
+
+
+ void LocationGraphEditorWidgetController::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 = widget.tableWidgetEdges->rowCount();
+ widget.tableWidgetEdges->setRowCount(row + 1);
+ widget.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())});
+ widget.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);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNode(const memoryx::GraphNodeBasePtr& node)
+ {
+ ARMARX_CHECK_EXPRESSION(node);
+ auto nodeId = node->getId();
+
+ armarx::FramedPosePtr globalNodePose;
+ try
+ {
+ globalNodePose = armarx::FramedPosePtr::dynamicCast(gnpr->resolveToGlobalPose(node));
+ }
+ catch (...)
+ {
+ return;
+ }
+
+ if (hasNode(nodeId))
+ {
+ NodeData& oldNode = nodes.at(nodeId);
+ ARMARX_VERBOSE << "Node: " << nodeId << " was overwritten! Old: "
+ << oldNode.pose->position->x << ", " << oldNode.pose->position->y << ", " << getYawAngle(oldNode.pose) << "| New: "
+ << globalNodePose->position->x << ", " << globalNodePose->position->y << ", " << getYawAngle(globalNodePose);
+ // update node data
+ // table
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
+ widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
+
+ // data
+ oldNode.pose = globalNodePose;
+
+ // update connected edges
+ for (const auto& edge : edges)
+ {
+ if ((edge.first.first == nodeId) || (edge.first.second == nodeId))
+ {
+ updateEdge(edge.first);
+ }
+ }
+ }
+ else
+ {
+ // add&draw node
+ // table
+ int row = widget.tableWidgetNodes->rowCount();
+ widget.tableWidgetNodes->setRowCount(row + 1);
+ widget.tableWidgetNodes->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node->getName())});
+ widget.tableWidgetNodes->setItem(row, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
+ widget.tableWidgetNodes->setItem(row, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
+ widget.tableWidgetNodes->setItem(row, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
+ // scene
+ QGraphicsEllipseItem* graphicsItem = dynamic_cast<QGraphicsEllipseItem*>(new GraphVisualizerGraphicsEllipseItem
+ {
+ *this, node->getName(),
+ globalNodePose->position->x, -globalNodePose->position->y, 0, 0
+ });
+ // auto graphicsItem= scene->addEllipse(node->x,-node->y,0,0);
+ scene->addItem(graphicsItem);
+ // setToolTip on graphicsItem does not work
+ graphicsItem->setZValue(std::numeric_limits<qreal>::max());
+ dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Node:"} +QString::fromStdString(node->getName()));
+
+ // data
+ NodeData data {node, globalNodePose, graphicsItem, row, false};
+ nodes.insert({nodeId, data});
+ }
+
+ updateNode(nodeId);
+ }
+
+
+ void LocationGraphEditorWidgetController::clearEdges()
+ {
+ for (auto& edge : edges)
+ {
+ // remove from graphics scene
+ scene->removeItem(edge.second.graphicsItem);
+ delete edge.second.graphicsItem;
+ // remove from debug layer
+ debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(edge.first));
+ }
+
+ // clear table widget
+ widget.tableWidgetEdges->clearContents();
+ widget.tableWidgetEdges->setRowCount(0);
+ // clear data structures
+ edges.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::clearGraph()
+ {
+ // remove from debug layer
+ for (auto& edge : edges)
+ {
+ debugDrawer->removeLineVisu(debugDrawerLayerName, iceName(edge.first));
+ }
+
+ for (auto& node : nodes)
+ {
+ debugDrawer->removeArrowVisu(debugDrawerLayerName, iceName(node.first));
+ debugDrawer->removeTextVisu(debugDrawerLayerName, iceName(node.first) + "text");
+ debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(node.first));
+ }
+
+ // clear scene
+ scene->clear();
+ // clear table widgets
+ widget.tableWidgetEdges->clearContents();
+ widget.tableWidgetEdges->setRowCount(0);
+ widget.tableWidgetNodes->clearContents();
+ widget.tableWidgetNodes->setRowCount(0);
+ // clear data structures
+
+ edges.clear();
+ nodes.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::resetHighlight()
+ {
+ for (auto& edge : edges)
+ {
+ if (edge.second.highlighted)
+ {
+ edge.second.highlighted = false;
+ updateEdge(edge.first);
+ }
+ }
+
+ for (auto& node : nodes)
+ {
+ if (node.second.highlighted)
+ {
+ node.second.highlighted = false;
+ updateNode(node.first);
+ }
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::updateEdge(const EdgeId& id)
+ {
+ const EdgeData& data = edges.at(id);
+ auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
+ QColor qColor;
+ qColor.setRedF(color.r);
+ qColor.setGreenF(color.g);
+ qColor.setBlueF(color.b);
+
+ auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+ armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.first).pose->position)->toEigen());
+ posStart->z += posStart->z < 1 ? 10 : 0;
+ armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.second).pose->position)->toEigen());
+ posEnd->z += posEnd->z < 1 ? 10 : 0;
+ // debug layer
+ debugDrawer->setLineVisu(debugDrawerLayerName,
+ iceName(id),
+ posStart,
+ posEnd,
+ lineWidth,
+ color);
+
+ data.graphicsItem->setLine(nodes[id.first].pose->position->x, -nodes[id.first].pose->position->y,
+ nodes[id.second].pose->position->x, -nodes[id.second].pose->position->y);
+
+ // scene
+ QPen pen {qColor};
+ pen.setWidthF(lineWidth * SCENE_LINE_SCALE_FACTOR);
+ data.graphicsItem->setPen(pen);
+ // table
+ QFont font {};
+ font.setBold(data.highlighted);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setFont(font);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setFont(font);
+ }
+
+
+ void LocationGraphEditorWidgetController::updateNode(const NodeId& id)
+ {
+ NodeData& data = nodes.at(id);
+
+ if (editStartNodeNext)
+ {
+ widget.editStartNodeId->setText(QString::fromStdString(data.node->getId()));
+ widget.editStartNodeName->setText(QString::fromStdString(data.node->getName()));
+ }
+ else
+ {
+ widget.editEndNodeId->setText(QString::fromStdString(data.node->getId()));
+ widget.editEndNodeName->setText(QString::fromStdString(data.node->getName()));
+ }
+
+ setEditFields(data);
+
+ editStartNodeNext = !editStartNodeNext;
+
+ auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
+ QColor qColor;
+ qColor.setRedF(color.r);
+ qColor.setGreenF(color.g);
+ qColor.setBlueF(color.b);
+
+ auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+
+ // debug layer
+ float yaw = getYawAngle(data.pose) / 180 * M_PI;
+ Eigen::AngleAxisf aa(yaw, Eigen::Vector3f(0, 0, 1));
+ Eigen::Vector3f dir {0, 1, 0};
+ dir = aa.toRotationMatrix() * dir;
+ debugDrawer->setArrowVisu(debugDrawerLayerName, iceName(id), data.pose->position,
+ new armarx::Vector3(dir),
+ armarx::DrawColor {0, 0, 1, 1},
+ 100,
+ lineWidth);
+ debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", data.node->getName(), data.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
+
+ // scene
+ data.graphicsItem->setPen(QPen {qColor});
+ data.graphicsItem->setBrush(QBrush {qColor});
+ data.graphicsItem->setRect(data.pose->position->x - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
+ -data.pose->position->y - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
+ lineWidth * SCENE_NODES_SCALE_FACTOR,
+ lineWidth * SCENE_NODES_SCALE_FACTOR);
+ // table
+ QFont font {};
+ font.setBold(data.highlighted);
+ for (int i = 0; i <= 3; ++i)
+ {
+ widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setData(Qt::BackgroundRole, qColor);
+ widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setFont(font);
+ }
+
+ // highlight all edges between highlighted nodes
+ std::vector<memoryx::GraphNodeBasePtr> highlightedNodes;
+ for (const auto& nodeData : nodes)
+ {
+ if (nodeData.second.highlighted)
+ {
+ highlightedNodes.push_back(nodeData.second.node);
+ }
+ }
+
+ for (auto& edge : edges)
+ {
+ if (edge.second.highlighted)
+ {
+ edge.second.highlighted = false;
+ updateEdge(edge.first);
+ }
+ }
+
+ for (const auto& nodeFrom : highlightedNodes)
+ {
+ for (const auto& nodeTo : highlightedNodes)
+ {
+ const auto nodeFromId = nodeFrom->getId();
+ const auto nodeToId = nodeTo->getId();
+ if (hasEdge(nodeFromId, nodeToId))
+ {
+ auto edgeId = toEdge(nodeFromId, nodeToId);
+ auto& edge = edges[edgeId];
+ edge.highlighted = true;
+ updateEdge(edgeId);
+ }
+ }
+ }
+ }
+
+
+ float LocationGraphEditorWidgetController::getYawAngle(const armarx::PoseBasePtr& pose) const
+ {
+ Eigen::Vector3f rpy;
+ armarx::PosePtr p = armarx::PosePtr::dynamicCast(pose);
+ VirtualRobot::MathTools::eigen4f2rpy(p->toEigen(), rpy);
+ return VirtualRobot::MathTools::rad2deg(rpy[2]);
+ }
+
+
+ void LocationGraphEditorWidgetController::highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted)
+ {
+ if (!hasEdge(node1Id, node2Id))
+ {
+ ARMARX_WARNING << "No edge for: " << node1Id << " and " << node2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ return;
+ }
+
+ EdgeId edge = toEdge(node1Id, node2Id);
+
+ if (edges.at(edge).highlighted != highlighted)
+ {
+ edges.at(edge).highlighted = highlighted;
+ updateEdge(edge);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::highlightNode(const std::string& nodeId, bool highlighted)
+ {
+ if (!hasNode(nodeId))
+ {
+ ARMARX_WARNING << "No node: " << nodeId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ return;
+ }
+
+ if (nodes.at(nodeId).highlighted != highlighted)
+ {
+ nodes.at(nodeId).highlighted = highlighted;
+ updateNode(nodeId);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::nodeTableDoubleClicked(int row, int)
+ {
+ auto nodeIt = std::find_if(nodes.cbegin(), nodes.cend(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ // return d.second.node->getName() == widget.tableWidgetNodes->item(row, 0)->text().toStdString();
+ return d.second.tableWidgetNodesIndex == row;
+ });
+ auto nodeId = nodeIt->second.node->getId();
+ nodeDoubleClicked(nodeId);
+ }
+
+
+ void LocationGraphEditorWidgetController::edgeTableDoubleClicked(int row, int)
+ {
+ auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
+ {
+ return d.second.tableWidgetEdgesIndex == row;
+ });
+
+ edgeDoubleClicked(edgeIt->first);
+ }
+
+
+
+ void LocationGraphEditorWidgetController::nodeDoubleClicked(NodeId id)
+ {
+ nodes.at(id).highlighted ^= true;
+ updateNode(id);
+ }
+
+
+ void LocationGraphEditorWidgetController::edgeDoubleClicked(EdgeId id)
+ {
+ // edges.at(id).highlighted ^= true;
+ // updateEdge(id);
+ bool highlight = !nodes.at(id.first).highlighted && !nodes.at(id.second).highlighted;
+ nodes.at(id.first).highlighted = highlight;
+ nodes.at(id.second).highlighted = highlight;
+ updateNode(id.first);
+ updateNode(id.second);
+ }
+
+
+ void LocationGraphEditorWidgetController::redraw()
+ {
+ drawScene();
+ }
+
+
+ void LocationGraphEditorWidgetController::setEditFields(const NodeData& nodeData)
+ {
+ widget.editNodeId->setText(QString::fromStdString(nodeData.node->getId()));
+ widget.editSceneName->setText(QString::fromStdString(nodeData.node->getScene()));
+ widget.editNodeName->setText(QString::fromStdString(nodeData.node->getName()));
+ widget.editFrameName->setText(QString::fromStdString(nodeData.pose->frame));
+ widget.editAgentName->setText(QString::fromStdString(nodeData.pose->agent));
+
+ Eigen::Vector3f rpy;
+ VirtualRobot::MathTools::eigen4f2rpy(nodeData.pose->toEigen(), rpy);
+ widget.spinBoxX->setValue(nodeData.pose->position->x);
+ widget.spinBoxY->setValue(nodeData.pose->position->y);
+ widget.spinBoxZ->setValue(nodeData.pose->position->z);
+
+ widget.spinBoxRoll->setValue(VirtualRobot::MathTools::rad2deg(rpy[0]));
+ widget.spinBoxPitch->setValue(VirtualRobot::MathTools::rad2deg(rpy[1]));
+ widget.spinBoxYaw->setValue(VirtualRobot::MathTools::rad2deg(rpy[2]));
+ }
+
+
+ void LocationGraphEditorWidgetController::refreshGraph()
+ {
+ clearGraph();
+ drawScene();
+ }
+
+
+ void LocationGraphEditorWidgetController::transformView()
+ {
+ double d = widget.viewZoomFactor->value();
+ widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(viewAngle));
+ }
+
+
+ void LocationGraphEditorWidgetController::viewRotatedClock()
+ {
+ viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
+ transformView();
+ }
+
+
+ void LocationGraphEditorWidgetController::viewRotatedCounterClock()
+ {
+ viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
+ transformView();
+ }
+
+
+ void LocationGraphEditorWidgetController::adjustView()
+ {
+ float maxX = std::numeric_limits<float>::min();
+ float minX = std::numeric_limits<float>::max();
+ float maxY = std::numeric_limits<float>::min();
+ float minY = std::numeric_limits<float>::max();
+
+ // search bounding box
+ for (const auto& node : nodes)
+ {
+ maxX = (maxX < node.second.pose->position->x) ? node.second.pose->position->x : maxX;
+ minX = (minX > node.second.pose->position->x) ? node.second.pose->position->x : minX;
+ maxY = (maxY < node.second.pose->position->y) ? node.second.pose->position->y : maxY;
+ minY = (minY > node.second.pose->position->y) ? node.second.pose->position->y : minY;
+ }
+
+ auto deltaX = maxX - minX; // >=0
+ auto deltaY = maxY - minY; // >=0
+
+ // compare ratio of graph and view. if both horizontal (vertical) ->rotate to 0 or 180 (90,270)
+ if (std::signbit(deltaX / deltaY - 1) == std::signbit(widget.graphicsViewGraph->width() / widget.graphicsViewGraph->height() - 1))
+ {
+ // same => rotate to 0 or 180
+ viewAngle = (viewAngle < std::abs(180 - viewAngle)) ? 0 : 180;
+ // set zoom => update
+ widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaX,
+ widget.graphicsViewGraph->height() / deltaY) * 0.9);
+ }
+ else
+ {
+ // different rotate to 90 or 270
+ viewAngle = (std::abs(90 - viewAngle) < std::abs(270 - viewAngle)) ? 90 : 270;
+ // set zoom => update
+ widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaY,
+ widget.graphicsViewGraph->height() / deltaX) * 0.9);
+ }
+
+ }
+
+
+ bool LocationGraphEditorWidgetController::addNewEdge(const std::string& fromId, const std::string& toId)
+ {
+ std::string errorMsg;
+
+ if (!graphSeg->hasEntityById(fromId))
+ {
+ errorMsg = "start node with Id '" + fromId + "' not found in segment";
+ }
+ else if (!graphSeg->hasEntityById(toId))
+ {
+ errorMsg = "end node with Id '" + toId + "' not found in segment";
+
+ }
+ else if (fromId == toId)
+ {
+ errorMsg = "starting and ending node are the same";
+ }
+ else
+ {
+ auto fromNode = graphSeg->getNodeById(fromId);
+ for (const auto& adjacent : fromNode->getAdjacentNodes())
+ {
+ if (toId == adjacent->getId())
+ {
+ errorMsg = "edge '" + fromNode->getName() + "' -> '" + adjacent->getName() + "' already exists";
+ break;
+ }
+ }
+ }
+
+ if (errorMsg.empty())
+ {
+ widget.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
+ graphSeg->addEdge(fromId, toId);
+ gnpr->forceRefetch(fromId);
+ gnpr->forceRefetch(toId);
+ addEdge(fromId, toId);
+ updateNode(fromId);
+ updateNode(toId);
+ widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
+ }
+ else
+ {
+ ARMARX_WARNING << errorMsg;
+ widget.labelAddEdgeStatus->setText(QString::fromStdString(errorMsg));
+ widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : orange; }");
+ }
+
+ return errorMsg.empty();
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewEdgeBoth()
+ {
+ std::string startId = widget.editStartNodeId->text().toStdString();
+ std::string endId = widget.editEndNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ addNewEdge(endId, startId);
+ }
+
+ void LocationGraphEditorWidgetController::addNewEdgeStartEnd()
+ {
+ std::string startId = widget.editStartNodeId->text().toStdString();
+ std::string endId = widget.editEndNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewEdgeEndStart()
+ {
+ std::string startId = widget.editEndNodeId->text().toStdString();
+ std::string endId = widget.editStartNodeId->text().toStdString();
+ addNewEdge(startId, endId);
+ }
+
+
+ void LocationGraphEditorWidgetController::selectedSceneChanged(int i)
+ {
+ const auto current = widget.scenesComboBox->currentText();
+ if (!current.isEmpty())
+ {
+ lastSelectedSceneName = current;
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::updateSceneList()
+ {
+ auto scenes = graphSeg->getScenes();
+ widget.scenesComboBox->clear();
+ int idx = -1;
+
+ for (std::size_t i = 0; i < scenes.size(); i++)
+ {
+ const auto currentScene = QString::fromStdString(scenes[i]);
+ widget.scenesComboBox->addItem(currentScene);
+
+ if (currentScene == lastSelectedSceneName)
+ {
+ idx = i;
+ }
+ }
+
+ widget.scenesComboBox->setCurrentIndex(idx);
+ }
+
+
+ void LocationGraphEditorWidgetController::drawScene()
+ {
+ std::vector<std::string> highlightedNodes;
+ for (const auto& nodeData : nodes)
+ {
+ bool sameScene = widget.scenesComboBox->currentText().toStdString() == nodeData.second.node->getScene();
+ if (nodeData.second.highlighted && sameScene)
+ {
+ highlightedNodes.push_back(nodeData.first);
+ }
+ }
+
+ clearGraph();
+ auto graphNodes = graphSeg->getNodesByScene(widget.scenesComboBox->currentText().toStdString());
+
+ // add nodes
+ for (auto& node : graphNodes)
+ {
+ auto pos = armarx::FramedPosePtr::dynamicCast(node->getPose());
+
+ if (!pos || node->isMetaEntity())
+ {
+ continue;
+ }
+
+ addNode(node);
+ }
+
+ // add edges
+ for (auto& node : graphNodes)
+ {
+ auto nodeId = node->getId();
+
+ for (int i = 0; i < node->getOutdegree(); i++)
+ {
+ auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
+ ARMARX_CHECK_EXPRESSION(adjacent);
+ auto adjacentId = adjacent->getId();
+
+ addEdge(nodeId, adjacentId);
+ }
+ }
+
+ for (const auto& nodeId : highlightedNodes)
+ {
+ auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ return d.first == nodeId;
+ });
+
+ if (nodeIt != nodes.end())
+ {
+ nodeIt->second.highlighted = true;
+ updateNode(nodeIt->first);
+ }
+ }
+
+ adjustView();
+ }
+
+
+ void LocationGraphEditorWidgetController::selectScene()
+ {
+ // QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
+ // std::string xmlSceneFile = std::string(fi.toLatin1());
+ // loadScene(xmlSceneFile);
+ }
+
+
+ void LocationGraphEditorWidgetController::loadScene(const std::string& xmlFile)
+ {
+ // VirtualRobot::ScenePtr SceneIO::loadScene(const std::string& xmlFile)
+ // {
+ // load file
+ std::ifstream in(xmlFile.c_str());
+
+ if (!in.is_open())
+ {
+ ARMARX_WARNING << "Could not open XML file:" << xmlFile;
+ return;
+ }
+
+ std::stringstream buffer;
+ buffer << in.rdbuf();
+ std::string sceneXML(buffer.str());
+ std::filesystem::path filenameBaseComplete(xmlFile);
+ std::filesystem::path filenameBasePath = filenameBaseComplete.parent_path();
+ std::string basePath = filenameBasePath.string();
+
+ in.close();
+
+ // VirtualRobot::ScenePtr res = createSceneFromString(robotXML, basePath);
+ // THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile);
+
+ // return res;
+ // }
+ }
+
+
+ void LocationGraphEditorWidgetController::addKitchenGraph()
+ {
+ std::string scene {"GraphKitchen"};
+ graphSeg->clearScene(scene);
+
+ // if you insist on hardcoding scenes, use these convenience functions for improved readability:
+ auto addNode = [&](const std::string & name, float x, float y, float angle)
+ {
+ graphSeg->addNode(new ::memoryx::GraphNode {x, y, angle, name, scene});
+ ARMARX_INFO_S << "added node '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
+ };
+
+ auto addEdges = [&](const std::string & nodeFromName, const std::string & nodeToName, bool bidirectional)
+ {
+ auto nodeFrom = graphSeg->getNodeFromSceneByName(scene, nodeFromName);
+ auto nodeTo = graphSeg->getNodeFromSceneByName(scene, nodeToName);
+ ARMARX_CHECK_EXPRESSION(nodeFrom);
+ ARMARX_CHECK_EXPRESSION(nodeTo);
+ ARMARX_INFO_S << "'" << nodeFrom->getName() << "' -> '" << nodeTo->getName() << "', status: " << graphSeg->addEdge(nodeFrom->getId(), nodeTo->getId());
+ if (bidirectional)
+ {
+ ARMARX_INFO_S << "'" << nodeTo->getName() << "' -> '" << nodeFrom->getName() << "', status: " << graphSeg->addEdge(nodeTo->getId(), nodeFrom->getId());
+ }
+ };
+
+ // ex
+ addNode("initialnode", 2900, 7000, 0);
+ addNode("sideboard", 3400, 7000, 0);
+ addEdges("initialnode", "sideboard", true);
+ }
+
+
+ void LocationGraphEditorWidgetController::addNewGraphNode()
+ {
+ Eigen::Matrix4f mat;
+ Eigen::Vector3f rpy;
+ Eigen::Vector3f pos;
+ rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
+ pos << widget.spinBoxX->value(),
+ widget.spinBoxY->value(),
+ widget.spinBoxZ->value();
+ VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
+ armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
+ widget.editFrameName->text().toStdString(),
+ widget.editAgentName->text().toStdString());
+ memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
+ widget.editNodeName->text().toStdString(),
+ widget.editSceneName->text().toStdString());
+ auto entityId = graphSeg->addNode(node);
+ gnpr->forceRefetch(entityId);
+ node->setId(entityId);
+ if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ {
+ widget.editNodeId->setText(QString::fromStdString(entityId));
+ addNode(node);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::editGraphNode()
+ {
+ Eigen::Matrix4f mat;
+ Eigen::Vector3f rpy;
+ Eigen::Vector3f pos;
+ rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
+ VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
+ pos << widget.spinBoxX->value(),
+ widget.spinBoxY->value(),
+ widget.spinBoxZ->value();
+ VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
+ armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
+ widget.editFrameName->text().toStdString(),
+ widget.editAgentName->text().toStdString());
+ memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
+ widget.editNodeName->text().toStdString(),
+ widget.editSceneName->text().toStdString());
+ auto id = widget.editNodeId->text().toStdString();
+
+ node->setId(id);
+ graphSeg->updateEntity(id, node);
+ gnpr->forceRefetch(id);
+ if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ {
+ addNode(node);
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::tableWidgetNodesCustomContextMenu(QPoint pos)
+ {
+ int row = widget.tableWidgetNodes->rowAt(pos.y());
+ auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
+ {
+ return d.second.tableWidgetNodesIndex == row;
+ });
+ QMenu menu;
+ QAction* deleteAction = menu.addAction("Delete Node");
+
+ if (menu.exec(QCursor::pos()) == deleteAction)
+ {
+ ARMARX_CHECK_EXPRESSION(nodeIt != nodes.end());
+ graphSeg->removeNode(nodeIt->second.node->getId());
+ drawScene();
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
+ {
+ int row = widget.tableWidgetEdges->rowAt(pos.y());
+ auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
+ {
+ return d.second.tableWidgetEdgesIndex == row;
+ });
+ QMenu menu;
+ QAction* deleteAction = menu.addAction("Delete Edge");
+
+ if (menu.exec(QCursor::pos()) == deleteAction)
+ {
+ ARMARX_CHECK_EXPRESSION(edgeIt != edges.end());
+ graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.second);
+ drawScene();
+ }
+ }
+
+
+ bool MouseEventProcessor::eventFilter(QObject* obj, QEvent* event)
+ {
+ if (obj == gvw->widget.graphicsViewGraph && event->type() == QEvent::MouseButtonPress)
+ {
+ QMouseEvent* me = static_cast<QMouseEvent*>(event);
+ if (me->button() == Qt::LeftButton)
+ {
+ QPointF scenePoint = gvw->widget.graphicsViewGraph->mapToScene(me->pos());
+ scenePoint.setY(-scenePoint.y()); // not sure why
+
+ float minDist = std::numeric_limits<float>::max();
+ auto bestIt = gvw->nodes.cend();
+
+ for (auto it = gvw->nodes.cbegin(); it != gvw->nodes.cend(); ++it)
+ {
+ float deltaX = it->second.pose->position->x - scenePoint.x();
+ float deltaY = it->second.pose->position->y - scenePoint.y();
+ float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
+
+ if (dist < minDist)
+ {
+ minDist = dist;
+ bestIt = it;
+ }
+ }
+
+ if (bestIt != gvw->nodes.cend())
+ {
+ gvw->nodeDoubleClicked(bestIt->first);
+ }
+ }
+ }
+ else if (event->type() == QEvent::Resize)
+ {
+ gvw->adjustView();
+ }
+ return QObject::eventFilter(obj, event);
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
new file mode 100644
index 00000000..b87a5c2a
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -0,0 +1,487 @@
+/*
+ * 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 Navigation::gui-plugins::LocationGraphEditorWidgetController
+ * @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 <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
+
+
+#include <MemoryX/components/PriorKnowledge/PriorKnowledge.h>
+#include <MemoryX/interface/gui/GraphVisualizerInterface.h>
+#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <QDialog>
+#include <QGraphicsScene>
+#include <QGraphicsLineItem>
+#include <QGraphicsEllipseItem>
+#include <QMainWindow>
+#include <QMouseEvent>
+
+#include <string>
+#include <map>
+#include <vector>
+#include <tuple>
+
+
+namespace armarx
+{
+ class GraphVisualizerGraphicsEllipseItem;
+ class GraphVisualizerGraphicsLineItem;
+
+ class LocationGraphEditorWidgetController;
+
+ class MouseEventProcessor;
+
+
+ /**
+ \page Navigation-GuiPlugins-LocationGraphEditor LocationGraphEditor
+ \brief The LocationGraphEditor allows visualizing ...
+
+ \image html LocationGraphEditor.png
+ The user can
+
+ API Documentation \ref LocationGraphEditorWidgetController
+
+ \see LocationGraphEditorGuiPlugin
+ */
+
+
+ /**
+ * \class LocationGraphEditorGuiPlugin
+ * \ingroup ArmarXGuiPlugins
+ * \brief LocationGraphEditorGuiPlugin brief description
+ *
+ * Detailed description
+ */
+
+ /**
+ * \class LocationGraphEditorWidgetController
+ * \brief LocationGraphEditorWidgetController brief one line description
+ *
+ * Detailed description
+ */
+ class ARMARXCOMPONENT_IMPORT_EXPORT
+ LocationGraphEditorWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate < LocationGraphEditorWidgetController >
+ {
+ Q_OBJECT
+ friend class MouseEventProcessor;
+
+
+ public:
+
+ /// The type of node ids. (This type implies the node exists)
+ using NodeId = const std::string;
+
+ /// The type of edge ids. (This type implies the edge exists)
+ using EdgeId = const std::pair<const std::string, const std::string>;
+
+
+ static QString GetWidgetName();
+ static QIcon GetWidgetIcon();
+
+
+ explicit LocationGraphEditorWidgetController();
+ virtual ~LocationGraphEditorWidgetController() override;
+
+
+ //// @see ArmarXWidgetController::loadSettings()
+ void loadSettings(QSettings* settings) override;
+ /// @see ArmarXWidgetController::saveSettings()
+ void saveSettings(QSettings* settings) override;
+
+
+ QPointer<QDialog> getConfigDialog(QWidget* parent = nullptr) override;
+ void configured() override;
+
+
+ void onInitComponent() override;
+ void onConnectComponent() override;
+
+
+ //because the above load/save functions are *obviously* for "Save/Load Gui Config." :/
+ virtual void loadAutomaticSettings();
+ virtual void saveAutomaticSettings();
+
+
+ // slice interface implementation
+ bool hasEdge(const std::string& node1, const std::string& node2)
+ {
+ return (edges.find(toEdge(node1, node2)) != edges.end());
+ }
+ bool hasNode(const std::string& id)
+ {
+ return (nodes.find(id) != nodes.end());
+ }
+
+
+ public slots:
+
+ // slice interface implementation
+ void addEdge(const std::string& node1Id, const std::string& node2Id);
+ void addNode(const memoryx::GraphNodeBasePtr& node);
+
+ void highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted = true);
+ void highlightNode(const std::string& nodeId, bool highlighted = true);
+
+ void clearEdges();
+ void clearGraph();
+
+ void resetHighlight();
+
+ void redraw();
+ void refreshGraph();
+
+ void tableWidgetNodesCustomContextMenu(QPoint pos);
+ void tableWidgetEdgesCustomContextMenu(QPoint pos);
+
+
+ private slots:
+ /**
+ * @brief add kitchen graph (H2T Armar3a robot kitchen)
+ */
+ void addKitchenGraph();
+
+ void selectedSceneChanged(int i);
+ void updateSceneList();
+
+ void drawScene();
+
+ /**
+ * @brief Toggles the double clicked node's selection state.
+ * @param row Identifies the node.
+ */
+ void nodeTableDoubleClicked(int row, int);
+ /**
+ * @brief Toggles the double clicked edge's selection state.
+ * @param row Identifies the edge.
+ */
+ void edgeTableDoubleClicked(int row, int);
+
+ /**
+ * @brief Toggles the double clicked node's selection state.
+ * @param id Identifies the node.
+ */
+ void nodeDoubleClicked(NodeId id);
+
+ /**
+ * @brief Toggles the double clicked edge's selection state.
+ * @param id Identifies the edge.
+ */
+ void edgeDoubleClicked(EdgeId id);
+
+ /**
+ * @brief Rotates the view clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
+ */
+ void viewRotatedClock();
+
+ /**
+ * @brief Rotates the view counter clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
+ */
+ void viewRotatedCounterClock();
+
+ /**
+ * @brief Applies the current transforamtion to the view.
+ */
+ void transformView();
+
+ /**
+ * @brief Adjusts the view's zoom and rotation to display most of the graph.
+ */
+ void adjustView();
+
+ bool addNewEdge(const std::string& from, const std::string& to);
+ void addNewEdgeBoth();
+ void addNewEdgeStartEnd();
+ void addNewEdgeEndStart();
+
+ void addNewGraphNode();
+ void editGraphNode();
+
+
+ private:
+
+ /// Widget Form
+ Ui::LocationGraphEditorWidget widget;
+
+
+ /**
+ * @brief The NodeData struct holds data required for the node.
+ * The name is stored in the key used in the map nodes.
+ */
+ struct NodeData
+ {
+ /**
+ * @brief The Entity of the graph segment this struct represents
+ */
+ memoryx::GraphNodeBasePtr node;
+
+ /**
+ * @brief The pose drawn to debugDrawer.
+ */
+ armarx::FramedPosePtr pose;
+
+ /**
+ * @brief The ellipse in the scene.
+ */
+ QGraphicsEllipseItem* graphicsItem;
+
+ /**
+ * @brief The row in the table tableWidgetNodes.
+ */
+ int tableWidgetNodesIndex;
+
+ /**
+ * @brief Whether the node is highlighted.
+ */
+ bool highlighted;
+ };
+
+ /**
+ * @brief The EdgeData struct holds data required for the edge.
+ * The name is stored in the key used in the map edges.
+ */
+ struct EdgeData
+ {
+ /**
+ * @brief The line in the scene.
+ */
+ QGraphicsLineItem* graphicsItem;
+ /**
+ * @brief The row in the table tableWidgetEdges.
+ */
+ int tableWidgetEdgesIndex;
+
+ /**
+ * @brief Whether the edge is highlighted.
+ */
+ bool highlighted;
+ };
+
+
+ /**
+ * @brief Returns the EdgeId corresponding to two nodes.
+ * @param node1 First node id.
+ * @param node2 Second node id.
+ * @return The EdgeId corresponding to two nodes.
+ */
+ static EdgeId toEdge(const std::string& node1, const std::string& node2)
+ {
+ return EdgeId {node1, node2};
+ }
+
+ /**
+ * @brief Updates an edge.
+ * @param The edge to update.
+ */
+ void updateEdge(const EdgeId& id);
+
+ /**
+ * @brief Updates a node.
+ * @param The node to update.
+ */
+ void updateNode(const NodeId& id);
+
+ void setEditFields(const NodeData& node);
+
+ /**
+ * @brief The topic name used by debugDrawer.
+ */
+ std::string debugDrawerTopicName;
+
+ /**
+ * @brief Used to draw onto debug layers.
+ */
+ ::armarx::DebugDrawerInterfacePrx debugDrawer;
+
+ /**
+ * @brief The config dialog.
+ */
+ QPointer<SimpleConfigDialog> dialog;
+
+ float getYawAngle(const armarx::PoseBasePtr& pose) const;
+
+ /**
+ * @brief The scene displayed in the widget.
+ *
+ * For y coordinates -pos->y is used to mirror the scene on the y axis.
+ * If pos->y would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
+ */
+ QPointer<QGraphicsScene> scene;
+
+ /**
+ * @brief The nodes.
+ */
+ std::map<std::string, NodeData> nodes;
+
+ /**
+ * @brief The edges.
+ */
+ std::map<EdgeId, EdgeData> edges;
+
+ /**
+ * @brief The view's rotation angle.
+ */
+ qreal viewAngle;
+
+ /**
+ * @brief The layer to draw on.
+ */
+ std::string debugDrawerLayerName;
+
+ bool editStartNodeNext;
+
+ std::string priorKnowledgeProxyName;
+ memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
+ memoryx::GraphNodePoseResolverInterfacePrx gnpr;
+ memoryx::GraphMemorySegmentBasePrx graphSeg;
+ QSettings settings;
+ QString lastSelectedSceneName;
+
+ /**
+ * selectScene(): private function called when button load is pushed, and calls the function loadScene()
+ */
+ void selectScene();
+
+ /**
+ * @brief loadScene Private function that parses XML file to load a scene
+ * @param xmlFile
+ */
+ void loadScene(const std::string& xmlFile);
+
+
+ friend class GraphVisualizerGraphicsEllipseItem;
+ friend class GraphVisualizerGraphicsLineItem;
+
+
+ private:
+
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ armem::client::MemoryNameSystem memoryNameSystem;
+
+ };
+
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
+ {
+ public:
+ using NodeId = LocationGraphEditorWidgetController::NodeId;
+
+ GraphVisualizerGraphicsEllipseItem(LocationGraphEditorWidgetController& visuWidget, NodeId name, qreal x, qreal y, qreal width, qreal height, QGraphicsItem* parent = nullptr):
+ QGraphicsEllipseItem {x, y, width, height, parent},
+ id {name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ ~GraphVisualizerGraphicsEllipseItem() override
+ {
+ }
+ protected:
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
+ {
+ parentVisuWidget.nodeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const NodeId id;
+
+ /**
+ * @brief Required to call nodeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ */
+ LocationGraphEditorWidgetController& parentVisuWidget;
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event. This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
+ {
+ public:
+ using EdgeId = LocationGraphEditorWidgetController::EdgeId;
+
+ GraphVisualizerGraphicsLineItem(LocationGraphEditorWidgetController& visuWidget, EdgeId name, qreal x1, qreal y1, qreal x2, qreal y2, QGraphicsItem* parent = 0):
+ QGraphicsLineItem {x1, y1, x2, y2, parent},
+ id {name},
+ parentVisuWidget(visuWidget)
+ {
+ }
+
+ ~GraphVisualizerGraphicsLineItem() override
+ {
+ }
+ signals:
+ protected:
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
+ {
+ parentVisuWidget.edgeDoubleClicked(id);
+ }
+ private:
+ /**
+ * @brief Required to identify the element.
+ */
+ const EdgeId id;
+
+ /// Required to call edgeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ LocationGraphEditorWidgetController& parentVisuWidget;
+ };
+
+
+ class MouseEventProcessor : public QObject
+ {
+ Q_OBJECT
+ public:
+
+ MouseEventProcessor(LocationGraphEditorWidgetController* gvw) : gvw(gvw) {}
+
+
+ protected:
+
+ LocationGraphEditorWidgetController* gvw;
+ bool eventFilter(QObject* obj, QEvent* event) override;
+
+ };
+
+}
--
GitLab
From 89db9295363205db3bc005dbdce09442abc98453 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 20 Aug 2021 14:30:53 +0200
Subject: [PATCH 12/33] Transfer GraphVisuGui, refactor its structure and adapt
to new memory stuff (WIP)
---
source/armarx/navigation/graph/Graph.cpp | 10 +
source/armarx/navigation/graph/Graph.h | 3 +
.../LocationGraphEditor/CMakeLists.txt | 30 +-
.../FunctionalEventFilter.cpp | 18 +
.../FunctionalEventFilter.h | 32 +
.../LocationGraphEditor/GraphScene.cpp | 165 +++
.../LocationGraphEditor/GraphScene.h | 119 ++
.../LocationGraphEditor/GuiGraph.cpp | 73 ++
.../LocationGraphEditor/GuiGraph.h | 86 ++
.../LocationGraphEditorWidget.ui | 482 ++-----
.../LocationGraphEditorWidgetController.cpp | 1117 +++++++----------
.../LocationGraphEditorWidgetController.h | 430 ++-----
.../widgets/EdgeTableWidget.cpp | 75 ++
.../widgets/EdgeTableWidget.h | 57 +
.../widgets/VertexDataWidget.cpp | 139 ++
.../widgets/VertexDataWidget.h | 78 ++
.../widgets/VertexTableWidget.cpp | 87 ++
.../widgets/VertexTableWidget.h | 56 +
18 files changed, 1691 insertions(+), 1366 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index d8350099..bce79ae3 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -26,6 +26,16 @@
namespace armarx::nav::graph
{
+ std::string VertexAttribs::getName() const
+ {
+ return aron.locationID.entityName;
+ }
+
+ Eigen::Matrix4f VertexAttribs::getPose() const
+ {
+ return aron.globalRobotPose;
+ }
+
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index 10ec0e29..dd5704bc 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -33,6 +33,9 @@ namespace armarx::nav::graph
struct VertexAttribs : public semrel::ShapeVertex
{
armarx::nav::graph::arondto::Vertex aron;
+
+ std::string getName() const;
+ Eigen::Matrix4f getPose() const;
};
struct EdgeAttribs
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 1c9c13a4..3d865290 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -12,9 +12,25 @@ armarx_build_if(ArmarXGui_FOUND "ArmarXGui not available")
# do not rename this variable, it is used in armarx_gui_library()...
set(SOURCES
LocationGraphEditorWidgetController.cpp
+
+ FunctionalEventFilter.cpp
+ GraphScene.cpp
+ GuiGraph.cpp
+
+ widgets/EdgeTableWidget.cpp
+ widgets/VertexDataWidget.cpp
+ widgets/VertexTableWidget.cpp
)
set(HEADERS
LocationGraphEditorWidgetController.h
+
+ FunctionalEventFilter.h
+ GraphScene.h
+ GuiGraph.h
+
+ widgets/EdgeTableWidget.h
+ widgets/VertexDataWidget.h
+ widgets/VertexTableWidget.h
)
set(GUI_UIS
LocationGraphEditorWidget.ui
@@ -29,24 +45,14 @@ set(COMPONENT_LIBS
# RobotAPI
armem
- # MemoryX
- MemoryXCore
- MemoryXMemoryTypes
+ Navigation::Location
+ Navigation::Graph
)
if(ArmarXGui_FOUND)
armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
-
- # ToDo: Remove
- 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()
-
-
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
# all target_include_directories must be guarded by if(Xyz_FOUND)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
new file mode 100644
index 00000000..1ffe763d
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
@@ -0,0 +1,18 @@
+#include "FunctionalEventFilter.h"
+
+
+namespace simox::gui
+{
+
+ FunctionalEventFilter::FunctionalEventFilter(Function&& function) :
+ function(function)
+ {
+ }
+
+
+ bool FunctionalEventFilter::eventFilter(QObject* obj, QEvent* event)
+ {
+ return function(obj, event);
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
new file mode 100644
index 00000000..3513a795
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
@@ -0,0 +1,32 @@
+#pragma once
+
+#include <QObject>
+
+#include <functional>
+
+class QEvent;
+
+
+namespace simox::gui
+{
+
+ class FunctionalEventFilter : public QObject
+ {
+ Q_OBJECT
+
+ public:
+
+ using Function = std::function<bool(QObject* obj, QEvent* event)>;
+
+ FunctionalEventFilter(Function&& function);
+
+
+ protected:
+
+ bool eventFilter(QObject* obj, QEvent* event) override;
+
+ Function function;
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
new file mode 100644
index 00000000..9b927dcf
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
@@ -0,0 +1,165 @@
+#include "GraphScene.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+
+#include <Eigen/Core>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ QGraphicsEllipseItem*
+ GraphScene::addVertex(const graph::Graph::Vertex& vertex)
+ {
+ const Eigen::Matrix4f& pose = vertex.attrib().getPose();
+
+ // To capture by copy
+ semrel::ShapeID vertexID = vertex.objectID();
+
+ GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
+ {
+ [this, vertexID]() { emit vertexSelected(vertexID); },
+ static_cast<qreal>(pose(0, 3)),
+ static_cast<qreal>(- pose(1, 3)),
+ 0, 0
+ };
+ addItem(item);
+
+ // setToolTip on graphicsItem does not work
+ item->setZValue(std::numeric_limits<qreal>::max());
+ // dynamic_cast<QGraphicsItem*>(item)->setToolTip(QString::fromStdString("Vertex '" + name + "'"));
+ item->setToolTip(QString::fromStdString("Vertex '" + vertex.attrib().getName() + "'"));
+
+ return item;
+ }
+
+
+ QGraphicsLineItem*
+ GraphScene::addEdge(const graph::Graph::Edge& edge)
+ {
+ semrel::ShapeID sourceID = edge.sourceObjectID();
+ semrel::ShapeID targetID = edge.targetObjectID();
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
+ {
+ [this, sourceID, targetID]() { emit edgeSelected(sourceID, targetID); },
+ sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3)
+ };
+ addItem(item);
+
+ // setToolTip on item does not work
+ std::stringstream toolTip;
+ toolTip << "Edge '" << edge.source().attrib().getName()
+ << "' -> '" << edge.target().attrib().getName();
+ item->setToolTip(QString::fromStdString(toolTip.str()));
+
+ return item;
+ }
+
+
+ void GraphScene::updateVertex(GuiGraph::Vertex& vertex)
+ {
+ QGraphicsEllipseItem* item = vertex.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+
+ QColor color = vertex.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = vertex.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ item->setPen(QPen {color});
+ item->setBrush(QBrush {color});
+ item->setRect( pose(0, 3) - lineWidth * verticesScaleFactor / 2,
+ - pose(1, 3) - lineWidth * verticesScaleFactor / 2,
+ lineWidth * verticesScaleFactor,
+ lineWidth * verticesScaleFactor);
+ }
+
+
+ void GraphScene::updateEdge(GuiGraph::Edge& edge)
+ {
+ QGraphicsLineItem* item = edge.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ QColor color = edge.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = edge.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ QPen pen {color};
+ pen.setWidthF(lineWidth * lineScaleFactor);
+
+ item->setPen(pen);
+ item->setLine(sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3));
+ }
+
+
+ void GraphScene::removeVertex(QGraphicsEllipseItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ void GraphScene::removeEdge(QGraphicsLineItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y,
+ qreal width, qreal height,
+ QGraphicsItem* parent) :
+ QGraphicsEllipseItem {x, y, width, height, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::~GraphVisualizerGraphicsEllipseItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+
+
+ GraphVisualizerGraphicsLineItem::GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1, qreal x2, qreal y2,
+ QGraphicsItem* parent) :
+ QGraphicsLineItem {x1, y1, x2, y2, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsLineItem::~GraphVisualizerGraphicsLineItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
new file mode 100644
index 00000000..514bd6a4
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -0,0 +1,119 @@
+#pragma once
+
+#include "GuiGraph.h"
+
+#include <armarx/navigation/graph/Graph.h>
+
+#include <QGraphicsScene>
+#include <QGraphicsEllipseItem>
+#include <QGraphicsLineItem>
+
+#include <functional>
+
+
+namespace semrel
+{
+ struct ShapeID;
+}
+namespace armarx::nav::locgrapheditor
+{
+ class GraphScene : public QGraphicsScene
+ {
+ Q_OBJECT
+
+ public:
+
+ using QGraphicsScene::QGraphicsScene;
+
+ QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
+ QGraphicsLineItem* addEdge(const graph::Graph::Edge& Edge);
+
+ void updateVertex(GuiGraph::Vertex& vertex);
+ void updateEdge(GuiGraph::Edge& edge);
+
+ void removeVertex(QGraphicsEllipseItem*& item);
+ void removeEdge(QGraphicsLineItem*& item);
+
+
+ public slots:
+
+
+ signals:
+
+ void vertexSelected(const semrel::ShapeID& vertexID);
+ void edgeSelected(const semrel::ShapeID& sourceID, const semrel::ShapeID& targetID);
+
+
+ public:
+
+ double lineWidthDefault = 5;
+ double lineWidthSelected = 10;
+
+ double sceneScaleFactor = 2;
+ double verticesScaleFactor = 3.0 * sceneScaleFactor;
+ double lineScaleFactor = 1.0 * sceneScaleFactor;
+
+ QColor colorDefault = QColor::fromRgb(128, 128, 255);
+ QColor colorSelected = QColor::fromRgb(255, 128, 0);
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
+ {
+ public:
+
+ GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y, qreal width, qreal height,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsEllipseItem() override;
+
+
+ protected:
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
+ {
+ public:
+
+ GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1,
+ qreal x2, qreal y2,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsLineItem() override;
+
+
+ protected:
+
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
new file mode 100644
index 00000000..377f726e
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -0,0 +1,73 @@
+/*
+ * 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 "GuiGraph.h"
+
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+}
+
+namespace armarx::nav
+{
+
+ float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+ {
+ Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
+ return simox::math::rad_to_deg(rpy[2]);
+ }
+
+ auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+ {
+ GuiGraph gui;
+ for (auto v : nav.vertices())
+ {
+ gui.addVertex(v.objectID(), { v.attrib() });
+ }
+ for (auto e : nav.edges())
+ {
+ gui.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ }
+ return gui;
+ }
+
+
+ nav::graph::Graph locgrapheditor::fromGuiGraph(const GuiGraph& gui)
+ {
+ graph::Graph nav;
+ for (auto v : gui.vertices())
+ {
+ nav.addVertex(v.objectID(), { v.attrib() });
+ }
+ for (auto e : gui.edges())
+ {
+ nav.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ }
+ return nav;
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
new file mode 100644
index 00000000..f0b6e0bb
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -0,0 +1,86 @@
+/*
+ * 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/>.
+ *
+ * @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 <armarx/navigation/graph/Graph.h>
+
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+
+
+class QGraphicsEllipseItem;
+class QGraphicsLineItem;
+class QTableWidgetItem;
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ /**
+ * @brief The NodeData struct holds data required for the node.
+ * The name is stored in the key used in the map vertices.
+ */
+ struct VertexData : public nav::graph::VertexAttribs
+ {
+ /// The ellipse in the scene.
+ QGraphicsEllipseItem* graphicsItem = nullptr;
+
+ /// The item in the table tableWidgetVertices.
+ QTableWidgetItem* tableWidgetItem = nullptr;
+
+ /// Whether the node is highlighted.
+ bool highlighted = false;
+ };
+
+
+ /**
+ * @brief The EdgeData struct holds data required for the edge.
+ * The name is stored in the key used in the map edges.
+ */
+ struct EdgeData : public nav::graph::EdgeAttribs
+ {
+ /// The line in the scene.
+ QGraphicsLineItem* graphicsItem = nullptr;
+
+ /// The item in the table tableWidgetEdges.
+ QTableWidgetItem* tableWidgetItem = nullptr;
+
+ /// Whether the edge is highlighted.
+ bool highlighted = false;
+ };
+
+
+ struct GraphData : public nav::graph::GraphAttribs
+ {
+ };
+
+
+ using GuiGraph = semrel::RelationGraph<VertexData, EdgeData, GraphData>;
+
+
+ GuiGraph toGuiGraph(const graph::Graph& graph);
+ graph::Graph fromGuiGraph(const GuiGraph& graph);
+
+
+
+ float getYawAngleDegree(const Eigen::Matrix4f& pose);
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 4e2a1782..6a33a012 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>783</width>
- <height>664</height>
+ <height>684</height>
</rect>
</property>
<property name="windowTitle">
@@ -21,7 +21,7 @@
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
- <widget class="QPushButton" name="refreshScenesButton">
+ <widget class="QPushButton" name="refreshGraphsButton">
<property name="toolTip">
<string>Reloads the list of scenes from the memory</string>
</property>
@@ -31,7 +31,7 @@
</widget>
</item>
<item>
- <widget class="QComboBox" name="scenesComboBox">
+ <widget class="QComboBox" name="graphsComboBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Expanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -41,7 +41,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="drawSceneButton">
+ <widget class="QPushButton" name="loadGraphButton">
<property name="toolTip">
<string>Draws the selected scene</string>
</property>
@@ -54,7 +54,7 @@
</widget>
</item>
<item>
- <widget class="QSplitter" name="splitter_3">
+ <widget class="QSplitter" name="verticalSplitter">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
@@ -98,7 +98,7 @@
<item>
<widget class="QPushButton" name="buttonClear">
<property name="text">
- <string>Clear graph</string>
+ <string>Clear Graph</string>
</property>
</widget>
</item>
@@ -173,391 +173,89 @@
</widget>
<widget class="QWidget" name="tables" native="true">
<layout class="QVBoxLayout" name="verticalLayout">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
<item>
- <widget class="QSplitter" name="splitter_2">
+ <widget class="QSplitter" name="horizontalSplitter">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
- <widget class="QSplitter" name="splitter">
+ <widget class="QWidget" name="verticalLayout_12Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_12">
+ <item>
+ <widget class="QGroupBox" name="locationsTableGroupBox">
+ <property name="title">
+ <string>Locations (Graph Vertices)</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_7">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayout_13Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_13">
+ <item>
+ <widget class="QGroupBox" name="edgesTableGroupBox">
+ <property name="title">
+ <string>Graph Edges</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_8">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ </layout>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="verticalLayout_6Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <item>
+ <widget class="QGroupBox" name="locationDataGroupBox">
+ <property name="title">
+ <string>Location Data</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3"/>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QSplitter" name="horizontalSplitter_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
- <widget class="QWidget" name="layoutWidget">
- <layout class="QVBoxLayout" name="verticalLayout_nodes">
- <item>
- <widget class="QLabel" name="label_nodes">
- <property name="text">
- <string>Nodes</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QTableWidget" name="tableWidgetNodes">
- <property name="editTriggers">
- <set>QAbstractItemView::NoEditTriggers</set>
- </property>
- <column>
- <property name="text">
- <string>Name</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>X</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>Y</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>Yaw Angle</string>
- </property>
- </column>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="layoutWidget2">
- <layout class="QVBoxLayout" name="verticalLayout_edges">
- <item>
- <widget class="QLabel" name="label_edges">
- <property name="text">
- <string>Edges</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QTableWidget" name="tableWidgetEdges">
- <property name="editTriggers">
- <set>QAbstractItemView::NoEditTriggers</set>
- </property>
- <column>
- <property name="text">
- <string>Node 1</string>
- </property>
- </column>
- <column>
- <property name="text">
- <string>Node 2</string>
- </property>
- </column>
- </widget>
- </item>
- </layout>
- </widget>
- </widget>
- <widget class="QTabWidget" name="tabWidget">
- <property name="currentIndex">
- <number>0</number>
- </property>
- <widget class="QWidget" name="tabWidgetPage1">
- <attribute name="title">
- <string>Add Node</string>
- </attribute>
- <layout class="QFormLayout" name="formLayout">
- <property name="fieldGrowthPolicy">
- <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
- </property>
- <item row="1" column="0">
- <widget class="QLabel" name="label_12">
- <property name="text">
- <string>NodeId</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="editNodeId">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Name</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="editNodeName"/>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>Frame</string>
- </property>
- </widget>
- </item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="editFrameName"/>
- </item>
- <item row="5" column="0">
- <widget class="QLabel" name="label_2">
- <property name="text">
- <string>Agent</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="editAgentName"/>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_3">
- <property name="text">
- <string>X</string>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxX">
- <property name="decimals">
- <number>0</number>
- </property>
- <property name="minimum">
- <double>-1000000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000000.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="8" column="0">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Y</string>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxY">
- <property name="decimals">
- <number>0</number>
- </property>
- <property name="minimum">
- <double>-1000000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000000.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="9" column="0">
- <widget class="QLabel" name="label_5">
- <property name="text">
- <string>Z</string>
- </property>
- </widget>
- </item>
- <item row="9" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxZ">
- <property name="decimals">
- <number>0</number>
- </property>
- <property name="minimum">
- <double>-1000000.000000000000000</double>
- </property>
- <property name="maximum">
- <double>1000000.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="10" column="0">
- <widget class="QLabel" name="label_6">
- <property name="text">
- <string>R in deg</string>
- </property>
- </widget>
- </item>
- <item row="10" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxRoll">
- <property name="minimum">
- <double>-360.000000000000000</double>
- </property>
- <property name="maximum">
- <double>360.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="11" column="0">
- <widget class="QLabel" name="label_7">
- <property name="text">
- <string>P in deg</string>
- </property>
- </widget>
- </item>
- <item row="11" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxPitch">
- <property name="minimum">
- <double>-360.000000000000000</double>
- </property>
- <property name="maximum">
- <double>360.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="12" column="0">
- <widget class="QLabel" name="label_8">
- <property name="text">
- <string>Y in deg</string>
- </property>
- </widget>
- </item>
- <item row="12" column="1">
- <widget class="QDoubleSpinBox" name="spinBoxYaw">
- <property name="minimum">
- <double>-360.000000000000000</double>
- </property>
- <property name="maximum">
- <double>360.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="13" column="0">
- <widget class="QPushButton" name="btnAdd">
- <property name="text">
- <string>Add</string>
- </property>
- </widget>
- </item>
- <item row="13" column="1">
- <widget class="QPushButton" name="btnEdit">
- <property name="enabled">
- <bool>true</bool>
- </property>
- <property name="text">
- <string>Edit</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="editSceneName"/>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_13">
- <property name="text">
- <string>Scene</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab">
- <attribute name="title">
- <string>Add Edge</string>
- </attribute>
- <layout class="QFormLayout" name="gridLayout">
- <property name="fieldGrowthPolicy">
- <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
- </property>
- <item row="0" column="0">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Start Node Id</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QLineEdit" name="editStartNodeId">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="labelStartNode">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Start Node</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="editStartNodeName">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>End Node Id</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="editEndNodeId">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="labelEndNode">
- <property name="text">
- <string>End Node</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="editEndNodeName">
- <property name="enabled">
- <bool>false</bool>
- </property>
- </widget>
- </item>
- <item row="4" column="0" colspan="2">
- <widget class="QPushButton" name="btnAddEdge">
- <property name="text">
- <string>Add Double-sided Edge</string>
- </property>
- </widget>
- </item>
- <item row="8" column="0">
- <spacer name="verticalSpacer">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>40</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="5" column="0" colspan="2">
- <widget class="QPushButton" name="btnAddEdgeStartEnd">
- <property name="text">
- <string>Add Edge Start -> End</string>
- </property>
- </widget>
- </item>
- <item row="6" column="0" colspan="2">
- <widget class="QPushButton" name="btnAddEdgeEndStart">
- <property name="text">
- <string>Add Edge End -> Start</string>
- </property>
- </widget>
- </item>
- <item row="7" column="0" colspan="2">
- <widget class="QLabel" name="labelAddEdgeStatus">
- <property name="text">
- <string/>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
</widget>
</widget>
</item>
@@ -565,6 +263,22 @@
</widget>
</widget>
</item>
+ <item>
+ <widget class="QLabel" name="statusLabel">
+ <property name="font">
+ <font>
+ <pointsize>9</pointsize>
+ <italic>true</italic>
+ </font>
+ </property>
+ <property name="text">
+ <string/>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<resources/>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index cb576778..9510d82c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -21,69 +21,36 @@
*/
#include "LocationGraphEditorWidgetController.h"
+#include "widgets/EdgeTableWidget.h"
+#include "widgets/VertexDataWidget.h"
+#include "widgets/VertexTableWidget.h"
-#include <MemoryX/libraries/memorytypes/variants/GraphNode/GraphNode.h>
+#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <armarx/navigation/location/constants.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <VirtualRobot/MathTools.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <SimoxUtility/color/Color.h>
// Qt headers
-#include <Qt>
-#include <QtGlobal>
-#include <QPushButton>
+#include <QDialog>
+#include <QHBoxLayout>
#include <QLabel>
#include <QLineEdit>
-#include <QHBoxLayout>
#include <QMenu>
+#include <QMouseEvent>
+#include <QObject>
+#include <QPushButton>
// std
-#include <memory>
#include <sstream>
-#include <unordered_set>
-#include <tuple>
-#include <filesystem>
-#include <fstream>
-
-
-// static values and helper functions
-#define DEFAULT_PRIOR_KNOWLEDGE_NAME "PriorKnowledge"
-#define DEFAULT_DEBUG_DRAWER_NAME "DebugDrawerUpdates"
-#define DEFAULT_DEBUG_DRAWER_LAYER_NAME "DebugDrawerUpdates_Graph"
-
-/**
- * @brief The default width of lines drawn onto the debug layer and scene.
- */
-static const ::Ice::Float LINE_WIDTH_DEFAULT = 5;
-/**
- * @brief The width of selected lines drawn onto the debug layer and scene.
- */
-static const ::Ice::Float LINE_WIDTH_SELECTED = 10;
-/**
- * @brief The default color of lines drawn onto the debug layer and scene.
- */
-static const ::armarx::DrawColor COLOR_DEFAULT = {0.5f, 0.5f, 1.f, 0.2f};
-/**
- * @brief The color of highlighted lines drawn onto the debug layer and scene.
- */
-static const ::armarx::DrawColor COLOR_HIGHLIGHT = {1.f, 0.0f, 0.f, 1.f};
-
-/**
- * @brief The scale factor for elements drawn onto the scene.
- */
-static const float SCENE_SCALE_FACTOR = 2;
-/**
- * @brief The scale factor for nodes drawn onto the scene.
- */
-static const float SCENE_NODES_SCALE_FACTOR = 3 * SCENE_SCALE_FACTOR;
-/**
- * @brief The scale factor for edges drawn onto the scene.
- */
-static const float SCENE_LINE_SCALE_FACTOR = SCENE_SCALE_FACTOR;
/**
* @brief The increment used when a rotation button is pressed.
@@ -99,28 +66,30 @@ static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
static const QString SETTING_LAST_SCENE = "lastScene";
-namespace armarx
+namespace armarx::nav::locgrapheditor
{
+
+
/**
* @brief Returns the name used on the debug layer.
* @param edge The edge.
* @return The name used on the debug layer.
*/
- inline std::string iceName(const LocationGraphEditorWidgetController::EdgeId& edge)
+ std::string iceName(const GuiGraph::Edge& edge)
{
- std::stringstream s;
- s << "edge_" << edge.first << "_" << edge.second;
- return s.str();
+ std::stringstream ss;
+ ss << "edge " << std::to_string(edge.sourceObjectID()) << " -> " << std::to_string(edge.targetObjectID());
+ return ss.str();
}
/**
* @brief iceName Returns the name used on the debug layer.
- * @param nodeName The node.
+ * @param vertexName The vertex.
* @return The name used on the debug layer.
*/
- inline std::string iceName(const LocationGraphEditorWidgetController::NodeId& nodeName)
+ std::string iceName(const GuiGraph::Vertex& vertex)
{
- return nodeName;
+ return std::to_string(vertex.objectID());
}
@@ -135,26 +104,122 @@ namespace armarx
LocationGraphEditorWidgetController::LocationGraphEditorWidgetController() :
- debugDrawerTopicName {DEFAULT_DEBUG_DRAWER_NAME},
- viewAngle {0},
- debugDrawerLayerName {DEFAULT_DEBUG_DRAWER_LAYER_NAME},
- priorKnowledgeProxyName {DEFAULT_PRIOR_KNOWLEDGE_NAME},
settings {"KIT", "LocationGraphEditorWidgetController"}
{
widget.setupUi(getWidget());
loadAutomaticSettings();
- editStartNodeNext = true;
+
+ dataWidgets.edgeTable = new EdgeTableWidget();
+ dataWidgets.vertexTable = new VertexTableWidget();
+ dataWidgets.vertexData = new VertexDataWidget();
+ widget.edgesTableGroupBox->layout()->addWidget(dataWidgets.edgeTable);
+ widget.locationsTableGroupBox->layout()->addWidget(dataWidgets.vertexTable);
+ widget.locationDataGroupBox->layout()->addWidget(dataWidgets.vertexData);
// Add scene
- std::unique_ptr<QGraphicsScene> scenePtr{new QGraphicsScene};
- scene = scenePtr.get();
- widget.graphicsViewGraph->setScene(scenePtr.release());
- MouseEventProcessor* mep = new MouseEventProcessor(this);
- widget.graphicsViewGraph->installEventFilter(mep);
+ view.view = widget.graphicsViewGraph;
+ view.setScene(new GraphScene());
+
+ auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
+ {
+#if 0
+ if (obj == this->view.view && event->type() == QEvent::MouseButtonPress)
+ {
+ QMouseEvent* me = static_cast<QMouseEvent*>(event);
+ if (me->button() == Qt::LeftButton)
+ {
+ QPointF scenePoint = this->view.view->mapToScene(me->pos());
+ scenePoint.setY(- scenePoint.y()); // not sure why
+
+ float minDist = std::numeric_limits<float>::max();
+ auto bestIt = this->vertices.cend();
+
+ for (auto it = this->vertices.cbegin(); it != this->vertices.cend(); ++it)
+ {
+ float deltaX = it->second.pose->position->x - scenePoint.x();
+ float deltaY = it->second.pose->position->y - scenePoint.y();
+ float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
+
+ if (dist < minDist)
+ {
+ minDist = dist;
+ bestIt = it;
+ }
+ }
+
+ if (bestIt != this->vertices.cend())
+ {
+ this->highlightVertex(bestIt->first);
+ }
+ }
+ }
+ else if (event->type() == QEvent::Resize)
+ {
+ this->adjustView();
+ }
+#endif
+ return QObject::eventFilter(obj, event);
+ };
+ view.view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
+
// Transform view
transformView();
+
+ // Widgets -> This
+
+ // Memory Access
+ connect(widget.refreshGraphsButton, &QPushButton::clicked, this, &This::updateGraphList);
+ connect(widget.loadGraphButton, &QPushButton::clicked, this, &This::loadGraph);
+
+
+#if 0
+ // Tables
+ connect(dataWidgets.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
+ connect(dataWidgets.vertexTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetVerticesCustomContextMenu);
+ connect(dataWidgets.edgeTable, &QTableWidget::cellDoubleClicked, this, &This::edgeTableDoubleClicked);
+ connect(dataWidgets.edgeTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetEdgesCustomContextMenu);
+
+ connect(widget.btnAdd, &QPushButton::clicked, this, &This::addNewGraphVertex);
+ connect(widget.btnAddEdge, &QPushButton::clicked, this, &This::addNewEdgeBoth);
+ connect(widget.btnAddEdgeStartEnd, &QPushButton::clicked, this, &This::addNewEdgeStartEnd);
+ connect(widget.btnAddEdgeEndStart, &QPushButton::clicked, this, &This::addNewEdgeEndStart);
+ connect(widget.btnEdit, &QPushButton::clicked, this, &This::editGraphVertex);
+#endif
+
+ // Zoom
+ connect(widget.viewZoomFactor, QOverload<double>::of(&QDoubleSpinBox::valueChanged), this, &This::transformView);
+
+#if 0
+ // Rotation
+ connect(widget.buttonRotateClock, &QPushButton::clicked, this, &This::viewRotatedClock);
+ connect(widget.buttonRotateCounterClock, &QPushButton::clicked, this, &This::viewRotatedCounterClock);
+
+ // Redraw+clear
+ connect(widget.buttonRedraw, &QPushButton::clicked, this, &This::redraw);
+ connect(widget.buttonClear, &QPushButton::clicked, this, &This::clearGraph);
+
+ // Auto adjust view
+ connect(widget.buttonAutoAdjust, &QPushButton::clicked, this, &This::adjustView);
+#endif
+
+ connect(view.scene, &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
+
+
+ // This -> This
+
+ // Intra-connections.
+ connect(this, &This::connected, this, &This::queryGraphs);
+ connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
+
+ }
+
+
+ void LocationGraphEditorWidgetController::GraphView::setScene(GraphScene* scene)
+ {
+ this->scene = scene;
+ this->view->setScene(this->scene);
}
@@ -166,59 +231,57 @@ namespace armarx
QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
{
- if (!dialog)
+ if (not dialog)
{
dialog = new SimpleConfigDialog(parent);
+ dialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
}
-
- dialog->addProxyFinder<armarx::armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", memoryNameSystemName);
- dialog->addProxyFinder<memoryx::PriorKnowledgeInterfacePrx>("PriorKnowledge", "Prior Knowledge", priorKnowledgeProxyName);
-
return qobject_cast<SimpleConfigDialog*>(dialog);
}
void LocationGraphEditorWidgetController::configured()
{
- memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
- priorKnowledgeProxyName = dialog->getProxyName("PriorKnowledge");
+ remote.memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
}
void LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
{
- memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(memoryNameSystemName)).toString().toStdString();
- priorKnowledgeProxyName = settings->value("priorKnowledgeProxyName", QString::fromStdString(debugDrawerLayerName)).toString().toStdString();
+ remote.memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName)).toString().toStdString();
}
void LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("memoryNameSystemName", QString::fromStdString(memoryNameSystemName));
- settings->setValue("priorKnowledgeProxyName", QString::fromStdString(priorKnowledgeProxyName));
+ settings->setValue("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName));
+ }
+
+
+ void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ {
+ lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
+ }
+
+
+ void LocationGraphEditorWidgetController::saveAutomaticSettings()
+ {
+ settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
}
void LocationGraphEditorWidgetController::onInitComponent()
{
- usingProxy(priorKnowledgeProxyName);
- usingProxy("GraphNodePoseResolver");
- offeringTopic(debugDrawerTopicName);
+ usingProxy(remote.memoryNameSystemName);
}
void LocationGraphEditorWidgetController::onConnectComponent()
{
- debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
-
- debugDrawer = getTopic<armarx::DebugDrawerInterfacePrx>(debugDrawerTopicName);
- priorKnowledgePrx = getProxy<memoryx::PriorKnowledgeInterfacePrx>(priorKnowledgeProxyName);
- getProxy(gnpr, "GraphNodePoseResolver");
+ remote.connect(*this);
- if (priorKnowledgePrx->hasGraphSegment())
+ if (/* DISABLES CODE */ (true))
{
- ARMARX_VERBOSE << "get Proxy to graph segment";
- graphSeg = priorKnowledgePrx->getGraphSegment();
widget.sceneGroupBox->setEnabled(true);
widget.sceneGroupBox->setTitle("Scenes from graph memory segment");
}
@@ -228,435 +291,332 @@ namespace armarx
widget.sceneGroupBox->setTitle("Scenes from graph memory segment (No graph memory segment available)");
}
- widget.tableWidgetNodes->horizontalHeader()->setResizeMode(QHeaderView::ResizeToContents);
- widget.tableWidgetEdges->horizontalHeader()->setResizeMode(QHeaderView::Stretch);
-
- widget.tableWidgetNodes->setContextMenuPolicy(Qt::CustomContextMenu);
- widget.tableWidgetEdges->setContextMenuPolicy(Qt::CustomContextMenu);
-
-
- // tables
- QObject::connect(widget.tableWidgetNodes, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(nodeTableDoubleClicked(int, int)), Qt::UniqueConnection);
- QObject::connect(widget.tableWidgetNodes, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetNodesCustomContextMenu(QPoint)), Qt::UniqueConnection);
- QObject::connect(widget.tableWidgetEdges, SIGNAL(cellDoubleClicked(int, int)), this, SLOT(edgeTableDoubleClicked(int, int)), Qt::UniqueConnection);
- QObject::connect(widget.tableWidgetEdges, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(tableWidgetEdgesCustomContextMenu(QPoint)), Qt::UniqueConnection);
-
- QObject::connect(widget.btnAdd, SIGNAL(clicked()), this, SLOT(addNewGraphNode()), Qt::UniqueConnection);
- QObject::connect(widget.btnAddEdge, SIGNAL(clicked()), this, SLOT(addNewEdgeBoth()), Qt::UniqueConnection);
- QObject::connect(widget.btnAddEdgeStartEnd, SIGNAL(clicked()), this, SLOT(addNewEdgeStartEnd()), Qt::UniqueConnection);
- QObject::connect(widget.btnAddEdgeEndStart, SIGNAL(clicked()), this, SLOT(addNewEdgeEndStart()), Qt::UniqueConnection);
- QObject::connect(widget.btnEdit, SIGNAL(clicked()), this, SLOT(editGraphNode()), Qt::UniqueConnection);
-
- // zoom
- QObject::connect(widget.viewZoomFactor, SIGNAL(valueChanged(double)), this, SLOT(transformView()), Qt::UniqueConnection);
- // rota
- QObject::connect(widget.buttonRotateClock, SIGNAL(clicked()), this, SLOT(viewRotatedClock()), Qt::UniqueConnection);
- QObject::connect(widget.buttonRotateCounterClock, SIGNAL(clicked()), this, SLOT(viewRotatedCounterClock()), Qt::UniqueConnection);
- // redraw+clear
- QObject::connect(widget.buttonRedraw, SIGNAL(clicked()), this, SLOT(redraw()), Qt::UniqueConnection);
- QObject::connect(widget.buttonClear, SIGNAL(clicked()), this, SLOT(clearGraph()), Qt::UniqueConnection);
- // auto adjust
- QObject::connect(widget.buttonAutoAdjust, SIGNAL(clicked()), this, SLOT(adjustView()), Qt::UniqueConnection);
- // memory
- QObject::connect(widget.refreshScenesButton, SIGNAL(clicked()), this, SLOT(updateSceneList()), Qt::UniqueConnection);
- QObject::connect(widget.drawSceneButton, SIGNAL(clicked()), this, SLOT(drawScene()), Qt::UniqueConnection); // BUTTON LOAD
- QObject::connect(widget.scenesComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(selectedSceneChanged(int)), Qt::UniqueConnection);
-
- ARMARX_VERBOSE << "connected";
-
- updateSceneList();
+ emit connected();
}
- void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ void LocationGraphEditorWidgetController::Remote::connect(Component& parent)
{
- lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
- }
+ auto mnsProxy = parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
+ memoryNameSystem = armem::client::MemoryNameSystem(mnsProxy, &parent);
+ locationReader = memoryNameSystem.useReader(nav::loc::coreSegmentID);
+ locationWriter = memoryNameSystem.useWriter(nav::loc::coreSegmentID);
- void LocationGraphEditorWidgetController::saveAutomaticSettings()
- {
- settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
+ graphReader = memoryNameSystem.useReader(nav::graph::coreSegmentID);
+ graphWriter = memoryNameSystem.useWriter(nav::graph::coreSegmentID);
}
- void LocationGraphEditorWidgetController::addEdge(const std::string& node1Id, const std::string& node2Id)
+ void LocationGraphEditorWidgetController::queryGraphs()
{
- if (!hasNode(node1Id))
+ armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
+ if (result.success)
{
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node1Id << " does not exist.";
- return;
+ data.memory = std::move(result.memory);
+ emit memoryDataChanged();
}
-
- if (!hasNode(node2Id))
+ else
{
- ARMARX_WARNING << "Edge: " << node1Id << ", " << node2Id << " can't be created! Node " << node2Id << " does not exist.";
- return;
+ widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
}
+ }
- 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;
- }
+ void LocationGraphEditorWidgetController::updateGraphList()
+ {
+ widget.graphsComboBox->clear();
- auto edgeId = toEdge(node1Id, node2Id);
-
- // add
- // table
- int row = widget.tableWidgetEdges->rowCount();
- widget.tableWidgetEdges->setRowCount(row + 1);
- widget.tableWidgetEdges->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node1dat.node->getName())});
- widget.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
+ data.memory.forEachEntity([this](const armem::wm::Entity& entity)
{
- *this, edgeId,
- node1dat.pose->position->x, -node1dat.pose->position->y,
- node2dat.pose->position->x, -node2dat.pose->position->y
+ widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
});
- // 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);
}
- void LocationGraphEditorWidgetController::addNode(const memoryx::GraphNodeBasePtr& node)
+ void LocationGraphEditorWidgetController::loadGraph()
{
- ARMARX_CHECK_EXPRESSION(node);
- auto nodeId = node->getId();
+ // Store vertex highlighting.
+ std::vector<semrel::ShapeID> highlightedVertices;
+ for (auto vertex : data.graph.vertices())
+ {
+ if (vertex.attrib().highlighted)
+ {
+ highlightedVertices.push_back(vertex.objectID());
+ }
+ }
- armarx::FramedPosePtr globalNodePose;
- try
+ const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
+ const armem::wm::EntityInstance* instance = data.memory.findLatestInstance(entityID);
+ if (not instance)
{
- globalNodePose = armarx::FramedPosePtr::dynamicCast(gnpr->resolveToGlobalPose(node));
+ std::stringstream ss;
+ ss << "No latest instance of entity " << entityID << " in memory.";
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
}
- catch (...)
+
+ nav::graph::Graph nav;
{
- return;
+ nav::graph::arondto::Graph dto;
+ dto.fromAron(instance->data());
+ fromAron(dto, nav);
+ }
+
+ clearGraph();
+
+ // Add vertices
+ for (auto vertex : nav.vertices())
+ {
+ addVertex(vertex);
}
- if (hasNode(nodeId))
+ // Add edges
+ for (auto edge : nav.edges())
{
- NodeData& oldNode = nodes.at(nodeId);
- ARMARX_VERBOSE << "Node: " << nodeId << " was overwritten! Old: "
- << oldNode.pose->position->x << ", " << oldNode.pose->position->y << ", " << getYawAngle(oldNode.pose) << "| New: "
- << globalNodePose->position->x << ", " << globalNodePose->position->y << ", " << getYawAngle(globalNodePose);
- // update node data
- // table
- widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
- widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
- widget.tableWidgetNodes->setItem(oldNode.tableWidgetNodesIndex, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
-
- // data
- oldNode.pose = globalNodePose;
-
- // update connected edges
- for (const auto& edge : edges)
+ addEdge(edge);
+ }
+
+ // Restore vertex highlighting.
+ for (const semrel::ShapeID& vertexID : highlightedVertices)
+ {
+ for (auto vertex : data.graph.vertices())
{
- if ((edge.first.first == nodeId) || (edge.first.second == nodeId))
+ if (vertex.objectID() == vertexID)
{
- updateEdge(edge.first);
+ vertex.attrib().highlighted = true;
+ updateVertex(vertex);
}
}
}
- else
- {
- // add&draw node
- // table
- int row = widget.tableWidgetNodes->rowCount();
- widget.tableWidgetNodes->setRowCount(row + 1);
- widget.tableWidgetNodes->setItem(row, 0, new QTableWidgetItem {QString::fromStdString(node->getName())});
- widget.tableWidgetNodes->setItem(row, 1, new QTableWidgetItem {QString::number(globalNodePose->position->x)});
- widget.tableWidgetNodes->setItem(row, 2, new QTableWidgetItem {QString::number(globalNodePose->position->y)});
- widget.tableWidgetNodes->setItem(row, 3, new QTableWidgetItem {QString::number(getYawAngle(globalNodePose))});
- // scene
- QGraphicsEllipseItem* graphicsItem = dynamic_cast<QGraphicsEllipseItem*>(new GraphVisualizerGraphicsEllipseItem
- {
- *this, node->getName(),
- globalNodePose->position->x, -globalNodePose->position->y, 0, 0
- });
- // auto graphicsItem= scene->addEllipse(node->x,-node->y,0,0);
- scene->addItem(graphicsItem);
- // setToolTip on graphicsItem does not work
- graphicsItem->setZValue(std::numeric_limits<qreal>::max());
- dynamic_cast<QGraphicsItem*>(graphicsItem)->setToolTip(QString {"Node:"} +QString::fromStdString(node->getName()));
-
- // data
- NodeData data {node, globalNodePose, graphicsItem, row, false};
- nodes.insert({nodeId, data});
- }
- updateNode(nodeId);
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::clearEdges()
+ GuiGraph::Vertex
+ LocationGraphEditorWidgetController::addVertex(graph::Graph::Vertex vertex)
{
- for (auto& edge : edges)
- {
- // remove from graphics scene
- scene->removeItem(edge.second.graphicsItem);
- delete edge.second.graphicsItem;
- // remove from debug layer
- debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(edge.first));
- }
+ ARMARX_CHECK(not data.graph.hasVertex(vertex.objectID()));
+
+ VertexData attrib { vertex.attrib() };
+ attrib.graphicsItem = view.scene->addVertex(vertex);
+ attrib.tableWidgetItem = dataWidgets.vertexTable->addVertex(vertex);
+ auto guiVertex = data.graph.addVertex(vertex.objectID(), attrib);
- // clear table widget
- widget.tableWidgetEdges->clearContents();
- widget.tableWidgetEdges->setRowCount(0);
- // clear data structures
- edges.clear();
+ updateVertex(guiVertex);
+ return guiVertex;
}
- void LocationGraphEditorWidgetController::clearGraph()
+ GuiGraph::Edge
+ LocationGraphEditorWidgetController::addEdge(graph::Graph::Edge edge)
{
- // remove from debug layer
- for (auto& edge : edges)
- {
- debugDrawer->removeLineVisu(debugDrawerLayerName, iceName(edge.first));
- }
+ ARMARX_CHECK(not data.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
+ << "Edge must not exist before being added: '"
+ << edge.source().attrib().getName() << "' -> '"
+ << edge.target().attrib().getName() << "'";
- for (auto& node : nodes)
- {
- debugDrawer->removeArrowVisu(debugDrawerLayerName, iceName(node.first));
- debugDrawer->removeTextVisu(debugDrawerLayerName, iceName(node.first) + "text");
- debugDrawer->removePoseVisu(debugDrawerLayerName, iceName(node.first));
- }
+ auto source = data.graph.vertex(edge.sourceObjectID());
+ auto target = data.graph.vertex(edge.targetObjectID());
+
+ EdgeData attrib { edge.attrib() };
+ attrib.tableWidgetItem = dataWidgets.edgeTable->addEdge(edge);
+ attrib.graphicsItem = view.scene->addEdge(edge);
+ auto guiEdge = data.graph.addEdge(source, target, attrib);
- // clear scene
- scene->clear();
- // clear table widgets
- widget.tableWidgetEdges->clearContents();
- widget.tableWidgetEdges->setRowCount(0);
- widget.tableWidgetNodes->clearContents();
- widget.tableWidgetNodes->setRowCount(0);
- // clear data structures
-
- edges.clear();
- nodes.clear();
+ updateEdge(guiEdge);
+ return guiEdge;
}
- void LocationGraphEditorWidgetController::resetHighlight()
+ void LocationGraphEditorWidgetController::updateVertex(GuiGraph::Vertex vertex)
{
- for (auto& edge : edges)
+ view.scene->updateVertex(vertex);
+ dataWidgets.vertexTable->updateVertex(vertex);
+
+ setEditFields(vertex);
+
{
- if (edge.second.highlighted)
+ // Highlight all edges between highlighted vertices
+ std::set<semrel::ShapeID> highlightedVertices;
+ for (auto v : data.graph.vertices())
{
- edge.second.highlighted = false;
- updateEdge(edge.first);
+ if (v.attrib().highlighted)
+ {
+ highlightedVertices.insert(v.objectID());
+ }
}
- }
- for (auto& node : nodes)
- {
- if (node.second.highlighted)
+ for (auto edge : data.graph.edges())
{
- node.second.highlighted = false;
- updateNode(node.first);
+ bool verticesHighlighted =
+ highlightedVertices.count(edge.sourceObjectID())
+ and highlightedVertices.count(edge.targetObjectID());
+
+ // Already highlighted but vertices not highlighted => to false, update
+ // Not highlighted but vertices highlighted => to true, update
+ if (edge.attrib().highlighted != verticesHighlighted)
+ {
+ edge.attrib().highlighted = verticesHighlighted;
+ updateEdge(edge);
+ }
}
}
+
+ // ToDo: Update ArViz
+#if 0
+ float yaw = getYawAngleDegree(attrib.pose) / 180 * M_PI;
+ Eigen::AngleAxisf aa(yaw, Eigen::Vector3f(0, 0, 1));
+ Eigen::Vector3f dir {0, 1, 0};
+ dir = aa.toRotationMatrix() * dir;
+ debugDrawer->setArrowVisu(debugDrawerLayerName, iceName(id), attrib.pose->position,
+ new armarx::Vector3(dir),
+ armarx::DrawColor {0, 0, 1, 1},
+ 100,
+ lineWidth);
+ debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", attrib.vertex->getName(),
+ attrib.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
+#endif
}
- void LocationGraphEditorWidgetController::updateEdge(const EdgeId& id)
+ void LocationGraphEditorWidgetController::updateEdge(GuiGraph::Edge edge)
{
- const EdgeData& data = edges.at(id);
- auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
- QColor qColor;
- qColor.setRedF(color.r);
- qColor.setGreenF(color.g);
- qColor.setBlueF(color.b);
-
- auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
- armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.first).pose->position)->toEigen());
+ view.scene->updateEdge(edge);
+ dataWidgets.edgeTable->updateEdge(edge);
+
+#if 0
+ armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.first).pose->position)->toEigen());
posStart->z += posStart->z < 1 ? 10 : 0;
- armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(nodes.at(id.second).pose->position)->toEigen());
+ armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.attrib()).pose->position)->toEigen());
posEnd->z += posEnd->z < 1 ? 10 : 0;
// debug layer
+ // ToDo: Update ArViz
debugDrawer->setLineVisu(debugDrawerLayerName,
iceName(id),
posStart,
posEnd,
lineWidth,
color);
-
- data.graphicsItem->setLine(nodes[id.first].pose->position->x, -nodes[id.first].pose->position->y,
- nodes[id.second].pose->position->x, -nodes[id.second].pose->position->y);
-
- // scene
- QPen pen {qColor};
- pen.setWidthF(lineWidth * SCENE_LINE_SCALE_FACTOR);
- data.graphicsItem->setPen(pen);
- // table
- QFont font {};
- font.setBold(data.highlighted);
- widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setData(Qt::BackgroundRole, qColor);
- widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 0)->setFont(font);
- widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setData(Qt::BackgroundRole, qColor);
- widget.tableWidgetEdges->item(data.tableWidgetEdgesIndex, 1)->setFont(font);
+#endif
}
- void LocationGraphEditorWidgetController::updateNode(const NodeId& id)
+ void LocationGraphEditorWidgetController::clearEdges()
{
- NodeData& data = nodes.at(id);
-
- if (editStartNodeNext)
+ // Remove from graphics scene
+ for (auto edge : data.graph.edges())
{
- widget.editStartNodeId->setText(QString::fromStdString(data.node->getId()));
- widget.editStartNodeName->setText(QString::fromStdString(data.node->getName()));
+ view.scene->removeEdge(edge.attrib().graphicsItem);
}
- else
+
+ // Clear table widget
+ dataWidgets.edgeTable->clearContents();
+ dataWidgets.edgeTable->setRowCount(0);
+
+ // Clear data structure
+ while (data.graph.edges().begin() != data.graph.edges().end())
{
- widget.editEndNodeId->setText(QString::fromStdString(data.node->getId()));
- widget.editEndNodeName->setText(QString::fromStdString(data.node->getName()));
+ data.graph.removeEdge(*data.graph.edges().begin());
}
- setEditFields(data);
+ // ToDo: Update ArViz
+ }
- editStartNodeNext = !editStartNodeNext;
- auto color = (data.highlighted) ? COLOR_HIGHLIGHT : COLOR_DEFAULT;
- QColor qColor;
- qColor.setRedF(color.r);
- qColor.setGreenF(color.g);
- qColor.setBlueF(color.b);
+ void LocationGraphEditorWidgetController::clearGraph()
+ {
+ // Clear scene
+ view.scene->clear();
- auto lineWidth = (data.highlighted) ? LINE_WIDTH_SELECTED : LINE_WIDTH_DEFAULT;
+ // Clear table widgets
+ dataWidgets.edgeTable->clearContents();
+ dataWidgets.edgeTable->setRowCount(0);
+ dataWidgets.vertexTable->clearContents();
+ dataWidgets.vertexTable->setRowCount(0);
- // debug layer
- float yaw = getYawAngle(data.pose) / 180 * M_PI;
- Eigen::AngleAxisf aa(yaw, Eigen::Vector3f(0, 0, 1));
- Eigen::Vector3f dir {0, 1, 0};
- dir = aa.toRotationMatrix() * dir;
- debugDrawer->setArrowVisu(debugDrawerLayerName, iceName(id), data.pose->position,
- new armarx::Vector3(dir),
- armarx::DrawColor {0, 0, 1, 1},
- 100,
- lineWidth);
- debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", data.node->getName(), data.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
-
- // scene
- data.graphicsItem->setPen(QPen {qColor});
- data.graphicsItem->setBrush(QBrush {qColor});
- data.graphicsItem->setRect(data.pose->position->x - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
- -data.pose->position->y - lineWidth * SCENE_NODES_SCALE_FACTOR / 2,
- lineWidth * SCENE_NODES_SCALE_FACTOR,
- lineWidth * SCENE_NODES_SCALE_FACTOR);
- // table
- QFont font {};
- font.setBold(data.highlighted);
- for (int i = 0; i <= 3; ++i)
- {
- widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setData(Qt::BackgroundRole, qColor);
- widget.tableWidgetNodes->item(data.tableWidgetNodesIndex, i)->setFont(font);
- }
+ // Clear data structure
+ data.graph.clear();
+ }
+
+
+ void LocationGraphEditorWidgetController::toggleVertexSelected(const semrel::ShapeID& vertexID)
+ {
+ auto vertex = data.graph.vertex(vertexID);
+ vertex.attrib().highlighted ^= true;
+ updateVertex(vertex);
+ }
- // highlight all edges between highlighted nodes
- std::vector<memoryx::GraphNodeBasePtr> highlightedNodes;
- for (const auto& nodeData : nodes)
- {
- if (nodeData.second.highlighted)
- {
- highlightedNodes.push_back(nodeData.second.node);
- }
- }
- for (auto& edge : edges)
+
+ void LocationGraphEditorWidgetController::resetHighlighting()
+ {
+ for (auto vertex : data.graph.vertices())
{
- if (edge.second.highlighted)
+ if (vertex.attrib().highlighted)
{
- edge.second.highlighted = false;
- updateEdge(edge.first);
+ vertex.attrib().highlighted = false;
+ updateVertex(vertex);
}
}
-
- for (const auto& nodeFrom : highlightedNodes)
+ for (auto edge : data.graph.edges())
{
- for (const auto& nodeTo : highlightedNodes)
+ if (edge.attrib().highlighted)
{
- const auto nodeFromId = nodeFrom->getId();
- const auto nodeToId = nodeTo->getId();
- if (hasEdge(nodeFromId, nodeToId))
- {
- auto edgeId = toEdge(nodeFromId, nodeToId);
- auto& edge = edges[edgeId];
- edge.highlighted = true;
- updateEdge(edgeId);
- }
+ edge.attrib().highlighted = false;
+ updateEdge(edge);
}
}
}
- float LocationGraphEditorWidgetController::getYawAngle(const armarx::PoseBasePtr& pose) const
- {
- Eigen::Vector3f rpy;
- armarx::PosePtr p = armarx::PosePtr::dynamicCast(pose);
- VirtualRobot::MathTools::eigen4f2rpy(p->toEigen(), rpy);
- return VirtualRobot::MathTools::rad2deg(rpy[2]);
- }
+#if 0
- void LocationGraphEditorWidgetController::highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted)
+
+ void LocationGraphEditorWidgetController::highlightVertex(const std::string& vertexId, bool highlighted)
{
- if (!hasEdge(node1Id, node2Id))
+ if (!hasVertex(vertexId))
{
- ARMARX_WARNING << "No edge for: " << node1Id << " and " << node2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ ARMARX_WARNING << "No vertex: " << vertexId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
return;
}
- EdgeId edge = toEdge(node1Id, node2Id);
-
- if (edges.at(edge).highlighted != highlighted)
+ if (vertices.at(vertexId).highlighted != highlighted)
{
- edges.at(edge).highlighted = highlighted;
- updateEdge(edge);
+ vertices.at(vertexId).highlighted = highlighted;
+ updateVertex(vertexId);
}
}
- void LocationGraphEditorWidgetController::highlightNode(const std::string& nodeId, bool highlighted)
+ void LocationGraphEditorWidgetController::highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted)
{
- if (!hasNode(nodeId))
+ if (!hasEdge(vertex1Id, vertex2Id))
{
- ARMARX_WARNING << "No node: " << nodeId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
+ ARMARX_WARNING << "No edge for: " << vertex1Id << " and " << vertex2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
return;
}
- if (nodes.at(nodeId).highlighted != highlighted)
+ EdgeId edge = toEdge(vertex1Id, vertex2Id);
+
+ if (edges.at(edge).highlighted != highlighted)
{
- nodes.at(nodeId).highlighted = highlighted;
- updateNode(nodeId);
+ edges.at(edge).highlighted = highlighted;
+ updateEdge(edge);
}
}
- void LocationGraphEditorWidgetController::nodeTableDoubleClicked(int row, int)
+ void LocationGraphEditorWidgetController::vertexTableDoubleClicked(int row, int)
{
- auto nodeIt = std::find_if(nodes.cbegin(), nodes.cend(), [&](const std::pair<std::string, NodeData>& d)
+ auto vertexIt = std::find_if(vertices.cbegin(), vertices.cend(), [&](const std::pair<std::string, VertexData>& d)
{
- // return d.second.node->getName() == widget.tableWidgetNodes->item(row, 0)->text().toStdString();
- return d.second.tableWidgetNodesIndex == row;
+ // return d.attrib().vertex->getName() == dataWidgets.vertexTable->item(row, 0)->text().toStdString();
+ return d.attrib().tableWidgetVerticesIndex == row;
});
- auto nodeId = nodeIt->second.node->getId();
- nodeDoubleClicked(nodeId);
+ auto vertexId = vertexIt->second.vertex->getId();
+ highlightVertex(vertexId);
}
@@ -664,7 +624,7 @@ namespace armarx
{
auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
{
- return d.second.tableWidgetEdgesIndex == row;
+ return d.attrib().tableWidgetEdgesIndex == row;
});
edgeDoubleClicked(edgeIt->first);
@@ -672,65 +632,65 @@ namespace armarx
- void LocationGraphEditorWidgetController::nodeDoubleClicked(NodeId id)
- {
- nodes.at(id).highlighted ^= true;
- updateNode(id);
- }
-
-
- void LocationGraphEditorWidgetController::edgeDoubleClicked(EdgeId id)
+ void LocationGraphEditorWidgetController::edgeDoubleClicked(GuiGraph::Edge edge)
{
// edges.at(id).highlighted ^= true;
// updateEdge(id);
- bool highlight = !nodes.at(id.first).highlighted && !nodes.at(id.second).highlighted;
- nodes.at(id.first).highlighted = highlight;
- nodes.at(id.second).highlighted = highlight;
- updateNode(id.first);
- updateNode(id.second);
+ bool highlight = !vertices.at(edge.first).highlighted && !vertices.at(edge.attrib()).highlighted;
+ vertices.at(edge.first).highlighted = highlight;
+ vertices.at(edge.attrib()).highlighted = highlight;
+ updateVertex(edge.first);
+ updateVertex(edge.attrib());
}
void LocationGraphEditorWidgetController::redraw()
{
- drawScene();
+ loadGraph();
}
+#endif
- void LocationGraphEditorWidgetController::setEditFields(const NodeData& nodeData)
+ void LocationGraphEditorWidgetController::setEditFields(GuiGraph::Vertex vertex)
{
- widget.editNodeId->setText(QString::fromStdString(nodeData.node->getId()));
- widget.editSceneName->setText(QString::fromStdString(nodeData.node->getScene()));
- widget.editNodeName->setText(QString::fromStdString(nodeData.node->getName()));
- widget.editFrameName->setText(QString::fromStdString(nodeData.pose->frame));
- widget.editAgentName->setText(QString::fromStdString(nodeData.pose->agent));
+#if 0
+ widget.editVertexId->setText(QString::fromStdString(vertexData.vertex->getId()));
+ widget.editSceneName->setText(QString::fromStdString(vertexData.vertex->getScene()));
+ widget.editVertexName->setText(QString::fromStdString(vertexData.vertex->getName()));
+ widget.editFrameName->setText(QString::fromStdString(vertexData.pose->frame));
+ widget.editAgentName->setText(QString::fromStdString(vertexData.pose->agent));
Eigen::Vector3f rpy;
- VirtualRobot::MathTools::eigen4f2rpy(nodeData.pose->toEigen(), rpy);
- widget.spinBoxX->setValue(nodeData.pose->position->x);
- widget.spinBoxY->setValue(nodeData.pose->position->y);
- widget.spinBoxZ->setValue(nodeData.pose->position->z);
+ VirtualRobot::MathTools::eigen4f2rpy(vertexData.pose->toEigen(), rpy);
+ widget.spinBoxX->setValue(vertexData.pose->position->x);
+ widget.spinBoxY->setValue(vertexData.pose->position->y);
+ widget.spinBoxZ->setValue(vertexData.pose->position->z);
widget.spinBoxRoll->setValue(VirtualRobot::MathTools::rad2deg(rpy[0]));
widget.spinBoxPitch->setValue(VirtualRobot::MathTools::rad2deg(rpy[1]));
widget.spinBoxYaw->setValue(VirtualRobot::MathTools::rad2deg(rpy[2]));
+#endif
}
-
+#if 0
void LocationGraphEditorWidgetController::refreshGraph()
{
clearGraph();
- drawScene();
+ loadGraph();
}
+#endif
void LocationGraphEditorWidgetController::transformView()
{
double d = widget.viewZoomFactor->value();
- widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(viewAngle));
+ widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(view.angle));
}
+#if 0
+
+
void LocationGraphEditorWidgetController::viewRotatedClock()
{
viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
@@ -743,22 +703,23 @@ namespace armarx
viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
transformView();
}
-
+#endif
void LocationGraphEditorWidgetController::adjustView()
{
+#if 0
float maxX = std::numeric_limits<float>::min();
float minX = std::numeric_limits<float>::max();
float maxY = std::numeric_limits<float>::min();
float minY = std::numeric_limits<float>::max();
// search bounding box
- for (const auto& node : nodes)
+ for (const auto& vertex : vertices)
{
- maxX = (maxX < node.second.pose->position->x) ? node.second.pose->position->x : maxX;
- minX = (minX > node.second.pose->position->x) ? node.second.pose->position->x : minX;
- maxY = (maxY < node.second.pose->position->y) ? node.second.pose->position->y : maxY;
- minY = (minY > node.second.pose->position->y) ? node.second.pose->position->y : minY;
+ maxX = (maxX < vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : maxX;
+ minX = (minX > vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : minX;
+ maxY = (maxY < vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : maxY;
+ minY = (minY > vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : minY;
}
auto deltaX = maxX - minX; // >=0
@@ -781,35 +742,36 @@ namespace armarx
widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaY,
widget.graphicsViewGraph->height() / deltaX) * 0.9);
}
-
+#endif
}
+#if 0
bool LocationGraphEditorWidgetController::addNewEdge(const std::string& fromId, const std::string& toId)
{
std::string errorMsg;
if (!graphSeg->hasEntityById(fromId))
{
- errorMsg = "start node with Id '" + fromId + "' not found in segment";
+ errorMsg = "start vertex with Id '" + fromId + "' not found in segment";
}
else if (!graphSeg->hasEntityById(toId))
{
- errorMsg = "end node with Id '" + toId + "' not found in segment";
+ errorMsg = "end vertex with Id '" + toId + "' not found in segment";
}
else if (fromId == toId)
{
- errorMsg = "starting and ending node are the same";
+ errorMsg = "starting and ending vertex are the same";
}
else
{
- auto fromNode = graphSeg->getNodeById(fromId);
- for (const auto& adjacent : fromNode->getAdjacentNodes())
+ auto fromVertex = graphSeg->getVertexById(fromId);
+ for (const auto& adjacent : fromVertex->getAdjacentVertices())
{
if (toId == adjacent->getId())
{
- errorMsg = "edge '" + fromNode->getName() + "' -> '" + adjacent->getName() + "' already exists";
+ errorMsg = "edge '" + fromVertex->getName() + "' -> '" + adjacent->getName() + "' already exists";
break;
}
}
@@ -819,11 +781,11 @@ namespace armarx
{
widget.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
graphSeg->addEdge(fromId, toId);
- gnpr->forceRefetch(fromId);
- gnpr->forceRefetch(toId);
+ graphVertexPoseResolver->forceRefetch(fromId);
+ graphVertexPoseResolver->forceRefetch(toId);
addEdge(fromId, toId);
- updateNode(fromId);
- updateNode(toId);
+ updateVertex(fromId);
+ updateVertex(toId);
widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
}
else
@@ -839,157 +801,29 @@ namespace armarx
void LocationGraphEditorWidgetController::addNewEdgeBoth()
{
- std::string startId = widget.editStartNodeId->text().toStdString();
- std::string endId = widget.editEndNodeId->text().toStdString();
+ std::string startId = widget.editStartVertexId->text().toStdString();
+ std::string endId = widget.editEndVertexId->text().toStdString();
addNewEdge(startId, endId);
addNewEdge(endId, startId);
}
+
void LocationGraphEditorWidgetController::addNewEdgeStartEnd()
{
- std::string startId = widget.editStartNodeId->text().toStdString();
- std::string endId = widget.editEndNodeId->text().toStdString();
+ std::string startId = widget.editStartVertexId->text().toStdString();
+ std::string endId = widget.editEndVertexId->text().toStdString();
addNewEdge(startId, endId);
}
void LocationGraphEditorWidgetController::addNewEdgeEndStart()
{
- std::string startId = widget.editEndNodeId->text().toStdString();
- std::string endId = widget.editStartNodeId->text().toStdString();
+ std::string startId = widget.editEndVertexId->text().toStdString();
+ std::string endId = widget.editStartVertexId->text().toStdString();
addNewEdge(startId, endId);
}
- void LocationGraphEditorWidgetController::selectedSceneChanged(int i)
- {
- const auto current = widget.scenesComboBox->currentText();
- if (!current.isEmpty())
- {
- lastSelectedSceneName = current;
- }
- }
-
-
- void LocationGraphEditorWidgetController::updateSceneList()
- {
- auto scenes = graphSeg->getScenes();
- widget.scenesComboBox->clear();
- int idx = -1;
-
- for (std::size_t i = 0; i < scenes.size(); i++)
- {
- const auto currentScene = QString::fromStdString(scenes[i]);
- widget.scenesComboBox->addItem(currentScene);
-
- if (currentScene == lastSelectedSceneName)
- {
- idx = i;
- }
- }
-
- widget.scenesComboBox->setCurrentIndex(idx);
- }
-
-
- void LocationGraphEditorWidgetController::drawScene()
- {
- std::vector<std::string> highlightedNodes;
- for (const auto& nodeData : nodes)
- {
- bool sameScene = widget.scenesComboBox->currentText().toStdString() == nodeData.second.node->getScene();
- if (nodeData.second.highlighted && sameScene)
- {
- highlightedNodes.push_back(nodeData.first);
- }
- }
-
- clearGraph();
- auto graphNodes = graphSeg->getNodesByScene(widget.scenesComboBox->currentText().toStdString());
-
- // add nodes
- for (auto& node : graphNodes)
- {
- auto pos = armarx::FramedPosePtr::dynamicCast(node->getPose());
-
- if (!pos || node->isMetaEntity())
- {
- continue;
- }
-
- addNode(node);
- }
-
- // add edges
- for (auto& node : graphNodes)
- {
- auto nodeId = node->getId();
-
- for (int i = 0; i < node->getOutdegree(); i++)
- {
- auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
- ARMARX_CHECK_EXPRESSION(adjacent);
- auto adjacentId = adjacent->getId();
-
- addEdge(nodeId, adjacentId);
- }
- }
-
- for (const auto& nodeId : highlightedNodes)
- {
- auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
- {
- return d.first == nodeId;
- });
-
- if (nodeIt != nodes.end())
- {
- nodeIt->second.highlighted = true;
- updateNode(nodeIt->first);
- }
- }
-
- adjustView();
- }
-
-
- void LocationGraphEditorWidgetController::selectScene()
- {
- // QString fi = QFileDialog::getOpenFileName(this, tr("Open Scene File"), QString(), tr("XML Files (*.xml)"));
- // std::string xmlSceneFile = std::string(fi.toLatin1());
- // loadScene(xmlSceneFile);
- }
-
-
- void LocationGraphEditorWidgetController::loadScene(const std::string& xmlFile)
- {
- // VirtualRobot::ScenePtr SceneIO::loadScene(const std::string& xmlFile)
- // {
- // load file
- std::ifstream in(xmlFile.c_str());
-
- if (!in.is_open())
- {
- ARMARX_WARNING << "Could not open XML file:" << xmlFile;
- return;
- }
-
- std::stringstream buffer;
- buffer << in.rdbuf();
- std::string sceneXML(buffer.str());
- std::filesystem::path filenameBaseComplete(xmlFile);
- std::filesystem::path filenameBasePath = filenameBaseComplete.parent_path();
- std::string basePath = filenameBasePath.string();
-
- in.close();
-
- // VirtualRobot::ScenePtr res = createSceneFromString(robotXML, basePath);
- // THROW_VR_EXCEPTION_IF(!res, "Error while parsing file " << xmlFile);
-
- // return res;
- // }
- }
-
void LocationGraphEditorWidgetController::addKitchenGraph()
{
@@ -997,33 +831,33 @@ namespace armarx
graphSeg->clearScene(scene);
// if you insist on hardcoding scenes, use these convenience functions for improved readability:
- auto addNode = [&](const std::string & name, float x, float y, float angle)
+ auto addVertex = [&](const std::string & name, float x, float y, float angle)
{
- graphSeg->addNode(new ::memoryx::GraphNode {x, y, angle, name, scene});
- ARMARX_INFO_S << "added node '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
+ graphSeg->addVertex(new ::memoryx::GraphVertex {x, y, angle, name, scene});
+ ARMARX_INFO_S << "added vertex '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
};
- auto addEdges = [&](const std::string & nodeFromName, const std::string & nodeToName, bool bidirectional)
+ auto addEdges = [&](const std::string & vertexFromName, const std::string & vertexToName, bool bidirectional)
{
- auto nodeFrom = graphSeg->getNodeFromSceneByName(scene, nodeFromName);
- auto nodeTo = graphSeg->getNodeFromSceneByName(scene, nodeToName);
- ARMARX_CHECK_EXPRESSION(nodeFrom);
- ARMARX_CHECK_EXPRESSION(nodeTo);
- ARMARX_INFO_S << "'" << nodeFrom->getName() << "' -> '" << nodeTo->getName() << "', status: " << graphSeg->addEdge(nodeFrom->getId(), nodeTo->getId());
+ auto vertexFrom = graphSeg->getVertexFromSceneByName(scene, vertexFromName);
+ auto vertexTo = graphSeg->getVertexFromSceneByName(scene, vertexToName);
+ ARMARX_CHECK(vertexFrom);
+ ARMARX_CHECK(vertexTo);
+ ARMARX_INFO_S << "'" << vertexFrom->getName() << "' -> '" << vertexTo->getName() << "', status: " << graphSeg->addEdge(vertexFrom->getId(), vertexTo->getId());
if (bidirectional)
{
- ARMARX_INFO_S << "'" << nodeTo->getName() << "' -> '" << nodeFrom->getName() << "', status: " << graphSeg->addEdge(nodeTo->getId(), nodeFrom->getId());
+ ARMARX_INFO_S << "'" << vertexTo->getName() << "' -> '" << vertexFrom->getName() << "', status: " << graphSeg->addEdge(vertexTo->getId(), vertexFrom->getId());
}
};
// ex
- addNode("initialnode", 2900, 7000, 0);
- addNode("sideboard", 3400, 7000, 0);
- addEdges("initialnode", "sideboard", true);
+ addVertex("initialvertex", 2900, 7000, 0);
+ addVertex("sideboard", 3400, 7000, 0);
+ addEdges("initialvertex", "sideboard", true);
}
- void LocationGraphEditorWidgetController::addNewGraphNode()
+ void LocationGraphEditorWidgetController::addNewGraphVertex()
{
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
@@ -1031,28 +865,26 @@ namespace armarx
rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
- pos << widget.spinBoxX->value(),
- widget.spinBoxY->value(),
- widget.spinBoxZ->value();
+ pos << widget.spinBoxX->value(), widget.spinBoxY->value(), widget.spinBoxZ->value();
VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
widget.editFrameName->text().toStdString(),
widget.editAgentName->text().toStdString());
- memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
- widget.editNodeName->text().toStdString(),
+ memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
+ widget.editVertexName->text().toStdString(),
widget.editSceneName->text().toStdString());
- auto entityId = graphSeg->addNode(node);
- gnpr->forceRefetch(entityId);
- node->setId(entityId);
- if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ auto entityId = graphSeg->addVertex(vertex);
+ graphVertexPoseResolver->forceRefetch(entityId);
+ vertex->setId(entityId);
+ if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
{
- widget.editNodeId->setText(QString::fromStdString(entityId));
- addNode(node);
+ widget.editVertexId->setText(QString::fromStdString(entityId));
+ addVertex(vertex);
}
}
- void LocationGraphEditorWidgetController::editGraphNode()
+ void LocationGraphEditorWidgetController::editGraphVertex()
{
Eigen::Matrix4f mat;
Eigen::Vector3f rpy;
@@ -1067,96 +899,59 @@ namespace armarx
armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
widget.editFrameName->text().toStdString(),
widget.editAgentName->text().toStdString());
- memoryx::GraphNodePtr node = new memoryx::GraphNode(pose,
- widget.editNodeName->text().toStdString(),
+ memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
+ widget.editVertexName->text().toStdString(),
widget.editSceneName->text().toStdString());
- auto id = widget.editNodeId->text().toStdString();
+ auto id = widget.editVertexId->text().toStdString();
- node->setId(id);
- graphSeg->updateEntity(id, node);
- gnpr->forceRefetch(id);
- if (widget.scenesComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
+ vertex->setId(id);
+ graphSeg->updateEntity(id, vertex);
+ graphVertexPoseResolver->forceRefetch(id);
+ if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
{
- addNode(node);
+ addVertex(vertex);
}
}
- void LocationGraphEditorWidgetController::tableWidgetNodesCustomContextMenu(QPoint pos)
+ void LocationGraphEditorWidgetController::tableWidgetVerticesCustomContextMenu(QPoint pos)
{
- int row = widget.tableWidgetNodes->rowAt(pos.y());
- auto nodeIt = std::find_if(nodes.begin(), nodes.end(), [&](const std::pair<std::string, NodeData>& d)
+ int row = dataWidgets.vertexTable->rowAt(pos.y());
+ auto vertexIt = std::find_if(vertices.begin(), vertices.end(), [&](const std::pair<std::string, VertexData>& d)
{
- return d.second.tableWidgetNodesIndex == row;
+ return d.attrib().tableWidgetVerticesIndex == row;
});
QMenu menu;
- QAction* deleteAction = menu.addAction("Delete Node");
+ QAction* deleteAction = menu.addAction("Delete Vertex");
if (menu.exec(QCursor::pos()) == deleteAction)
{
- ARMARX_CHECK_EXPRESSION(nodeIt != nodes.end());
- graphSeg->removeNode(nodeIt->second.node->getId());
- drawScene();
+ ARMARX_CHECK(vertexIt != vertices.end());
+ graphSeg->removeVertex(vertexIt->second.vertex->getId());
+ loadGraph();
}
}
void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
{
- int row = widget.tableWidgetEdges->rowAt(pos.y());
+ int row = dataWidgets.edgeTable->rowAt(pos.y());
auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
{
- return d.second.tableWidgetEdgesIndex == row;
+ return d.attrib().tableWidgetEdgesIndex == row;
});
QMenu menu;
QAction* deleteAction = menu.addAction("Delete Edge");
if (menu.exec(QCursor::pos()) == deleteAction)
{
- ARMARX_CHECK_EXPRESSION(edgeIt != edges.end());
- graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.second);
- drawScene();
+ ARMARX_CHECK(edgeIt != edges.end());
+ graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.attrib());
+ loadGraph();
}
}
- bool MouseEventProcessor::eventFilter(QObject* obj, QEvent* event)
- {
- if (obj == gvw->widget.graphicsViewGraph && event->type() == QEvent::MouseButtonPress)
- {
- QMouseEvent* me = static_cast<QMouseEvent*>(event);
- if (me->button() == Qt::LeftButton)
- {
- QPointF scenePoint = gvw->widget.graphicsViewGraph->mapToScene(me->pos());
- scenePoint.setY(-scenePoint.y()); // not sure why
-
- float minDist = std::numeric_limits<float>::max();
- auto bestIt = gvw->nodes.cend();
-
- for (auto it = gvw->nodes.cbegin(); it != gvw->nodes.cend(); ++it)
- {
- float deltaX = it->second.pose->position->x - scenePoint.x();
- float deltaY = it->second.pose->position->y - scenePoint.y();
- float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
-
- if (dist < minDist)
- {
- minDist = dist;
- bestIt = it;
- }
- }
-
- if (bestIt != gvw->nodes.cend())
- {
- gvw->nodeDoubleClicked(bestIt->first);
- }
- }
- }
- else if (event->type() == QEvent::Resize)
- {
- gvw->adjustView();
- }
- return QObject::eventFilter(obj, event);
- }
+#endif
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index b87a5c2a..d0630335 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -21,18 +21,19 @@
*/
#pragma once
-#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-
+#include "FunctionalEventFilter.h"
+#include "GraphScene.h"
+#include "GuiGraph.h"
-#include <MemoryX/components/PriorKnowledge/PriorKnowledge.h>
-#include <MemoryX/interface/gui/GraphVisualizerInterface.h>
-#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+#include <armarx/navigation/graph/Graph.h>
+#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
-#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/client/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
@@ -40,27 +41,17 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <QDialog>
-#include <QGraphicsScene>
-#include <QGraphicsLineItem>
-#include <QGraphicsEllipseItem>
-#include <QMainWindow>
-#include <QMouseEvent>
-
#include <string>
-#include <map>
-#include <vector>
-#include <tuple>
-namespace armarx
-{
- class GraphVisualizerGraphicsEllipseItem;
- class GraphVisualizerGraphicsLineItem;
+class QDialog;
- class LocationGraphEditorWidgetController;
- class MouseEventProcessor;
+namespace armarx::nav::locgrapheditor
+{
+ class EdgeTableWidget;
+ class VertexDataWidget;
+ class VertexTableWidget;
/**
@@ -95,17 +86,11 @@ namespace armarx
public armarx::ArmarXComponentWidgetControllerTemplate < LocationGraphEditorWidgetController >
{
Q_OBJECT
- friend class MouseEventProcessor;
-
-
- public:
- /// The type of node ids. (This type implies the node exists)
- using NodeId = const std::string;
+ using This = LocationGraphEditorWidgetController;
- /// The type of edge ids. (This type implies the edge exists)
- using EdgeId = const std::pair<const std::string, const std::string>;
+ public:
static QString GetWidgetName();
static QIcon GetWidgetIcon();
@@ -134,354 +119,181 @@ namespace armarx
virtual void saveAutomaticSettings();
- // slice interface implementation
- bool hasEdge(const std::string& node1, const std::string& node2)
- {
- return (edges.find(toEdge(node1, node2)) != edges.end());
- }
- bool hasNode(const std::string& id)
- {
- return (nodes.find(id) != nodes.end());
- }
-
-
- public slots:
-
- // slice interface implementation
- void addEdge(const std::string& node1Id, const std::string& node2Id);
- void addNode(const memoryx::GraphNodeBasePtr& node);
-
- void highlightEdge(const std::string& node1Id, const std::string& node2Id, bool highlighted = true);
- void highlightNode(const std::string& nodeId, bool highlighted = true);
-
- void clearEdges();
- void clearGraph();
-
- void resetHighlight();
-
- void redraw();
- void refreshGraph();
+ signals:
- void tableWidgetNodesCustomContextMenu(QPoint pos);
- void tableWidgetEdgesCustomContextMenu(QPoint pos);
+ void connected();
+ void memoryDataChanged();
+ void graphChanged();
private slots:
- /**
- * @brief add kitchen graph (H2T Armar3a robot kitchen)
- */
- void addKitchenGraph();
-
- void selectedSceneChanged(int i);
- void updateSceneList();
- void drawScene();
+ // Data
+ void queryGraphs();
+ void updateGraphList();
+ void loadGraph();
- /**
- * @brief Toggles the double clicked node's selection state.
- * @param row Identifies the node.
- */
- void nodeTableDoubleClicked(int row, int);
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param row Identifies the edge.
- */
- void edgeTableDoubleClicked(int row, int);
+ GuiGraph::Vertex addVertex(graph::Graph::Vertex vertex);
+ GuiGraph::Edge addEdge(graph::Graph::Edge edge);
- /**
- * @brief Toggles the double clicked node's selection state.
- * @param id Identifies the node.
- */
- void nodeDoubleClicked(NodeId id);
+ void clearEdges();
+ void clearGraph();
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param id Identifies the edge.
- */
- void edgeDoubleClicked(EdgeId id);
- /**
- * @brief Rotates the view clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedClock();
+ // View & Tables
- /**
- * @brief Rotates the view counter clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedCounterClock();
+ void updateVertex(GuiGraph::Vertex vertex);
+ void updateEdge(GuiGraph::Edge edge);
- /**
- * @brief Applies the current transforamtion to the view.
- */
+ /// Applies the current transforamtion to the view.
void transformView();
-
- /**
- * @brief Adjusts the view's zoom and rotation to display most of the graph.
- */
+ /// Adjusts the view's zoom and rotation to display most of the graph.
void adjustView();
- bool addNewEdge(const std::string& from, const std::string& to);
- void addNewEdgeBoth();
- void addNewEdgeStartEnd();
- void addNewEdgeEndStart();
- void addNewGraphNode();
- void editGraphNode();
+ // Selection
+
+ void resetHighlighting();
+ void toggleVertexSelected(const semrel::ShapeID& vertexID);
private:
/// Widget Form
Ui::LocationGraphEditorWidget widget;
+ QPointer<SimpleConfigDialog> dialog;
- /**
- * @brief The NodeData struct holds data required for the node.
- * The name is stored in the key used in the map nodes.
- */
- struct NodeData
+ struct Remote
{
- /**
- * @brief The Entity of the graph segment this struct represents
- */
- memoryx::GraphNodeBasePtr node;
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ armem::client::MemoryNameSystem memoryNameSystem;
- /**
- * @brief The pose drawn to debugDrawer.
- */
- armarx::FramedPosePtr pose;
+ armem::client::Reader locationReader;
+ armem::client::Writer locationWriter;
- /**
- * @brief The ellipse in the scene.
- */
- QGraphicsEllipseItem* graphicsItem;
+ armem::client::Reader graphReader;
+ armem::client::Writer graphWriter;
- /**
- * @brief The row in the table tableWidgetNodes.
- */
- int tableWidgetNodesIndex;
- /**
- * @brief Whether the node is highlighted.
- */
- bool highlighted;
+ void connect(Component& parent);
};
+ Remote remote;
- /**
- * @brief The EdgeData struct holds data required for the edge.
- * The name is stored in the key used in the map edges.
- */
- struct EdgeData
+
+ struct Data
{
- /**
- * @brief The line in the scene.
- */
- QGraphicsLineItem* graphicsItem;
- /**
- * @brief The row in the table tableWidgetEdges.
- */
- int tableWidgetEdgesIndex;
+ armem::wm::Memory memory;
+ nav::locgrapheditor::GuiGraph graph;
+ };
+ Data data;
- /**
- * @brief Whether the edge is highlighted.
- */
- bool highlighted;
+
+ struct DataWidgets
+ {
+ VertexTableWidget* vertexTable = nullptr;
+ EdgeTableWidget* edgeTable = nullptr;
+ VertexDataWidget* vertexData = nullptr;
};
+ DataWidgets dataWidgets;
- /**
- * @brief Returns the EdgeId corresponding to two nodes.
- * @param node1 First node id.
- * @param node2 Second node id.
- * @return The EdgeId corresponding to two nodes.
- */
- static EdgeId toEdge(const std::string& node1, const std::string& node2)
+ struct GraphView
{
- return EdgeId {node1, node2};
- }
+ QGraphicsView* view = nullptr;
- /**
- * @brief Updates an edge.
- * @param The edge to update.
- */
- void updateEdge(const EdgeId& id);
+ /**
+ * @brief The scene displayed in the widget.
+ *
+ * For y coordinates (-y) is used to mirror the scene on the y axis.
+ * If (+y) would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
+ */
+ GraphScene* scene;
- /**
- * @brief Updates a node.
- * @param The node to update.
- */
- void updateNode(const NodeId& id);
+ /// The view's rotation angle.
+ qreal angle = 0;
- void setEditFields(const NodeData& node);
+ void setScene(GraphScene* scene);
+ };
+ GraphView view;
- /**
- * @brief The topic name used by debugDrawer.
- */
- std::string debugDrawerTopicName;
- /**
- * @brief Used to draw onto debug layers.
- */
- ::armarx::DebugDrawerInterfacePrx debugDrawer;
- /**
- * @brief The config dialog.
- */
- QPointer<SimpleConfigDialog> dialog;
+ // Non-refactored
- float getYawAngle(const armarx::PoseBasePtr& pose) const;
- /**
- * @brief The scene displayed in the widget.
- *
- * For y coordinates -pos->y is used to mirror the scene on the y axis.
- * If pos->y would be used the graph displayed in the scene would not
- * match the graph drawn to the debug layer.
- */
- QPointer<QGraphicsScene> scene;
+#if 0
+ private slots:
- /**
- * @brief The nodes.
- */
- std::map<std::string, NodeData> nodes;
+ void highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted = true);
+ void highlightVertex(const std::string& vertexId, bool highlighted = true);
- /**
- * @brief The edges.
- */
- std::map<EdgeId, EdgeData> edges;
+ void redraw();
+ void refreshGraph();
- /**
- * @brief The view's rotation angle.
- */
- qreal viewAngle;
+ void tableWidgetVerticesCustomContextMenu(QPoint pos);
+ void tableWidgetEdgesCustomContextMenu(QPoint pos);
- /**
- * @brief The layer to draw on.
- */
- std::string debugDrawerLayerName;
- bool editStartNodeNext;
+ private slots:
- std::string priorKnowledgeProxyName;
- memoryx::PriorKnowledgeInterfacePrx priorKnowledgePrx;
- memoryx::GraphNodePoseResolverInterfacePrx gnpr;
- memoryx::GraphMemorySegmentBasePrx graphSeg;
- QSettings settings;
- QString lastSelectedSceneName;
+ /// Add kitchen graph (H2T Armar3a robot kitchen)
+ void addKitchenGraph();
/**
- * selectScene(): private function called when button load is pushed, and calls the function loadScene()
+ * @brief Toggles the double clicked vertex's selection state.
+ * @param row Identifies the vertex.
*/
- void selectScene();
-
+ void vertexTableDoubleClicked(int row, int);
/**
- * @brief loadScene Private function that parses XML file to load a scene
- * @param xmlFile
+ * @brief Toggles the double clicked edge's selection state.
+ * @param row Identifies the edge.
*/
- void loadScene(const std::string& xmlFile);
-
-
- friend class GraphVisualizerGraphicsEllipseItem;
- friend class GraphVisualizerGraphicsLineItem;
-
-
- private:
-
- std::string memoryNameSystemName = "MemoryNameSystem";
- armem::client::MemoryNameSystem memoryNameSystem;
-
- };
-
-
- /**
- * @brief Required to override the double click event. This is required to toggle the select state.
- */
- class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
- {
- public:
- using NodeId = LocationGraphEditorWidgetController::NodeId;
-
- GraphVisualizerGraphicsEllipseItem(LocationGraphEditorWidgetController& visuWidget, NodeId name, qreal x, qreal y, qreal width, qreal height, QGraphicsItem* parent = nullptr):
- QGraphicsEllipseItem {x, y, width, height, parent},
- id {name},
- parentVisuWidget(visuWidget)
- {
- }
+ void edgeTableDoubleClicked(int row, int);
- ~GraphVisualizerGraphicsEllipseItem() override
- {
- }
- protected:
- void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
- {
- parentVisuWidget.nodeDoubleClicked(id);
- }
- private:
/**
- * @brief Required to identify the element.
+ * @brief Toggles the double clicked edge's selection state.
+ * @param id Identifies the edge.
*/
- const NodeId id;
+ void edgeDoubleClicked(GuiGraph::Edge edge);
/**
- * @brief Required to call nodeDoubleClicked on it. (This class is no QObject so it does not support signals)
+ * @brief Rotates the view clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
*/
- LocationGraphEditorWidgetController& parentVisuWidget;
- };
-
-
-
- /**
- * @brief Required to override the double click event. This is required to toggle the select state.
- */
- class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
- {
- public:
- using EdgeId = LocationGraphEditorWidgetController::EdgeId;
-
- GraphVisualizerGraphicsLineItem(LocationGraphEditorWidgetController& visuWidget, EdgeId name, qreal x1, qreal y1, qreal x2, qreal y2, QGraphicsItem* parent = 0):
- QGraphicsLineItem {x1, y1, x2, y2, parent},
- id {name},
- parentVisuWidget(visuWidget)
- {
- }
+ void viewRotatedClock();
- ~GraphVisualizerGraphicsLineItem() override
- {
- }
- signals:
- protected:
- void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override
- {
- parentVisuWidget.edgeDoubleClicked(id);
- }
- private:
/**
- * @brief Required to identify the element.
+ * @brief Rotates the view counter clockwise.
+ *
+ * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
*/
- const EdgeId id;
-
- /// Required to call edgeDoubleClicked on it. (This class is no QObject so it does not support signals)
- LocationGraphEditorWidgetController& parentVisuWidget;
- };
-
+ void viewRotatedCounterClock();
- class MouseEventProcessor : public QObject
- {
- Q_OBJECT
- public:
+ bool addNewEdge(const std::string& from, const std::string& to);
+ void addNewEdgeBoth();
+ void addNewEdgeStartEnd();
+ void addNewEdgeEndStart();
- MouseEventProcessor(LocationGraphEditorWidgetController* gvw) : gvw(gvw) {}
+ void addNewGraphVertex();
+ void editGraphVertex();
+#endif
+ void setEditFields(GuiGraph::Vertex vertex);
- protected:
- LocationGraphEditorWidgetController* gvw;
- bool eventFilter(QObject* obj, QEvent* event) override;
+ private:
+ QSettings settings;
+ QString lastSelectedSceneName;
};
}
+
+namespace armarx
+{
+ using armarx::nav::locgrapheditor::LocationGraphEditorWidgetController;
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
new file mode 100644
index 00000000..da5444cf
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -0,0 +1,75 @@
+/*
+ * 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/>.
+ *
+ * @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 "EdgeTableWidget.h"
+
+#include <QHeaderView>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ EdgeTableWidget::EdgeTableWidget()
+ {
+ QStringList columns{"Source", "Target"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ }
+
+
+ QTableWidgetItem*
+ EdgeTableWidget::addEdge(graph::Graph::Edge edge)
+ {
+ int row = rowCount();
+ setRowCount(row + 1);
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(edge.source().attrib().getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::fromStdString(edge.target().attrib().getName())});
+
+ return item(row, 0);
+ }
+
+
+ void
+ EdgeTableWidget::updateEdge(GuiGraph::Edge edge)
+ {
+ QColor bgColor = edge.attrib().highlighted ? bgColorSelected : bgColorDefault;
+ QFont font;
+ font.setBold(edge.attrib().highlighted);
+
+ int row = this->row(edge.attrib().tableWidgetItem);
+ for (int col = 0; col < 2; ++col)
+ {
+ auto* item = this->item(row, col);
+ ARMARX_CHECK_NOT_NULL(item);
+
+ item->setData(Qt::BackgroundRole, bgColor);
+ item->setFont(font);
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
new file mode 100644
index 00000000..b42c4480
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -0,0 +1,57 @@
+/*
+ * 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/>.
+ *
+ * @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 <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QTableWidget>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class EdgeTableWidget : public QTableWidget
+ {
+ using This = EdgeTableWidget;
+
+
+ public:
+
+ EdgeTableWidget();
+
+ QTableWidgetItem* addEdge(graph::Graph::Edge edge);
+
+ void updateEdge(GuiGraph::Edge edge);
+
+
+ private slots:
+
+
+ public:
+
+ QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
+ QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
new file mode 100644
index 00000000..b9df3993
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -0,0 +1,139 @@
+/*
+ * 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 "VertexDataWidget.h"
+
+#include <QDoubleSpinBox>
+#include <QFormLayout>
+#include <QHBoxLayout>
+#include <QLineEdit>
+#include <QRadioButton>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ VertexDataWidget::VertexDataWidget()
+ {
+ locationID = new QLineEdit(this);
+ frame = new QLineEdit(this);
+
+ x = new QDoubleSpinBox(this);
+ y = new QDoubleSpinBox(this);
+ z = new QDoubleSpinBox(this);
+
+ roll = new QDoubleSpinBox(this);
+ pitch = new QDoubleSpinBox(this);
+ yaw = new QDoubleSpinBox(this);
+
+ angleUnitDeg = new QRadioButton("Degrees");
+ angleUnitRad = new QRadioButton("Radians");
+
+
+ for (QDoubleSpinBox* pos : _positionSpinBoxes())
+ {
+ pos->setSuffix(" mm");
+ pos->setMinimum(-1e6);
+ pos->setMaximum(+1e6);
+ pos->setSingleStep(10);
+ pos->setValue(0);
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->setSuffix("");
+ pos->setMinimum(-360);
+ pos->setMaximum(+360);
+ pos->setValue(0);
+ }
+ for (QDoubleSpinBox* spinBox : _allSpinBoxes())
+ {
+ spinBox->setAlignment(Qt::AlignRight);
+ }
+
+ QFormLayout* layout = new QFormLayout();
+ this->setLayout(layout);
+
+ layout->addRow("Location ID", locationID);
+ layout->addRow("Frame", frame);
+ layout->addRow("X", x);
+ layout->addRow("Y", y);
+ layout->addRow("Z", z);
+ layout->addRow("Roll", roll);
+ layout->addRow("Pitch", pitch);
+ layout->addRow("Yaw", yaw);
+
+ QHBoxLayout* angleUnitLayout = new QHBoxLayout();
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ angleUnitLayout->addWidget(angleUnit);
+ }
+ layout->addRow("", angleUnitLayout);
+
+
+ // Connect
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
+ }
+
+ angleUnitDeg->click();
+ }
+
+
+ void VertexDataWidget::_updateAngleUnit()
+ {
+ QString suffix;
+ if (angleUnitDeg->isChecked())
+ {
+ suffix = " \u00b0";
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ suffix = " rad";
+ }
+ else
+ {
+ return;
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->setSuffix(suffix);
+ }
+ }
+
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_positionSpinBoxes()
+ {
+ return { x, y, z };
+ }
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_angleSpinBoxes()
+ {
+ return { roll, pitch, yaw };
+ }
+
+ std::vector<QDoubleSpinBox*> VertexDataWidget::_allSpinBoxes()
+ {
+ return { x, y, z, roll, pitch, yaw };
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
new file mode 100644
index 00000000..b96b8cbb
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -0,0 +1,78 @@
+/*
+ * 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/>.
+ *
+ * @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 <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QWidget>
+
+
+class QLineEdit;
+class QDoubleSpinBox;
+class QRadioButton;
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class VertexDataWidget : public QWidget
+ {
+ using This = VertexDataWidget;
+
+
+ public:
+
+ VertexDataWidget();
+
+
+
+ private slots:
+
+ void _updateAngleUnit();
+
+
+ private:
+
+ std::vector<QDoubleSpinBox*> _positionSpinBoxes();
+ std::vector<QDoubleSpinBox*> _angleSpinBoxes();
+ std::vector<QDoubleSpinBox*> _allSpinBoxes();
+
+
+ private:
+
+ QLineEdit* locationID = nullptr;
+ QLineEdit* frame = nullptr;
+
+ QDoubleSpinBox* x = nullptr;
+ QDoubleSpinBox* y = nullptr;
+ QDoubleSpinBox* z = nullptr;
+
+ QDoubleSpinBox* roll = nullptr;
+ QDoubleSpinBox* pitch = nullptr;
+ QDoubleSpinBox* yaw = nullptr;
+
+ QRadioButton* angleUnitDeg = nullptr;
+ QRadioButton* angleUnitRad = nullptr;
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
new file mode 100644
index 00000000..1906868b
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -0,0 +1,87 @@
+/*
+ * 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/>.
+ *
+ * @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 "VertexTableWidget.h"
+
+#include <QHeaderView>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ VertexTableWidget::VertexTableWidget()
+ {
+ QStringList columns{"Name", "X", "Y", "Yaw"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ }
+
+
+ QTableWidgetItem*
+ VertexTableWidget::addVertex(graph::Graph::Vertex vertex)
+ {
+ Eigen::Matrix4f pose = vertex.attrib().getPose();
+
+ char format = 'f';
+ const int precision = 2;
+
+ int row = rowCount();
+ setRowCount(row + 1);
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(vertex.attrib().getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::number(qreal(pose(0, 3)), format, precision)});
+ setItem(row, 2, new QTableWidgetItem {QString::number(qreal(pose(1, 3)), format, precision)});
+ setItem(row, 3, new QTableWidgetItem {QString::number(qreal(getYawAngleDegree(pose)), format, precision)});
+
+ for (int col = 1; col <= 3; ++col)
+ {
+ item(row, col)->setTextAlignment(Qt::AlignRight);
+ }
+
+ return item(row, 0);
+ }
+
+
+ void
+ VertexTableWidget::updateVertex(GuiGraph::Vertex vertex)
+ {
+ QColor bgColor = vertex.attrib().highlighted ? bgColorSelected : bgColorDefault;
+ QFont font;
+ font.setBold(vertex.attrib().highlighted);
+
+ int row = this->row(vertex.attrib().tableWidgetItem);
+ for (int col = 0; col < 4; ++col)
+ {
+ auto* item = this->item(row, col);
+ ARMARX_CHECK_NOT_NULL(item);
+
+ item->setData(Qt::BackgroundRole, bgColor);
+ item->setFont(font);
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
new file mode 100644
index 00000000..31fbed28
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -0,0 +1,56 @@
+/*
+ * 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/>.
+ *
+ * @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 <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+#include <QTableWidget>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class VertexTableWidget : public QTableWidget
+ {
+ using This = VertexTableWidget;
+
+
+ public:
+
+ VertexTableWidget();
+
+ QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
+
+ void updateVertex(GuiGraph::Vertex vertex);
+
+
+ private slots:
+
+
+ public:
+
+ QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
+ QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+
+ };
+
+}
--
GitLab
From 7ed5e65521ce6f85b1fedce02846596b9dd18975 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Aug 2021 11:40:36 +0200
Subject: [PATCH 13/33] Refactor default colors
---
.../LocationGraphEditor/CMakeLists.txt | 2 ++
.../widgets/EdgeTableWidget.h | 6 ++--
.../widgets/VertexTableWidget.cpp | 4 ++-
.../widgets/VertexTableWidget.h | 8 +++--
.../widgets/default_colors.cpp | 33 +++++++++++++++++++
.../widgets/default_colors.h | 33 +++++++++++++++++++
6 files changed, 81 insertions(+), 5 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 3d865290..8235d281 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -17,6 +17,7 @@ set(SOURCES
GraphScene.cpp
GuiGraph.cpp
+ widgets/default_colors.cpp
widgets/EdgeTableWidget.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
@@ -28,6 +29,7 @@ set(HEADERS
GraphScene.h
GuiGraph.h
+ widgets/default_colors.h
widgets/EdgeTableWidget.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index b42c4480..f0266930 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -22,7 +22,9 @@
#pragma once
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
+#include <QColor>
#include <QTableWidget>
@@ -48,8 +50,8 @@ namespace armarx::nav::locgrapheditor
public:
- QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
- QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+ QColor bgColorDefault = default_colors::tableBackgroundDefault;
+ QColor bgColorSelected = default_colors::tableBackgroundSelected;
};
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 1906868b..12697de5 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -39,6 +39,8 @@ namespace armarx::nav::locgrapheditor
setSortingEnabled(true);
setContextMenuPolicy(Qt::CustomContextMenu);
+
+ setAlternatingRowColors(true);
}
@@ -59,7 +61,7 @@ namespace armarx::nav::locgrapheditor
for (int col = 1; col <= 3; ++col)
{
- item(row, col)->setTextAlignment(Qt::AlignRight);
+ item(row, col)->setTextAlignment(Qt::AlignRight | Qt::AlignVCenter);
}
return item(row, 0);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 31fbed28..e00b356e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -22,7 +22,9 @@
#pragma once
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
+#include <QColor>
#include <QTableWidget>
@@ -31,6 +33,7 @@ namespace armarx::nav::locgrapheditor
class VertexTableWidget : public QTableWidget
{
+ Q_OBJECT
using This = VertexTableWidget;
@@ -48,8 +51,9 @@ namespace armarx::nav::locgrapheditor
public:
- QColor bgColorDefault = QColor::fromRgb(255, 255, 255);
- QColor bgColorSelected = QColor::fromRgb(0, 128, 255);
+ QColor bgColorDefault = default_colors::tableBackgroundDefault;
+ QColor bgColorSelected = default_colors::tableBackgroundSelected;
+
};
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
new file mode 100644
index 00000000..059cb71c
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
@@ -0,0 +1,33 @@
+/*
+ * 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/>.
+ *
+ * @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 "default_colors.h"
+
+#include <QColor>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ const QColor default_colors::tableBackgroundDefault = QColor::fromRgb(255, 255, 255);
+ const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(128, 196, 255);
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
new file mode 100644
index 00000000..0a355fa6
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
@@ -0,0 +1,33 @@
+/*
+ * 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/>.
+ *
+ * @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
+
+class QColor;
+
+
+namespace armarx::nav::locgrapheditor::default_colors
+{
+
+ extern const QColor tableBackgroundDefault;
+ extern const QColor tableBackgroundSelected;
+
+}
--
GitLab
From 0e8584e42758009a9e7320b22a4c40e582279f57 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Aug 2021 11:41:12 +0200
Subject: [PATCH 14/33] Extend / fix table/data widgets
---
.../FunctionalEventFilter.h | 3 +-
.../LocationGraphEditor/GraphScene.h | 1 +
.../widgets/EdgeTableWidget.cpp | 1 +
.../widgets/EdgeTableWidget.h | 6 +
.../widgets/VertexDataWidget.cpp | 147 +++++++++++++++++-
.../widgets/VertexDataWidget.h | 28 ++++
.../widgets/VertexTableWidget.cpp | 24 +++
.../widgets/VertexTableWidget.h | 7 +
8 files changed, 212 insertions(+), 5 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
index 3513a795..0b5fb30b 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
@@ -12,12 +12,11 @@ namespace simox::gui
class FunctionalEventFilter : public QObject
{
- Q_OBJECT
-
public:
using Function = std::function<bool(QObject* obj, QEvent* event)>;
+
FunctionalEventFilter(Function&& function);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
index 514bd6a4..099f4158 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -17,6 +17,7 @@ namespace semrel
}
namespace armarx::nav::locgrapheditor
{
+
class GraphScene : public QGraphicsScene
{
Q_OBJECT
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index da5444cf..09a1b2b1 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -72,4 +72,5 @@ namespace armarx::nav::locgrapheditor
}
}
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index f0266930..0991665d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -33,6 +33,7 @@ namespace armarx::nav::locgrapheditor
class EdgeTableWidget : public QTableWidget
{
+ Q_OBJECT
using This = EdgeTableWidget;
@@ -40,10 +41,15 @@ namespace armarx::nav::locgrapheditor
EdgeTableWidget();
+
QTableWidgetItem* addEdge(graph::Graph::Edge edge);
void updateEdge(GuiGraph::Edge edge);
+ void removeEdge(GuiGraph::Edge edge);
+
+ void clearEdges();
+
private slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index b9df3993..e79324ea 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -22,19 +22,33 @@
#include "VertexDataWidget.h"
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
#include <QDoubleSpinBox>
#include <QFormLayout>
#include <QHBoxLayout>
#include <QLineEdit>
#include <QRadioButton>
+#include <SimoxUtility/math/convert/deg_to_rad.h>
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+#include <SimoxUtility/math/convert/rpy_to_mat4f.h>
+#include <SimoxUtility/math/pose/pose.h>
+
namespace armarx::nav::locgrapheditor
{
VertexDataWidget::VertexDataWidget()
{
+ setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+
locationID = new QLineEdit(this);
+ locationID->setReadOnly(true);
+
frame = new QLineEdit(this);
x = new QDoubleSpinBox(this);
@@ -60,9 +74,9 @@ namespace armarx::nav::locgrapheditor
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
angle->setSuffix("");
- pos->setMinimum(-360);
- pos->setMaximum(+360);
- pos->setValue(0);
+ angle->setMinimum(-360);
+ angle->setMaximum(+360);
+ angle->setValue(0);
}
for (QDoubleSpinBox* spinBox : _allSpinBoxes())
{
@@ -90,6 +104,7 @@ namespace armarx::nav::locgrapheditor
// Connect
+
for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
{
connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
@@ -99,6 +114,71 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
+ {
+ setFromVertex(vertex);
+ setEnabled(true);
+ }
+
+
+ void VertexDataWidget::setFromVertex(const GuiGraph::Vertex& vertex)
+ {
+ const VertexData& attrib = vertex.attrib();
+ const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
+
+ locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
+ frame->setText("<WIP>");
+ _setXyz(simox::math::position(pose));
+ _setRpyRad(simox::math::mat4f_to_rpy(pose));
+ }
+
+
+ void VertexDataWidget::getToVertex(GuiGraph::Vertex& vertex)
+ {
+ VertexData& attrib = vertex.attrib();
+
+ // locationID is read-only
+ // frame->setText("<WIP>"); // WIP
+
+ Eigen::Matrix4d pose = simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad()));
+ attrib.setPose(pose.cast<float>());
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::xyz() const
+ {
+ return { x->value(), y->value(), z->value() };
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::rpyDeg() const
+ {
+ Eigen::Vector3d raw = _rpyRaw();
+ if (angleUnitRad->isChecked())
+ {
+ return {}; // return simox::math::rad_to_deg(raw);
+ }
+ else
+ {
+ return raw;
+ }
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::rpyRad() const
+ {
+ Eigen::Vector3d raw = _rpyRaw();
+ if (angleUnitDeg->isChecked())
+ {
+ return simox::math::deg_to_rad(raw);
+ }
+ else
+ {
+ return raw;
+ }
+ }
+
+
void VertexDataWidget::_updateAngleUnit()
{
QString suffix;
@@ -121,19 +201,80 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::_updateVertexAttribs()
+ {
+ if (vertex.has_value())
+ {
+ getToVertex(vertex.value());
+
+ emit vertexDataChanged(vertex.value());
+ }
+ }
+
+
std::vector<QDoubleSpinBox*> VertexDataWidget::_positionSpinBoxes()
{
return { x, y, z };
}
+
std::vector<QDoubleSpinBox*> VertexDataWidget::_angleSpinBoxes()
{
return { roll, pitch, yaw };
}
+
std::vector<QDoubleSpinBox*> VertexDataWidget::_allSpinBoxes()
{
return { x, y, z, roll, pitch, yaw };
}
+
+ void VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
+ {
+ x->setValue(xyz.x());
+ y->setValue(xyz.y());
+ z->setValue(xyz.z());
+ }
+
+
+ Eigen::Vector3d VertexDataWidget::_rpyRaw() const
+ {
+ return { roll->value(), pitch->value(), yaw->value() };
+ }
+
+
+ void VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
+ {
+ roll->setValue(rpy(0));
+ pitch->setValue(rpy(1));
+ yaw->setValue(rpy(2));
+ }
+
+
+ void VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
+ {
+ if (angleUnitDeg->isChecked())
+ {
+ _setRpyRaw(rpyDeg);
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ _setRpyRaw(simox::math::deg_to_rad(rpyDeg));
+ }
+ }
+
+
+ void VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
+ {
+ if (angleUnitDeg->isChecked())
+ {
+ _setRpyRaw(simox::math::rad_to_deg(rpyRad));
+ }
+ else if (angleUnitRad->isChecked())
+ {
+ _setRpyRaw(rpyRad);
+ }
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index b96b8cbb..75d17e33 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -23,6 +23,8 @@
#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <Eigen/Core>
+
#include <QWidget>
@@ -36,6 +38,7 @@ namespace armarx::nav::locgrapheditor
class VertexDataWidget : public QWidget
{
+ Q_OBJECT
using This = VertexDataWidget;
@@ -43,11 +46,26 @@ namespace armarx::nav::locgrapheditor
VertexDataWidget();
+ void setVertex(GuiGraph::Vertex vertex);
+
+ void setFromVertex(const GuiGraph::Vertex& vertex);
+ void getToVertex(GuiGraph::Vertex& vertex);
+
+
+ Eigen::Vector3d xyz() const;
+ Eigen::Vector3d rpyDeg() const;
+ Eigen::Vector3d rpyRad() const;
+
+
+ signals:
+
+ void vertexDataChanged(GuiGraph::Vertex vertex);
private slots:
void _updateAngleUnit();
+ void _updateVertexAttribs();
private:
@@ -56,9 +74,19 @@ namespace armarx::nav::locgrapheditor
std::vector<QDoubleSpinBox*> _angleSpinBoxes();
std::vector<QDoubleSpinBox*> _allSpinBoxes();
+ void _setXyz(const Eigen::Vector3d& xyz);
+
+ Eigen::Vector3d _rpyRaw() const;
+ void _setRpyRaw(const Eigen::Vector3d& rpy) const;
+ void _setRpyDeg(const Eigen::Vector3d& rpyDeg) const;
+ void _setRpyRad(const Eigen::Vector3d& rpyRad) const;
+
private:
+ std::optional<GuiGraph::Vertex> vertex;
+
+
QLineEdit* locationID = nullptr;
QLineEdit* frame = nullptr;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 12697de5..7df6d1b7 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -86,4 +86,28 @@ namespace armarx::nav::locgrapheditor
}
}
+
+
+ QList<QTableWidgetItem*>
+ VertexTableWidget::selectedEdgeItems()
+ {
+ // Only return items from the first column (and avoid duplicates).
+ std::set<QTableWidgetItem*> set;
+ for (QTableWidgetItem* selected : selectedItems())
+ {
+ if (column(selected) != 0)
+ {
+ selected = item(row(selected), 0);
+ }
+ set.insert(selected);
+ }
+
+ QList<QTableWidgetItem*> list;
+ for (auto i : set)
+ {
+ list.append(i);
+ }
+ return list;
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index e00b356e..6a8f24a7 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -41,11 +41,18 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
+
QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
void updateVertex(GuiGraph::Vertex vertex);
+ QList<QTableWidgetItem*> selectedEdgeItems();
+
+
+ public slots:
+
+
private slots:
--
GitLab
From b4edf637d5e63a6e090d6d7ebe086e7d8139fe63 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Aug 2021 11:41:26 +0200
Subject: [PATCH 15/33] Refactor graph view
---
source/armarx/navigation/graph/Graph.cpp | 5 +
source/armarx/navigation/graph/Graph.h | 2 +
.../LocationGraphEditor/CMakeLists.txt | 11 +-
.../LocationGraphEditor/GuiGraph.cpp | 52 +++-
.../LocationGraphEditor/GuiGraph.h | 16 +-
.../LocationGraphEditorWidget.ui | 121 ++------
.../LocationGraphEditorWidgetController.cpp | 268 +++++++-----------
.../LocationGraphEditorWidgetController.h | 47 +--
.../LocationGraphEditor/widgets/graph_scene.h | 17 ++
.../widgets/graph_scene/ControlWidget.cpp | 107 +++++++
.../widgets/graph_scene/ControlWidget.h | 59 ++++
.../widgets/graph_scene/Scene.cpp | 164 +++++++++++
.../widgets/graph_scene/Scene.h | 118 ++++++++
.../widgets/graph_scene/Widget.cpp | 83 ++++++
.../widgets/graph_scene/Widget.h | 58 ++++
15 files changed, 816 insertions(+), 312 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index bce79ae3..f8086dce 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -36,6 +36,11 @@ namespace armarx::nav::graph
return aron.globalRobotPose;
}
+ void VertexAttribs::setPose(const Eigen::Matrix4f& pose)
+ {
+ this->aron.globalRobotPose = pose;
+ }
+
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index dd5704bc..d895dd63 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -35,7 +35,9 @@ namespace armarx::nav::graph
armarx::nav::graph::arondto::Vertex aron;
std::string getName() const;
+
Eigen::Matrix4f getPose() const;
+ void setPose(const Eigen::Matrix4f& pose);
};
struct EdgeAttribs
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 8235d281..d2661dba 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -14,25 +14,32 @@ set(SOURCES
LocationGraphEditorWidgetController.cpp
FunctionalEventFilter.cpp
- GraphScene.cpp
GuiGraph.cpp
widgets/default_colors.cpp
widgets/EdgeTableWidget.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
+
+ widgets/graph_scene/ControlWidget.cpp
+ widgets/graph_scene/Scene.cpp
+ widgets/graph_scene/Widget.cpp
)
set(HEADERS
LocationGraphEditorWidgetController.h
FunctionalEventFilter.h
- GraphScene.h
GuiGraph.h
widgets/default_colors.h
widgets/EdgeTableWidget.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
+
+ widgets/graph_scene.h
+ widgets/graph_scene/ControlWidget.h
+ widgets/graph_scene/Scene.h
+ widgets/graph_scene/Widget.h
)
set(GUI_UIS
LocationGraphEditorWidget.ui
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
index 377f726e..f4fd3e1d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -29,24 +29,54 @@
namespace armarx::nav::locgrapheditor
{
+ std::optional<GuiGraph::Vertex>
+ GuiGraph::getVertexFromTableItem(QTableWidgetItem* item)
+ {
+ for (auto vertex : vertices())
+ {
+ if (vertex.attrib().tableWidgetItem == item)
+ {
+ return vertex;
+ }
+ }
+ return std::nullopt;
+ }
+
+
+ std::map<QTableWidgetItem*, GuiGraph::Vertex>
+ GuiGraph::getTableItemToVertexMap()
+ {
+ std::map<QTableWidgetItem*, GuiGraph::Vertex> map;
+
+ for (auto vertex : vertices())
+ {
+ if (vertex.attrib().tableWidgetItem != nullptr)
+ {
+ map[vertex.attrib().tableWidgetItem] = vertex;
+ }
+ }
+
+ return map;
+ }
+
}
namespace armarx::nav
{
- float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
- {
- Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
- return simox::math::rad_to_deg(rpy[2]);
- }
+float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+{
+ Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
+ return simox::math::rad_to_deg(rpy[2]);
+}
- auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+{
+ GuiGraph gui;
+ for (auto v : nav.vertices())
{
- GuiGraph gui;
- for (auto v : nav.vertices())
- {
- gui.addVertex(v.objectID(), { v.attrib() });
- }
+ gui.addVertex(v.objectID(), { v.attrib() });
+ }
for (auto e : nav.edges())
{
gui.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
index f0b6e0bb..87395a1f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -25,6 +25,9 @@
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+#include <map>
+#include <optional>
+
class QGraphicsEllipseItem;
class QGraphicsLineItem;
@@ -73,7 +76,18 @@ namespace armarx::nav::locgrapheditor
};
- using GuiGraph = semrel::RelationGraph<VertexData, EdgeData, GraphData>;
+ class GuiGraph : public semrel::RelationGraph<VertexData, EdgeData, GraphData>
+ {
+ public:
+
+ using RelationGraph::RelationGraph;
+
+
+ std::optional<Vertex> getVertexFromTableItem(QTableWidgetItem* item);
+
+ std::map<QTableWidgetItem*, Vertex> getTableItemToVertexMap();
+
+ };
GuiGraph toGuiGraph(const graph::Graph& graph);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 6a33a012..a25894c7 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -79,95 +79,7 @@
<number>0</number>
</property>
<item>
- <widget class="QWidget" name="graphHead" native="true">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
- <item>
- <widget class="QLabel" name="label_graph">
- <property name="text">
- <string>Graph</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="buttonRedraw">
- <property name="text">
- <string>Repaint</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="buttonClear">
- <property name="text">
- <string>Clear Graph</string>
- </property>
- </widget>
- </item>
- <item>
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item>
- <widget class="QToolButton" name="buttonRotateClock">
- <property name="text">
- <string>↻</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QToolButton" name="buttonRotateCounterClock">
- <property name="text">
- <string>↺</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="Line" name="line_3">
- <property name="orientation">
- <enum>Qt::Vertical</enum>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QLabel" name="label_zoom">
- <property name="text">
- <string>Zoom</string>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QDoubleSpinBox" name="viewZoomFactor">
- <property name="decimals">
- <number>5</number>
- </property>
- <property name="singleStep">
- <double>0.001000000000000</double>
- </property>
- <property name="value">
- <double>0.100000000000000</double>
- </property>
- </widget>
- </item>
- <item>
- <widget class="QToolButton" name="buttonAutoAdjust">
- <property name="text">
- <string>Auto</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <widget class="QGraphicsView" name="graphicsViewGraph"/>
+ <layout class="QVBoxLayout" name="graphSceneLayout"/>
</item>
</layout>
</widget>
@@ -193,11 +105,11 @@
<widget class="QWidget" name="verticalLayout_12Widget">
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
- <widget class="QGroupBox" name="locationsTableGroupBox">
+ <widget class="QGroupBox" name="locationDataGroupBox">
<property name="title">
- <string>Locations (Graph Vertices)</string>
+ <string>Location Data</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout_7">
+ <layout class="QVBoxLayout" name="verticalLayout_3">
<property name="leftMargin">
<number>3</number>
</property>
@@ -218,11 +130,11 @@
<widget class="QWidget" name="verticalLayout_13Widget">
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
- <widget class="QGroupBox" name="edgesTableGroupBox">
+ <widget class="QGroupBox" name="locationsTableGroupBox">
<property name="title">
- <string>Graph Edges</string>
+ <string>Locations (Graph Vertices)</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout_8">
+ <layout class="QVBoxLayout" name="verticalLayout_7">
<property name="leftMargin">
<number>3</number>
</property>
@@ -243,11 +155,24 @@
<widget class="QWidget" name="verticalLayout_6Widget">
<layout class="QVBoxLayout" name="verticalLayout_6">
<item>
- <widget class="QGroupBox" name="locationDataGroupBox">
+ <widget class="QGroupBox" name="edgesTableGroupBox">
<property name="title">
- <string>Location Data</string>
+ <string>Graph Edges</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout_3"/>
+ <layout class="QVBoxLayout" name="verticalLayout_8">
+ <property name="leftMargin">
+ <number>3</number>
+ </property>
+ <property name="topMargin">
+ <number>3</number>
+ </property>
+ <property name="rightMargin">
+ <number>3</number>
+ </property>
+ <property name="bottomMargin">
+ <number>3</number>
+ </property>
+ </layout>
</widget>
</item>
</layout>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 9510d82c..e23d6748 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -24,6 +24,8 @@
#include "widgets/EdgeTableWidget.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
+#include "widgets/graph_scene/Scene.h"
+#include "widgets/graph_scene/Widget.h"
#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
@@ -110,20 +112,22 @@ namespace armarx::nav::locgrapheditor
loadAutomaticSettings();
- dataWidgets.edgeTable = new EdgeTableWidget();
- dataWidgets.vertexTable = new VertexTableWidget();
- dataWidgets.vertexData = new VertexDataWidget();
- widget.edgesTableGroupBox->layout()->addWidget(dataWidgets.edgeTable);
- widget.locationsTableGroupBox->layout()->addWidget(dataWidgets.vertexTable);
- widget.locationDataGroupBox->layout()->addWidget(dataWidgets.vertexData);
+ widget.loadGraphButton->setEnabled(false);
- // Add scene
- view.view = widget.graphicsViewGraph;
- view.setScene(new GraphScene());
+ view.edgeTable = new EdgeTableWidget();
+ view.vertexTable = new VertexTableWidget();
+ view.vertexData = new VertexDataWidget();
+ view.vertexData->setEnabled(false); // Enable on first selection of vertex.
+ widget.edgesTableGroupBox->layout()->addWidget(view.edgeTable);
+ widget.locationsTableGroupBox->layout()->addWidget(view.vertexTable);
+ widget.locationDataGroupBox->layout()->addWidget(view.vertexData);
+ view.graph = new GraphSceneWidget();
+ widget.graphSceneLayout->addWidget(view.graph);
+
+#if 0
auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
{
-#if 0
if (obj == this->view.view && event->type() == QEvent::MouseButtonPress)
{
QMouseEvent* me = static_cast<QMouseEvent*>(event);
@@ -158,54 +162,38 @@ namespace armarx::nav::locgrapheditor
{
this->adjustView();
}
-#endif
- return QObject::eventFilter(obj, event);
+ else
+ {
+ return false;
+ }
};
view.view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
-
-
- // Transform view
- transformView();
+#endif
// Widgets -> This
// Memory Access
- connect(widget.refreshGraphsButton, &QPushButton::clicked, this, &This::updateGraphList);
- connect(widget.loadGraphButton, &QPushButton::clicked, this, &This::loadGraph);
+ connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
+ connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
#if 0
// Tables
- connect(dataWidgets.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
- connect(dataWidgets.vertexTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetVerticesCustomContextMenu);
- connect(dataWidgets.edgeTable, &QTableWidget::cellDoubleClicked, this, &This::edgeTableDoubleClicked);
- connect(dataWidgets.edgeTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetEdgesCustomContextMenu);
-
- connect(widget.btnAdd, &QPushButton::clicked, this, &This::addNewGraphVertex);
- connect(widget.btnAddEdge, &QPushButton::clicked, this, &This::addNewEdgeBoth);
- connect(widget.btnAddEdgeStartEnd, &QPushButton::clicked, this, &This::addNewEdgeStartEnd);
- connect(widget.btnAddEdgeEndStart, &QPushButton::clicked, this, &This::addNewEdgeEndStart);
- connect(widget.btnEdit, &QPushButton::clicked, this, &This::editGraphVertex);
+ connect(view.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
+ connect(view.vertexTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetVerticesCustomContextMenu);
+ connect(view.edgeTable, &QTableWidget::cellDoubleClicked, this, &This::edgeTableDoubleClicked);
+ connect(view.edgeTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetEdgesCustomContextMenu);
+
+ connect(widget.btnAdd, &QPushButton::pressed, this, &This::addNewGraphVertex);
+ connect(widget.btnAddEdge, &QPushButton::pressed, this, &This::addNewEdgeBoth);
+ connect(widget.btnAddEdgeStartEnd, &QPushButton::pressed, this, &This::addNewEdgeStartEnd);
+ connect(widget.btnAddEdgeEndStart, &QPushButton::pressed, this, &This::addNewEdgeEndStart);
+ connect(widget.btnEdit, &QPushButton::pressed, this, &This::editGraphVertex);
#endif
- // Zoom
- connect(widget.viewZoomFactor, QOverload<double>::of(&QDoubleSpinBox::valueChanged), this, &This::transformView);
-
-#if 0
- // Rotation
- connect(widget.buttonRotateClock, &QPushButton::clicked, this, &This::viewRotatedClock);
- connect(widget.buttonRotateCounterClock, &QPushButton::clicked, this, &This::viewRotatedCounterClock);
-
- // Redraw+clear
- connect(widget.buttonRedraw, &QPushButton::clicked, this, &This::redraw);
- connect(widget.buttonClear, &QPushButton::clicked, this, &This::clearGraph);
-
- // Auto adjust view
- connect(widget.buttonAutoAdjust, &QPushButton::clicked, this, &This::adjustView);
-#endif
-
- connect(view.scene, &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
+ // View
+ // connect(view.graph->scene(), &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
// This -> This
@@ -213,13 +201,19 @@ namespace armarx::nav::locgrapheditor
connect(this, &This::connected, this, &This::queryGraphs);
connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
- }
+ connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
+ this, &This::updateVertexView);
- void LocationGraphEditorWidgetController::GraphView::setScene(GraphScene* scene)
- {
- this->scene = scene;
- this->view->setScene(this->scene);
+ connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
+ [this](QTableWidgetItem* current, QTableWidgetItem* previous)
+ {
+ (void) previous;
+ this->selectVertex(current);
+ });
+
+ connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
+ this, &This::updateVertexHighlighting);
}
@@ -327,10 +321,13 @@ namespace armarx::nav::locgrapheditor
{
widget.graphsComboBox->clear();
- data.memory.forEachEntity([this](const armem::wm::Entity& entity)
+ bool enable = false;
+ data.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
{
widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
+ enable = true;
});
+ widget.loadGraphButton->setEnabled(enable);
}
@@ -384,7 +381,7 @@ namespace armarx::nav::locgrapheditor
if (vertex.objectID() == vertexID)
{
vertex.attrib().highlighted = true;
- updateVertex(vertex);
+ updateVertexView(vertex);
}
}
}
@@ -399,11 +396,11 @@ namespace armarx::nav::locgrapheditor
ARMARX_CHECK(not data.graph.hasVertex(vertex.objectID()));
VertexData attrib { vertex.attrib() };
- attrib.graphicsItem = view.scene->addVertex(vertex);
- attrib.tableWidgetItem = dataWidgets.vertexTable->addVertex(vertex);
+ attrib.graphicsItem = view.graph->scene()->addVertex(vertex);
+ attrib.tableWidgetItem = view.vertexTable->addVertex(vertex);
auto guiVertex = data.graph.addVertex(vertex.objectID(), attrib);
- updateVertex(guiVertex);
+ updateVertexView(guiVertex);
return guiVertex;
}
@@ -420,22 +417,21 @@ namespace armarx::nav::locgrapheditor
auto target = data.graph.vertex(edge.targetObjectID());
EdgeData attrib { edge.attrib() };
- attrib.tableWidgetItem = dataWidgets.edgeTable->addEdge(edge);
- attrib.graphicsItem = view.scene->addEdge(edge);
+ attrib.tableWidgetItem = view.edgeTable->addEdge(edge);
+ attrib.graphicsItem = view.graph->scene()->addEdge(edge);
auto guiEdge = data.graph.addEdge(source, target, attrib);
- updateEdge(guiEdge);
+ updateEdgeView(guiEdge);
return guiEdge;
}
- void LocationGraphEditorWidgetController::updateVertex(GuiGraph::Vertex vertex)
+ void LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
{
- view.scene->updateVertex(vertex);
- dataWidgets.vertexTable->updateVertex(vertex);
-
- setEditFields(vertex);
+ view.graph->scene()->updateVertex(vertex);
+ view.vertexTable->updateVertex(vertex);
+ if (/* DISABLES CODE */ (false)) // Disable this for the moment.
{
// Highlight all edges between highlighted vertices
std::set<semrel::ShapeID> highlightedVertices;
@@ -458,7 +454,7 @@ namespace armarx::nav::locgrapheditor
if (edge.attrib().highlighted != verticesHighlighted)
{
edge.attrib().highlighted = verticesHighlighted;
- updateEdge(edge);
+ updateEdgeView(edge);
}
}
}
@@ -480,10 +476,10 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::updateEdge(GuiGraph::Edge edge)
+ void LocationGraphEditorWidgetController::updateEdgeView(GuiGraph::Edge edge)
{
- view.scene->updateEdge(edge);
- dataWidgets.edgeTable->updateEdge(edge);
+ view.graph->scene()->updateEdge(edge);
+ view.edgeTable->updateEdge(edge);
#if 0
armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.first).pose->position)->toEigen());
@@ -507,12 +503,12 @@ namespace armarx::nav::locgrapheditor
// Remove from graphics scene
for (auto edge : data.graph.edges())
{
- view.scene->removeEdge(edge.attrib().graphicsItem);
+ view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
}
// Clear table widget
- dataWidgets.edgeTable->clearContents();
- dataWidgets.edgeTable->setRowCount(0);
+ view.edgeTable->clearContents();
+ view.edgeTable->setRowCount(0);
// Clear data structure
while (data.graph.edges().begin() != data.graph.edges().end())
@@ -527,13 +523,13 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::clearGraph()
{
// Clear scene
- view.scene->clear();
+ view.graph->scene()->clear();
// Clear table widgets
- dataWidgets.edgeTable->clearContents();
- dataWidgets.edgeTable->setRowCount(0);
- dataWidgets.vertexTable->clearContents();
- dataWidgets.vertexTable->setRowCount(0);
+ view.edgeTable->clearContents();
+ view.edgeTable->setRowCount(0);
+ view.vertexTable->clearContents();
+ view.vertexTable->setRowCount(0);
// Clear data structure
data.graph.clear();
@@ -544,11 +540,10 @@ namespace armarx::nav::locgrapheditor
{
auto vertex = data.graph.vertex(vertexID);
vertex.attrib().highlighted ^= true;
- updateVertex(vertex);
+ updateVertexView(vertex);
}
-
void LocationGraphEditorWidgetController::resetHighlighting()
{
for (auto vertex : data.graph.vertices())
@@ -556,7 +551,7 @@ namespace armarx::nav::locgrapheditor
if (vertex.attrib().highlighted)
{
vertex.attrib().highlighted = false;
- updateVertex(vertex);
+ updateVertexView(vertex);
}
}
for (auto edge : data.graph.edges())
@@ -564,12 +559,32 @@ namespace armarx::nav::locgrapheditor
if (edge.attrib().highlighted)
{
edge.attrib().highlighted = false;
- updateEdge(edge);
+ updateEdgeView(edge);
}
}
}
+ void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ {
+ std::map<QTableWidgetItem*, GuiGraph::Vertex> map = data.graph.getTableItemToVertexMap();
+
+ for (QTableWidgetItem* selected : view.vertexTable->selectedEdgeItems())
+ {
+ if (auto it = map.find(selected); it != map.end())
+ {
+ it->second.attrib().highlighted = true;
+ updateVertexView(it->second);
+ map.erase(it);
+ }
+ }
+ for (auto& [_, unselected] : map)
+ {
+ unselected.attrib().highlighted = false;
+ updateVertexView(unselected);
+ }
+ }
+
#if 0
@@ -612,7 +627,7 @@ namespace armarx::nav::locgrapheditor
{
auto vertexIt = std::find_if(vertices.cbegin(), vertices.cend(), [&](const std::pair<std::string, VertexData>& d)
{
- // return d.attrib().vertex->getName() == dataWidgets.vertexTable->item(row, 0)->text().toStdString();
+ // return d.attrib().vertex->getName() == view.vertexTable->item(row, 0)->text().toStdString();
return d.attrib().tableWidgetVerticesIndex == row;
});
auto vertexId = vertexIt->second.vertex->getId();
@@ -643,35 +658,9 @@ namespace armarx::nav::locgrapheditor
updateVertex(edge.attrib());
}
-
- void LocationGraphEditorWidgetController::redraw()
- {
- loadGraph();
- }
#endif
- void LocationGraphEditorWidgetController::setEditFields(GuiGraph::Vertex vertex)
- {
-#if 0
- widget.editVertexId->setText(QString::fromStdString(vertexData.vertex->getId()));
- widget.editSceneName->setText(QString::fromStdString(vertexData.vertex->getScene()));
- widget.editVertexName->setText(QString::fromStdString(vertexData.vertex->getName()));
- widget.editFrameName->setText(QString::fromStdString(vertexData.pose->frame));
- widget.editAgentName->setText(QString::fromStdString(vertexData.pose->agent));
-
- Eigen::Vector3f rpy;
- VirtualRobot::MathTools::eigen4f2rpy(vertexData.pose->toEigen(), rpy);
- widget.spinBoxX->setValue(vertexData.pose->position->x);
- widget.spinBoxY->setValue(vertexData.pose->position->y);
- widget.spinBoxZ->setValue(vertexData.pose->position->z);
-
- widget.spinBoxRoll->setValue(VirtualRobot::MathTools::rad2deg(rpy[0]));
- widget.spinBoxPitch->setValue(VirtualRobot::MathTools::rad2deg(rpy[1]));
- widget.spinBoxYaw->setValue(VirtualRobot::MathTools::rad2deg(rpy[2]));
-#endif
- }
-
#if 0
void LocationGraphEditorWidgetController::refreshGraph()
{
@@ -681,68 +670,19 @@ namespace armarx::nav::locgrapheditor
#endif
- void LocationGraphEditorWidgetController::transformView()
- {
- double d = widget.viewZoomFactor->value();
- widget.graphicsViewGraph->setTransform(QTransform::fromScale(d, d).rotate(view.angle));
- }
-
-
-#if 0
-
- void LocationGraphEditorWidgetController::viewRotatedClock()
+ void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
{
- viewAngle = std::fmod(viewAngle + VIEW_ROTATE_STEP_SIZE_CC, 360);
- transformView();
- }
-
-
- void LocationGraphEditorWidgetController::viewRotatedCounterClock()
- {
- viewAngle = std::fmod(viewAngle + 360 - VIEW_ROTATE_STEP_SIZE_CC, 360);
- transformView();
- }
-#endif
-
- void LocationGraphEditorWidgetController::adjustView()
- {
-#if 0
- float maxX = std::numeric_limits<float>::min();
- float minX = std::numeric_limits<float>::max();
- float maxY = std::numeric_limits<float>::min();
- float minY = std::numeric_limits<float>::max();
-
- // search bounding box
- for (const auto& vertex : vertices)
+ if (auto vertex = data.graph.getVertexFromTableItem(vertexItem))
{
- maxX = (maxX < vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : maxX;
- minX = (minX > vertex.attrib().pose->position->x) ? vertex.attrib().pose->position->x : minX;
- maxY = (maxY < vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : maxY;
- minY = (minY > vertex.attrib().pose->position->y) ? vertex.attrib().pose->position->y : minY;
+ selectVertex(vertex.value());
}
+ }
- auto deltaX = maxX - minX; // >=0
- auto deltaY = maxY - minY; // >=0
- // compare ratio of graph and view. if both horizontal (vertical) ->rotate to 0 or 180 (90,270)
- if (std::signbit(deltaX / deltaY - 1) == std::signbit(widget.graphicsViewGraph->width() / widget.graphicsViewGraph->height() - 1))
- {
- // same => rotate to 0 or 180
- viewAngle = (viewAngle < std::abs(180 - viewAngle)) ? 0 : 180;
- // set zoom => update
- widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaX,
- widget.graphicsViewGraph->height() / deltaY) * 0.9);
- }
- else
- {
- // different rotate to 90 or 270
- viewAngle = (std::abs(90 - viewAngle) < std::abs(270 - viewAngle)) ? 90 : 270;
- // set zoom => update
- widget.viewZoomFactor->setValue(std::min(widget.graphicsViewGraph->width() / deltaY,
- widget.graphicsViewGraph->height() / deltaX) * 0.9);
- }
-#endif
+ void LocationGraphEditorWidgetController::selectVertex(GuiGraph::Vertex vertex)
+ {
+ view.vertexData->setVertex(vertex);
}
@@ -916,7 +856,7 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::tableWidgetVerticesCustomContextMenu(QPoint pos)
{
- int row = dataWidgets.vertexTable->rowAt(pos.y());
+ int row = view.vertexTable->rowAt(pos.y());
auto vertexIt = std::find_if(vertices.begin(), vertices.end(), [&](const std::pair<std::string, VertexData>& d)
{
return d.attrib().tableWidgetVerticesIndex == row;
@@ -935,7 +875,7 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
{
- int row = dataWidgets.edgeTable->rowAt(pos.y());
+ int row = view.edgeTable->rowAt(pos.y());
auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
{
return d.attrib().tableWidgetEdgesIndex == row;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index d0630335..ded79af1 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -22,8 +22,8 @@
#pragma once
#include "FunctionalEventFilter.h"
-#include "GraphScene.h"
#include "GuiGraph.h"
+#include "widgets/graph_scene.h"
#include <armarx/navigation/graph/Graph.h>
@@ -142,18 +142,17 @@ namespace armarx::nav::locgrapheditor
// View & Tables
- void updateVertex(GuiGraph::Vertex vertex);
- void updateEdge(GuiGraph::Edge edge);
-
- /// Applies the current transforamtion to the view.
- void transformView();
- /// Adjusts the view's zoom and rotation to display most of the graph.
- void adjustView();
+ void updateVertexView(GuiGraph::Vertex vertex);
+ void updateEdgeView(GuiGraph::Edge edge);
// Selection
+ void selectVertex(QTableWidgetItem* vertexItem);
+ void selectVertex(GuiGraph::Vertex vertex);
+
void resetHighlighting();
+ void updateVertexHighlighting();
void toggleVertexSelected(const semrel::ShapeID& vertexID);
@@ -184,52 +183,30 @@ namespace armarx::nav::locgrapheditor
struct Data
{
armem::wm::Memory memory;
- nav::locgrapheditor::GuiGraph graph;
+ GuiGraph graph;
};
Data data;
- struct DataWidgets
+ struct View
{
VertexTableWidget* vertexTable = nullptr;
EdgeTableWidget* edgeTable = nullptr;
VertexDataWidget* vertexData = nullptr;
- };
- DataWidgets dataWidgets;
-
-
- struct GraphView
- {
- QGraphicsView* view = nullptr;
- /**
- * @brief The scene displayed in the widget.
- *
- * For y coordinates (-y) is used to mirror the scene on the y axis.
- * If (+y) would be used the graph displayed in the scene would not
- * match the graph drawn to the debug layer.
- */
- GraphScene* scene;
-
- /// The view's rotation angle.
- qreal angle = 0;
-
- void setScene(GraphScene* scene);
+ GraphSceneWidget* graph = nullptr;
};
- GraphView view;
-
+ View view;
// Non-refactored
-
#if 0
private slots:
void highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted = true);
void highlightVertex(const std::string& vertexId, bool highlighted = true);
- void redraw();
void refreshGraph();
void tableWidgetVerticesCustomContextMenu(QPoint pos);
@@ -281,8 +258,6 @@ namespace armarx::nav::locgrapheditor
void editGraphVertex();
#endif
- void setEditFields(GuiGraph::Vertex vertex);
-
private:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
new file mode 100644
index 00000000..5edd0120
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
@@ -0,0 +1,17 @@
+#pragma once
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+ class Scene;
+ class Widget;
+}
+
+namespace armarx::nav::locgrapheditor
+{
+
+ using GraphScene = graph_scene::Scene;
+ using GraphSceneWidget = graph_scene::Widget;
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
new file mode 100644
index 00000000..5549b56f
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
@@ -0,0 +1,107 @@
+#include "ControlWidget.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <QDoubleSpinBox>
+#include <QFrame>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QPushButton>
+
+#include <cmath>
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+
+ ControlWidget::ControlWidget()
+ {
+ _angle.turnClockwise = new QPushButton("\u21bb"); // https://unicode-table.com/de/21BB/
+ _angle.turnCounterClockwise = new QPushButton("\u21ba"); // https://unicode-table.com/de/21BA/
+ std::vector<QPushButton*> turnButtons;
+ for (QPushButton* button : turnButtons)
+ {
+ button->setMinimumWidth(button->minimumHeight());
+ button->setMaximumWidth(button->maximumHeight());
+ button->setSizePolicy(QSizePolicy::Maximum, QSizePolicy::Fixed);
+ }
+
+ _angle.value = new QDoubleSpinBox();
+ _angle.value->setMinimum(-360);
+ _angle.value->setMaximum(360);
+ _angle.value->setSingleStep(15);
+ _angle.value->setValue(0);
+
+ _zoom.value = new QDoubleSpinBox();
+ _zoom.value->setDecimals(5);
+ _zoom.value->setMinimum(1e-6);
+ _zoom.value->setSingleStep(0.01);
+ _zoom.value->setValue(0.1);
+ _zoom.audoAdjust = new QPushButton("Auto");
+
+
+ QHBoxLayout* layout = new QHBoxLayout();
+ this->setLayout(layout);
+
+ layout->addWidget(new QLabel("Angle: "));
+ layout->addWidget(_angle.turnClockwise);
+ layout->addWidget(_angle.value);
+ layout->addWidget(_angle.turnCounterClockwise);
+
+ // Vertical separator line.
+ QFrame* line = new QFrame();
+ line->setFrameShape(QFrame::VLine);
+ line->setFrameShadow(QFrame::Sunken);
+ layout->addWidget(line);
+
+ layout->addWidget(new QLabel("Zoom: "));
+ layout->addWidget(_zoom.value);
+ layout->addWidget(_zoom.audoAdjust);
+
+
+ // Connect
+
+ connect(_angle.value, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+ this, &This::angleChanged);
+
+ connect(_angle.turnClockwise, &QPushButton::pressed, [this]()
+ {
+ qreal newangle = std::fmod(angle() + _angle.rotateStepSize, 360.0);
+ ARMARX_IMPORTANT << VAROUT(newangle);
+ setAngle(newangle);
+ });
+ connect(_angle.turnCounterClockwise, &QPushButton::pressed, [this]()
+ {
+ setAngle(std::fmod(angle() - _angle.rotateStepSize, 360.0));
+ });
+
+ connect(_zoom.value, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+ this, &This::angleChanged);
+ connect(_zoom.audoAdjust, &QPushButton::pressed,
+ this, &This::zoomAutoAdjustRequested);
+ }
+
+
+ qreal ControlWidget::angle()
+ {
+ return _angle.value->value();
+ }
+
+ void ControlWidget::setAngle(qreal angle)
+ {
+ _angle.value->setValue(angle);
+ }
+
+
+ qreal ControlWidget::zoom()
+ {
+ return _zoom.value->value();
+ }
+
+ void ControlWidget::setZoom(qreal zoom)
+ {
+ _zoom.value->setValue(zoom);
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
new file mode 100644
index 00000000..64d4270e
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
@@ -0,0 +1,59 @@
+#pragma once
+
+#include <QWidget>
+
+class QDoubleSpinBox;
+class QHBoxLayout;
+class QPushButton;
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+
+ class ControlWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = ControlWidget;
+
+
+ public:
+
+ ControlWidget();
+
+ qreal angle();
+ void setAngle(qreal angle);
+ qreal zoom();
+ void setZoom(qreal zoom);
+
+
+ signals:
+
+ void angleChanged(qreal value);
+
+ void zoomChanged(qreal value);
+ void zoomAutoAdjustRequested();
+
+
+ public:
+
+ struct Angle
+ {
+ QPushButton* turnClockwise = nullptr;
+ QDoubleSpinBox* value = nullptr;
+ QPushButton* turnCounterClockwise = nullptr;
+
+ qreal rotateStepSize = 45;
+ };
+ Angle _angle;
+
+ struct Zoom
+ {
+ QDoubleSpinBox* value = nullptr;
+ QPushButton* audoAdjust = nullptr;
+ };
+ Zoom _zoom;
+
+ };
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
new file mode 100644
index 00000000..d66a24e5
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -0,0 +1,164 @@
+#include "Scene.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+
+#include <Eigen/Core>
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+
+ QGraphicsEllipseItem*
+ Scene::addVertex(const graph::Graph::Vertex& vertex)
+ {
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+
+ // To capture by copy
+ semrel::ShapeID vertexID = vertex.objectID();
+
+ GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
+ {
+ [this, vertexID]() { emit vertexSelected(vertexID); },
+ pose(0, 3), - pose(1, 3),
+ 1, 1
+ };
+ addItem(item);
+
+ // setToolTip on graphicsItem does not work
+ item->setZValue(std::numeric_limits<qreal>::max());
+ // dynamic_cast<QGraphicsItem*>(item)->setToolTip(QString::fromStdString("Vertex '" + name + "'"));
+ item->setToolTip(QString::fromStdString("Vertex '" + vertex.attrib().getName() + "'"));
+
+ return item;
+ }
+
+
+ QGraphicsLineItem*
+ Scene::addEdge(const graph::Graph::Edge& edge)
+ {
+ semrel::ShapeID sourceID = edge.sourceObjectID();
+ semrel::ShapeID targetID = edge.targetObjectID();
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
+ {
+ [this, sourceID, targetID]() { emit edgeSelected(sourceID, targetID); },
+ sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3)
+ };
+ addItem(item);
+
+ // setToolTip on item does not work
+ std::stringstream toolTip;
+ toolTip << "Edge '" << edge.source().attrib().getName()
+ << "' -> '" << edge.target().attrib().getName();
+ item->setToolTip(QString::fromStdString(toolTip.str()));
+
+ return item;
+ }
+
+
+ void Scene::updateVertex(GuiGraph::Vertex& vertex)
+ {
+ QGraphicsEllipseItem* item = vertex.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+
+ QColor color = vertex.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = vertex.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ item->setPen(QPen {color});
+ item->setBrush(QBrush {color});
+ item->setRect( pose(0, 3) - lineWidth * verticesScaleFactor / 2,
+ - pose(1, 3) - lineWidth * verticesScaleFactor / 2,
+ lineWidth * verticesScaleFactor,
+ lineWidth * verticesScaleFactor);
+ }
+
+
+ void Scene::updateEdge(GuiGraph::Edge& edge)
+ {
+ QGraphicsLineItem* item = edge.attrib().graphicsItem;
+ ARMARX_CHECK_NOT_NULL(item);
+
+ Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+
+ QColor color = edge.attrib().highlighted ? colorSelected : colorDefault;
+ double lineWidth = edge.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
+
+ QPen pen {color};
+ pen.setWidthF(lineWidth * lineScaleFactor);
+
+ item->setPen(pen);
+ item->setLine(sourcePose(0, 3), - sourcePose(1, 3),
+ targetPose(0, 3), - targetPose(1, 3));
+ }
+
+
+ void Scene::removeVertex(QGraphicsEllipseItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ void Scene::removeEdge(QGraphicsLineItem*& item)
+ {
+ removeItem(item);
+ delete item;
+ item = nullptr;
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y,
+ qreal width, qreal height,
+ QGraphicsItem* parent) :
+ QGraphicsEllipseItem {x, y, width, height, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsEllipseItem::~GraphVisualizerGraphicsEllipseItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+
+
+ GraphVisualizerGraphicsLineItem::GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1, qreal x2, qreal y2,
+ QGraphicsItem* parent) :
+ QGraphicsLineItem {x1, y1, x2, y2, parent},
+ onDoubleClicked{onDoubleClicked}
+ {
+ }
+
+
+ GraphVisualizerGraphicsLineItem::~GraphVisualizerGraphicsLineItem()
+ {
+ }
+
+
+ void GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ {
+ onDoubleClicked();
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
new file mode 100644
index 00000000..f1b1ac37
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -0,0 +1,118 @@
+#pragma once
+
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <armarx/navigation/graph/Graph.h>
+
+#include <QGraphicsScene>
+#include <QGraphicsEllipseItem>
+#include <QGraphicsLineItem>
+
+#include <functional>
+
+
+namespace semrel
+{
+ struct ShapeID;
+}
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+ class Scene : public QGraphicsScene
+ {
+ Q_OBJECT
+
+ public:
+
+ using QGraphicsScene::QGraphicsScene;
+
+ QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
+ QGraphicsLineItem* addEdge(const graph::Graph::Edge& Edge);
+
+ void updateVertex(GuiGraph::Vertex& vertex);
+ void updateEdge(GuiGraph::Edge& edge);
+
+ void removeVertex(QGraphicsEllipseItem*& item);
+ void removeEdge(QGraphicsLineItem*& item);
+
+
+ public slots:
+
+
+ signals:
+
+ void vertexSelected(const semrel::ShapeID& vertexID);
+ void edgeSelected(const semrel::ShapeID& sourceID, const semrel::ShapeID& targetID);
+
+
+ public:
+
+ double lineWidthDefault = 5;
+ double lineWidthSelected = 10;
+
+ double sceneScaleFactor = 2;
+ double verticesScaleFactor = 3.0 * sceneScaleFactor;
+ double lineScaleFactor = 1.0 * sceneScaleFactor;
+
+ QColor colorDefault = QColor::fromRgb(128, 128, 255);
+ QColor colorSelected = QColor::fromRgb(255, 128, 0);
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
+ {
+ public:
+
+ GraphVisualizerGraphicsEllipseItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x, qreal y, qreal width, qreal height,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsEllipseItem() override;
+
+
+ protected:
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+
+ /**
+ * @brief Required to override the double click event.
+ * This is required to toggle the select state.
+ */
+ class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
+ {
+ public:
+
+ GraphVisualizerGraphicsLineItem(
+ std::function<void(void)> onDoubleClicked,
+ qreal x1, qreal y1,
+ qreal x2, qreal y2,
+ QGraphicsItem* parent = nullptr);
+
+ ~GraphVisualizerGraphicsLineItem() override;
+
+
+ protected:
+
+
+ void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
+
+
+ std::function<void(void)> onDoubleClicked;
+
+ };
+
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
new file mode 100644
index 00000000..e05f7e6b
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
@@ -0,0 +1,83 @@
+#include "Widget.h"
+#include "Scene.h"
+#include "ControlWidget.h"
+
+#include <QGraphicsView>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QVBoxLayout>
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+
+ Widget::Widget()
+ {
+ _control = new ControlWidget();
+
+ _scene = new Scene();
+
+ _view = new QGraphicsView();
+ _view->setScene(_scene);
+ _view->setRenderHint(QPainter::Antialiasing, true);
+
+ int margin = 0;
+ this->setContentsMargins(margin, margin, margin, margin);
+ this->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
+
+ QHBoxLayout* header = new QHBoxLayout();
+ header->setSpacing(0);
+ header->addWidget(new QLabel("Graph"));
+ header->addStretch();
+ header->addWidget(_control);
+
+ QVBoxLayout* layout = new QVBoxLayout();
+ this->setLayout(layout);
+
+ layout->addLayout(header);
+ layout->addWidget(_view);
+
+
+ connect(_control, &ControlWidget::angleChanged,
+ this, &This::transform);
+ connect(_control, &ControlWidget::zoomChanged,
+ this, &This::transform);
+ connect(_control, &ControlWidget::zoomAutoAdjustRequested,
+ this, &This::autoAdjustZoom);
+
+
+ // Initial transform.
+ transform();
+ }
+
+
+ QGraphicsView* Widget::view()
+ {
+ return _view;
+ }
+
+
+ Scene* Widget::scene()
+ {
+ return _scene;
+ }
+
+
+ void Widget::transform()
+ {
+ double angle = _control->angle();
+ double zoom = _control->zoom();
+ _view->setTransform(QTransform::fromScale(zoom, zoom).rotate(angle));
+ }
+
+
+ void Widget::autoAdjustZoom()
+ {
+ int margin = 3;
+ QRectF rect = _scene->sceneRect() + QMargins(margin, margin, margin, margin);
+ _view->fitInView(rect, Qt::AspectRatioMode::KeepAspectRatio);
+ _control->setZoom(_view->transform().rotate(- _control->angle()).m11());
+ }
+
+}
+
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
new file mode 100644
index 00000000..c8a4b649
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
@@ -0,0 +1,58 @@
+#pragma once
+
+#include <QWidget>
+
+class QGraphicsView;
+
+
+namespace armarx::nav::locgrapheditor::graph_scene
+{
+ class ControlWidget;
+ class Scene;
+
+
+ class Widget : public QWidget
+ {
+ Q_OBJECT
+ using This = Widget;
+
+
+ public:
+
+ Widget();
+
+ QGraphicsView* view();
+ Scene* scene();
+
+
+ public slots:
+
+ void transform();
+ void autoAdjustZoom();
+
+
+ private:
+
+ /// Some buttons and input fields.
+ ControlWidget* _control = nullptr;
+
+ /// The "canvas".
+ QGraphicsView* _view = nullptr;
+
+ /**
+ * @brief The scene displayed in the widget.
+ *
+ * For y coordinates (-y) is used to mirror the scene on the y axis.
+ * If (+y) would be used the graph displayed in the scene would not
+ * match the graph drawn to the debug layer.
+ */
+ Scene* _scene;
+
+
+ /// The view's rotation angle.
+ qreal _angle = 0;
+
+ };
+
+}
+
--
GitLab
From 7c354b072259f10f796551923a59902263e2c199 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Aug 2021 16:25:37 +0200
Subject: [PATCH 16/33] Add ArViz visu, improve change handling
---
.../LocationGraphEditor/CMakeLists.txt | 5 +
.../LocationGraphEditor/GuiGraph.cpp | 43 +-
.../LocationGraphEditor/GuiGraph.h | 3 +-
.../LocationGraphEditorWidgetController.cpp | 369 ++++++------------
.../LocationGraphEditorWidgetController.h | 55 +--
.../gui-plugins/LocationGraphEditor/Visu.cpp | 121 ++++++
.../gui-plugins/LocationGraphEditor/Visu.h | 88 +++++
.../widgets/EdgeTableWidget.cpp | 19 +-
.../widgets/EdgeTableWidget.h | 5 +-
.../widgets/VertexDataWidget.cpp | 85 ++--
.../widgets/VertexDataWidget.h | 13 +-
.../widgets/VertexTableWidget.cpp | 53 ++-
.../widgets/VertexTableWidget.h | 4 +-
.../widgets/default_colors.cpp | 2 +-
.../widgets/graph_scene/Scene.cpp | 4 +-
.../widgets/graph_scene/Scene.h | 4 +-
.../widgets/graph_scene/Widget.cpp | 45 +++
.../LocationGraphEditor/widgets/utils.cpp | 53 +++
.../LocationGraphEditor/widgets/utils.h | 35 ++
19 files changed, 631 insertions(+), 375 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index d2661dba..0820ee1b 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -15,8 +15,10 @@ set(SOURCES
FunctionalEventFilter.cpp
GuiGraph.cpp
+ Visu.cpp
widgets/default_colors.cpp
+ widgets/utils.cpp
widgets/EdgeTableWidget.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
@@ -30,8 +32,10 @@ set(HEADERS
FunctionalEventFilter.h
GuiGraph.h
+ Visu.h
widgets/default_colors.h
+ widgets/utils.h
widgets/EdgeTableWidget.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
@@ -52,6 +56,7 @@ set(COMPONENT_LIBS
SimpleConfigDialog
# RobotAPI
+ ArViz
armem
Navigation::Location
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
index f4fd3e1d..49361f44 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -59,24 +59,47 @@ namespace armarx::nav::locgrapheditor
return map;
}
+
+ std::map<QTableWidgetItem*, GuiGraph::Edge>
+ GuiGraph::getTableItemToEdgeMap()
+ {
+ std::map<QTableWidgetItem*, GuiGraph::Edge> map;
+
+ for (auto edge : edges())
+ {
+ if (edge.attrib().tableWidgetItem != nullptr)
+ {
+ map[edge.attrib().tableWidgetItem] = edge;
+ }
+ }
+
+ return map;
+ }
+
}
namespace armarx::nav
{
-float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
-{
- Eigen::Vector3f rpy = simox::math::mat4f_to_rpy(pose);
- return simox::math::rad_to_deg(rpy[2]);
-}
+ float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+ {
+ return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(2));
+ }
-auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
-{
- GuiGraph gui;
- for (auto v : nav.vertices())
+
+ double locgrapheditor::getYawAngleDegree(const Eigen::Matrix4d& pose)
{
- gui.addVertex(v.objectID(), { v.attrib() });
+ return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(2));
}
+
+
+ auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+ {
+ GuiGraph gui;
+ for (auto v : nav.vertices())
+ {
+ gui.addVertex(v.objectID(), { v.attrib() });
+ }
for (auto e : nav.edges())
{
gui.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
index 87395a1f..33f4ddf5 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -86,7 +86,7 @@ namespace armarx::nav::locgrapheditor
std::optional<Vertex> getVertexFromTableItem(QTableWidgetItem* item);
std::map<QTableWidgetItem*, Vertex> getTableItemToVertexMap();
-
+ std::map<QTableWidgetItem*, Edge> getTableItemToEdgeMap();
};
@@ -96,5 +96,6 @@ namespace armarx::nav::locgrapheditor
float getYawAngleDegree(const Eigen::Matrix4f& pose);
+ double getYawAngleDegree(const Eigen::Matrix4d& pose);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index e23d6748..59de014f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -20,7 +20,10 @@
* GNU General Public License
*/
+#include <VirtualRobot/VirtualRobot.h>
+
#include "LocationGraphEditorWidgetController.h"
+#include "Visu.h"
#include "widgets/EdgeTableWidget.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
@@ -35,6 +38,7 @@
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -54,17 +58,6 @@
#include <sstream>
-/**
- * @brief The increment used when a rotation button is pressed.
- *
- * A positive rotation is counter clockwise.
- * This value should be positive.
- *
- * rotation buttons: LocationGraphEditorWidgetController::widget.buttonRotateClock and
- * LocationGraphEditorWidgetController::widget.buttonRotateCounterClock
- */
-static const float VIEW_ROTATE_STEP_SIZE_CC = 45;
-
static const QString SETTING_LAST_SCENE = "lastScene";
@@ -125,58 +118,36 @@ namespace armarx::nav::locgrapheditor
view.graph = new GraphSceneWidget();
widget.graphSceneLayout->addWidget(view.graph);
-#if 0
- auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
- {
- if (obj == this->view.view && event->type() == QEvent::MouseButtonPress)
- {
- QMouseEvent* me = static_cast<QMouseEvent*>(event);
- if (me->button() == Qt::LeftButton)
- {
- QPointF scenePoint = this->view.view->mapToScene(me->pos());
- scenePoint.setY(- scenePoint.y()); // not sure why
-
- float minDist = std::numeric_limits<float>::max();
- auto bestIt = this->vertices.cend();
-
- for (auto it = this->vertices.cbegin(); it != this->vertices.cend(); ++it)
- {
- float deltaX = it->second.pose->position->x - scenePoint.x();
- float deltaY = it->second.pose->position->y - scenePoint.y();
- float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
-
- if (dist < minDist)
- {
- minDist = dist;
- bestIt = it;
- }
- }
-
- if (bestIt != this->vertices.cend())
- {
- this->highlightVertex(bestIt->first);
- }
- }
- }
- else if (event->type() == QEvent::Resize)
- {
- this->adjustView();
- }
- else
- {
- return false;
- }
- };
- view.view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
-#endif
// Widgets -> This
// Memory Access
+ connect(this, &This::connected, this, &This::queryGraphs);
+ connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
+
connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
+ // Update views
+ connect(this, &This::graphChanged, this, &This::updateGraphView);
+ connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
+ this, &This::updateGraphView);
+
+ // Selection
+ connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
+ [this](QTableWidgetItem* current, QTableWidgetItem* previous)
+ {
+ (void) previous;
+ this->selectVertex(current);
+ });
+
+ connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
+ this, &This::updateVertexHighlighting);
+ connect(view.edgeTable, &EdgeTableWidget::itemSelectionChanged,
+ this, &This::updateEdgeHighlighting);
+
+
#if 0
// Tables
connect(view.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
@@ -190,30 +161,6 @@ namespace armarx::nav::locgrapheditor
connect(widget.btnAddEdgeEndStart, &QPushButton::pressed, this, &This::addNewEdgeEndStart);
connect(widget.btnEdit, &QPushButton::pressed, this, &This::editGraphVertex);
#endif
-
- // View
-
- // connect(view.graph->scene(), &GraphScene::vertexSelected, this, &This::toggleVertexSelected);
-
- // This -> This
-
- // Intra-connections.
- connect(this, &This::connected, this, &This::queryGraphs);
- connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
-
-
- connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
- this, &This::updateVertexView);
-
- connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
- [this](QTableWidgetItem* current, QTableWidgetItem* previous)
- {
- (void) previous;
- this->selectVertex(current);
- });
-
- connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
- this, &This::updateVertexHighlighting);
}
@@ -299,6 +246,8 @@ namespace armarx::nav::locgrapheditor
graphReader = memoryNameSystem.useReader(nav::graph::coreSegmentID);
graphWriter = memoryNameSystem.useWriter(nav::graph::coreSegmentID);
+
+ arviz = std::make_unique<viz::Client>(viz::Client::createForGuiPlugin(parent));
}
@@ -307,7 +256,7 @@ namespace armarx::nav::locgrapheditor
armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
if (result.success)
{
- data.memory = std::move(result.memory);
+ model.memory = std::move(result.memory);
emit memoryDataChanged();
}
else
@@ -322,7 +271,7 @@ namespace armarx::nav::locgrapheditor
widget.graphsComboBox->clear();
bool enable = false;
- data.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
+ model.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
{
widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
enable = true;
@@ -333,18 +282,8 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::loadGraph()
{
- // Store vertex highlighting.
- std::vector<semrel::ShapeID> highlightedVertices;
- for (auto vertex : data.graph.vertices())
- {
- if (vertex.attrib().highlighted)
- {
- highlightedVertices.push_back(vertex.objectID());
- }
- }
-
const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
- const armem::wm::EntityInstance* instance = data.memory.findLatestInstance(entityID);
+ const armem::wm::EntityInstance* instance = model.memory.findLatestInstance(entityID);
if (not instance)
{
std::stringstream ss;
@@ -359,30 +298,39 @@ namespace armarx::nav::locgrapheditor
fromAron(dto, nav);
}
- clearGraph();
+ setGraph(nav);
+ }
+
+
+ void LocationGraphEditorWidgetController::setGraph(graph::Graph& nav)
+ {
+ // Store vertex highlighting.
+ std::set<std::string> highlightedVertices;
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().highlighted)
+ {
+ highlightedVertices.insert(vertex.attrib().getName());
+ }
+ }
- // Add vertices
+ // Build graph.
+ clearGraph();
for (auto vertex : nav.vertices())
{
addVertex(vertex);
}
-
- // Add edges
for (auto edge : nav.edges())
{
addEdge(edge);
}
// Restore vertex highlighting.
- for (const semrel::ShapeID& vertexID : highlightedVertices)
+ for (auto vertex : model.graph.vertices())
{
- for (auto vertex : data.graph.vertices())
+ if (highlightedVertices.count(vertex.attrib().getName()))
{
- if (vertex.objectID() == vertexID)
- {
- vertex.attrib().highlighted = true;
- updateVertexView(vertex);
- }
+ vertex.attrib().highlighted = true;
}
}
@@ -391,42 +339,52 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Vertex
- LocationGraphEditorWidgetController::addVertex(graph::Graph::Vertex vertex)
+ LocationGraphEditorWidgetController::addVertex(graph::Graph::ConstVertex vertex)
{
- ARMARX_CHECK(not data.graph.hasVertex(vertex.objectID()));
+ ARMARX_CHECK(not model.graph.hasVertex(vertex.objectID()));
VertexData attrib { vertex.attrib() };
attrib.graphicsItem = view.graph->scene()->addVertex(vertex);
attrib.tableWidgetItem = view.vertexTable->addVertex(vertex);
- auto guiVertex = data.graph.addVertex(vertex.objectID(), attrib);
-
- updateVertexView(guiVertex);
- return guiVertex;
+ return model.graph.addVertex(vertex.objectID(), attrib);
}
GuiGraph::Edge
- LocationGraphEditorWidgetController::addEdge(graph::Graph::Edge edge)
+ LocationGraphEditorWidgetController::addEdge(graph::Graph::ConstEdge edge)
{
- ARMARX_CHECK(not data.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
+ ARMARX_CHECK(not model.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
<< "Edge must not exist before being added: '"
<< edge.source().attrib().getName() << "' -> '"
<< edge.target().attrib().getName() << "'";
- auto source = data.graph.vertex(edge.sourceObjectID());
- auto target = data.graph.vertex(edge.targetObjectID());
+ auto source = model.graph.vertex(edge.sourceObjectID());
+ auto target = model.graph.vertex(edge.targetObjectID());
EdgeData attrib { edge.attrib() };
attrib.tableWidgetItem = view.edgeTable->addEdge(edge);
attrib.graphicsItem = view.graph->scene()->addEdge(edge);
- auto guiEdge = data.graph.addEdge(source, target, attrib);
+ return model.graph.addEdge(source, target, attrib);
+ }
+
- updateEdgeView(guiEdge);
- return guiEdge;
+
+ void LocationGraphEditorWidgetController::updateGraphView()
+ {
+ for (auto vertex : model.graph.vertices())
+ {
+ updateVertexView(vertex);
+ }
+ for (auto edge : model.graph.edges())
+ {
+ updateEdgeView(edge);
+ }
+ updateArViz();
}
- void LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
+ void
+ LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
{
view.graph->scene()->updateVertex(vertex);
view.vertexTable->updateVertex(vertex);
@@ -435,7 +393,7 @@ namespace armarx::nav::locgrapheditor
{
// Highlight all edges between highlighted vertices
std::set<semrel::ShapeID> highlightedVertices;
- for (auto v : data.graph.vertices())
+ for (auto v : model.graph.vertices())
{
if (v.attrib().highlighted)
{
@@ -443,7 +401,7 @@ namespace armarx::nav::locgrapheditor
}
}
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
bool verticesHighlighted =
highlightedVertices.count(edge.sourceObjectID())
@@ -454,54 +412,36 @@ namespace armarx::nav::locgrapheditor
if (edge.attrib().highlighted != verticesHighlighted)
{
edge.attrib().highlighted = verticesHighlighted;
- updateEdgeView(edge);
}
}
}
-
- // ToDo: Update ArViz
-#if 0
- float yaw = getYawAngleDegree(attrib.pose) / 180 * M_PI;
- Eigen::AngleAxisf aa(yaw, Eigen::Vector3f(0, 0, 1));
- Eigen::Vector3f dir {0, 1, 0};
- dir = aa.toRotationMatrix() * dir;
- debugDrawer->setArrowVisu(debugDrawerLayerName, iceName(id), attrib.pose->position,
- new armarx::Vector3(dir),
- armarx::DrawColor {0, 0, 1, 1},
- 100,
- lineWidth);
- debugDrawer->setTextVisu(debugDrawerLayerName, iceName(id) + "text", attrib.vertex->getName(),
- attrib.pose->position, armarx::DrawColor {0, 0, 1, 1}, 10);
-#endif
}
- void LocationGraphEditorWidgetController::updateEdgeView(GuiGraph::Edge edge)
+ void
+ LocationGraphEditorWidgetController::updateEdgeView(GuiGraph::Edge edge)
{
view.graph->scene()->updateEdge(edge);
view.edgeTable->updateEdge(edge);
+ }
-#if 0
- armarx::Vector3Ptr posStart = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.first).pose->position)->toEigen());
- posStart->z += posStart->z < 1 ? 10 : 0;
- armarx::Vector3Ptr posEnd = new armarx::Vector3(armarx::Vector3Ptr::dynamicCast(vertices.at(id.attrib()).pose->position)->toEigen());
- posEnd->z += posEnd->z < 1 ? 10 : 0;
- // debug layer
- // ToDo: Update ArViz
- debugDrawer->setLineVisu(debugDrawerLayerName,
- iceName(id),
- posStart,
- posEnd,
- lineWidth,
- color);
-#endif
+
+ void LocationGraphEditorWidgetController::updateArViz()
+ {
+ if (remote.arviz)
+ {
+ viz::Layer layer = remote.arviz->layer(widget.graphsComboBox->currentText().toStdString());
+ GraphVisu visu;
+ visu.draw(layer, model.graph);
+ remote.arviz->commit(layer);
+ }
}
void LocationGraphEditorWidgetController::clearEdges()
{
// Remove from graphics scene
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
}
@@ -511,12 +451,12 @@ namespace armarx::nav::locgrapheditor
view.edgeTable->setRowCount(0);
// Clear data structure
- while (data.graph.edges().begin() != data.graph.edges().end())
+ while (model.graph.edges().begin() != model.graph.edges().end())
{
- data.graph.removeEdge(*data.graph.edges().begin());
+ model.graph.removeEdge(*model.graph.edges().begin());
}
- // ToDo: Update ArViz
+ emit graphChanged();
}
@@ -532,148 +472,71 @@ namespace armarx::nav::locgrapheditor
view.vertexTable->setRowCount(0);
// Clear data structure
- data.graph.clear();
- }
-
+ model.graph.clear();
- void LocationGraphEditorWidgetController::toggleVertexSelected(const semrel::ShapeID& vertexID)
- {
- auto vertex = data.graph.vertex(vertexID);
- vertex.attrib().highlighted ^= true;
- updateVertexView(vertex);
+ emit graphChanged();
}
void LocationGraphEditorWidgetController::resetHighlighting()
{
- for (auto vertex : data.graph.vertices())
+ for (auto vertex : model.graph.vertices())
{
if (vertex.attrib().highlighted)
{
vertex.attrib().highlighted = false;
- updateVertexView(vertex);
}
}
- for (auto edge : data.graph.edges())
+ for (auto edge : model.graph.edges())
{
if (edge.attrib().highlighted)
{
edge.attrib().highlighted = false;
- updateEdgeView(edge);
}
}
+
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ template <class T>
+ static
+ void updateElementHighlighting(
+ QList<QTableWidgetItem*> selectedItems,
+ std::map<QTableWidgetItem*, T>&& itemToElementMap)
{
- std::map<QTableWidgetItem*, GuiGraph::Vertex> map = data.graph.getTableItemToVertexMap();
-
- for (QTableWidgetItem* selected : view.vertexTable->selectedEdgeItems())
+ for (QTableWidgetItem* selected : selectedItems)
{
- if (auto it = map.find(selected); it != map.end())
+ if (auto it = itemToElementMap.find(selected); it != itemToElementMap.end())
{
it->second.attrib().highlighted = true;
- updateVertexView(it->second);
- map.erase(it);
+ itemToElementMap.erase(it);
}
}
- for (auto& [_, unselected] : map)
+ for (auto& [_, unselected] : itemToElementMap)
{
unselected.attrib().highlighted = false;
- updateVertexView(unselected);
}
}
-#if 0
-
-
- void LocationGraphEditorWidgetController::highlightVertex(const std::string& vertexId, bool highlighted)
- {
- if (!hasVertex(vertexId))
- {
- ARMARX_WARNING << "No vertex: " << vertexId << " [pushBfile: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
- return;
- }
-
- if (vertices.at(vertexId).highlighted != highlighted)
- {
- vertices.at(vertexId).highlighted = highlighted;
- updateVertex(vertexId);
- }
- }
-
-
- void LocationGraphEditorWidgetController::highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted)
- {
- if (!hasEdge(vertex1Id, vertex2Id))
- {
- ARMARX_WARNING << "No edge for: " << vertex1Id << " and " << vertex2Id << " [file: " << __FILE__ << " | line: " << __LINE__ << " | function: " << __PRETTY_FUNCTION__ << "]";
- return;
- }
-
- EdgeId edge = toEdge(vertex1Id, vertex2Id);
-
- if (edges.at(edge).highlighted != highlighted)
- {
- edges.at(edge).highlighted = highlighted;
- updateEdge(edge);
- }
- }
-
-
- void LocationGraphEditorWidgetController::vertexTableDoubleClicked(int row, int)
- {
- auto vertexIt = std::find_if(vertices.cbegin(), vertices.cend(), [&](const std::pair<std::string, VertexData>& d)
- {
- // return d.attrib().vertex->getName() == view.vertexTable->item(row, 0)->text().toStdString();
- return d.attrib().tableWidgetVerticesIndex == row;
- });
- auto vertexId = vertexIt->second.vertex->getId();
- highlightVertex(vertexId);
- }
-
-
- void LocationGraphEditorWidgetController::edgeTableDoubleClicked(int row, int)
- {
- auto edgeIt = std::find_if(edges.cbegin(), edges.cend(), [&](const std::pair<EdgeId, EdgeData>& d)
- {
- return d.attrib().tableWidgetEdgesIndex == row;
- });
-
- edgeDoubleClicked(edgeIt->first);
- }
-
-
-
- void LocationGraphEditorWidgetController::edgeDoubleClicked(GuiGraph::Edge edge)
+ void LocationGraphEditorWidgetController::updateVertexHighlighting()
{
- // edges.at(id).highlighted ^= true;
- // updateEdge(id);
- bool highlight = !vertices.at(edge.first).highlighted && !vertices.at(edge.attrib()).highlighted;
- vertices.at(edge.first).highlighted = highlight;
- vertices.at(edge.attrib()).highlighted = highlight;
- updateVertex(edge.first);
- updateVertex(edge.attrib());
+ updateElementHighlighting(view.vertexTable->selectedVertexItems(), model.graph.getTableItemToVertexMap());
+ emit graphChanged();
}
-#endif
-
-#if 0
- void LocationGraphEditorWidgetController::refreshGraph()
+ void LocationGraphEditorWidgetController::updateEdgeHighlighting()
{
- clearGraph();
- loadGraph();
+ updateElementHighlighting(view.edgeTable->selectedEdgeItems(), model.graph.getTableItemToEdgeMap());
+ emit graphChanged();
}
-#endif
-
void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
{
- if (auto vertex = data.graph.getVertexFromTableItem(vertexItem))
+ if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
{
selectVertex(vertex.value());
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index ded79af1..fe161065 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -41,12 +41,17 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <memory>
#include <string>
class QDialog;
+namespace armarx::viz
+{
+ class Client;
+}
namespace armarx::nav::locgrapheditor
{
class EdgeTableWidget;
@@ -133,8 +138,9 @@ namespace armarx::nav::locgrapheditor
void updateGraphList();
void loadGraph();
- GuiGraph::Vertex addVertex(graph::Graph::Vertex vertex);
- GuiGraph::Edge addEdge(graph::Graph::Edge edge);
+ void setGraph(graph::Graph& nav);
+ GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
+ GuiGraph::Edge addEdge(graph::Graph::ConstEdge edge);
void clearEdges();
void clearGraph();
@@ -142,8 +148,10 @@ namespace armarx::nav::locgrapheditor
// View & Tables
+ void updateGraphView();
void updateVertexView(GuiGraph::Vertex vertex);
void updateEdgeView(GuiGraph::Edge edge);
+ void updateArViz();
// Selection
@@ -153,7 +161,7 @@ namespace armarx::nav::locgrapheditor
void resetHighlighting();
void updateVertexHighlighting();
- void toggleVertexSelected(const semrel::ShapeID& vertexID);
+ void updateEdgeHighlighting();
private:
@@ -174,18 +182,19 @@ namespace armarx::nav::locgrapheditor
armem::client::Reader graphReader;
armem::client::Writer graphWriter;
+ std::unique_ptr<viz::Client> arviz;
void connect(Component& parent);
};
Remote remote;
- struct Data
+ struct Model
{
armem::wm::Memory memory;
GuiGraph graph;
};
- Data data;
+ Model model;
struct View
@@ -204,11 +213,6 @@ namespace armarx::nav::locgrapheditor
#if 0
private slots:
- void highlightEdge(const std::string& vertex1Id, const std::string& vertex2Id, bool highlighted = true);
- void highlightVertex(const std::string& vertexId, bool highlighted = true);
-
- void refreshGraph();
-
void tableWidgetVerticesCustomContextMenu(QPoint pos);
void tableWidgetEdgesCustomContextMenu(QPoint pos);
@@ -218,37 +222,6 @@ namespace armarx::nav::locgrapheditor
/// Add kitchen graph (H2T Armar3a robot kitchen)
void addKitchenGraph();
- /**
- * @brief Toggles the double clicked vertex's selection state.
- * @param row Identifies the vertex.
- */
- void vertexTableDoubleClicked(int row, int);
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param row Identifies the edge.
- */
- void edgeTableDoubleClicked(int row, int);
-
- /**
- * @brief Toggles the double clicked edge's selection state.
- * @param id Identifies the edge.
- */
- void edgeDoubleClicked(GuiGraph::Edge edge);
-
- /**
- * @brief Rotates the view clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedClock();
-
- /**
- * @brief Rotates the view counter clockwise.
- *
- * Stepsize set by VIEW_ROTATE_STEP_SIZE_CC in GraphVisualizerGuiPlugin.cpp
- */
- void viewRotatedCounterClock();
-
bool addNewEdge(const std::string& from, const std::string& to);
void addNewEdgeBoth();
void addNewEdgeStartEnd();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
new file mode 100644
index 00000000..d978ffb6
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -0,0 +1,121 @@
+/*
+ * 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/>.
+ *
+ * @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 <VirtualRobot/VirtualRobot.h>
+
+#include "Visu.h"
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ const simox::Color visu::defaultColorHighlighted = simox::Color::orange();
+
+
+ viz::Pose VertexVisu::Pose::draw(const VertexData& attribs) const
+ {
+ viz::Pose pose = nav::graph::VertexVisu::Pose::draw(attribs);
+ if (attribs.highlighted)
+ {
+ pose.scale(scale * scaleFactorHighlighted);
+ }
+ return pose;
+ }
+
+
+ viz::Arrow VertexVisu::ForwardArrow::draw(const VertexData& attribs) const
+ {
+ viz::Arrow arrow = nav::graph::VertexVisu::ForwardArrow::draw(attribs);
+ if (attribs.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ arrow.width(width * 1.5f);
+ }
+ return arrow;
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, const VertexData& attribs) const
+ {
+ if (pose.has_value())
+ {
+ layer.add(pose->draw(attribs));
+ }
+ if (forwardArrow.has_value())
+ {
+ layer.add(forwardArrow->draw(attribs));
+ }
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(GuiGraph::ConstEdge edge) const
+ {
+ return draw(edge.attrib(), edge.source().attrib(), edge.target().attrib());
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(
+ const EdgeData& edge,
+ const VertexData& source,
+ const VertexData& target) const
+ {
+ viz::Arrow arrow = nav::graph::EdgeVisu::Arrow::draw(edge, source, target);
+ if (edge.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ }
+ return arrow;
+ }
+
+
+ void EdgeVisu::draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const
+ {
+ if (arrow.has_value())
+ {
+ layer.add(arrow->draw(edge));
+ }
+ }
+
+
+ void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph)
+ {
+ if (vertex.has_value())
+ {
+ for (GuiGraph::ConstVertex v : graph.vertices())
+ {
+ vertex->draw(layer, v.attrib());
+ }
+ }
+ if (edge.has_value())
+ {
+ for (GuiGraph::ConstEdge e : graph.edges())
+ {
+ edge->draw(layer, e);
+ }
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
new file mode 100644
index 00000000..5e77c8be
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -0,0 +1,88 @@
+/*
+ * 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/>.
+ *
+ * @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 "GuiGraph.h"
+
+#include <armarx/navigation/graph/Visu.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include <optional>
+
+
+namespace armarx::nav::locgrapheditor::visu
+{
+ extern const simox::Color defaultColorHighlighted;
+}
+namespace armarx::nav::locgrapheditor
+{
+
+ struct VertexVisu
+ {
+ struct Pose : public nav::graph::VertexVisu::Pose
+ {
+ float scaleFactorHighlighted = 1.5;
+
+ viz::Pose draw(const VertexData& attribs) const;
+ };
+ std::optional<Pose> pose = Pose {};
+
+ struct ForwardArrow : public nav::graph::VertexVisu::ForwardArrow
+ {
+ simox::Color colorHighlighted = visu::defaultColorHighlighted;
+
+ viz::Arrow draw(const VertexData& attribs) const;
+ };
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+
+
+ void draw(viz::Layer& layer, const VertexData& attribs) const;
+ };
+
+
+ struct EdgeVisu
+ {
+ struct Arrow : public nav::graph::EdgeVisu::Arrow
+ {
+ simox::Color colorHighlighted = visu::defaultColorHighlighted;
+
+ viz::Arrow draw(GuiGraph::ConstEdge edge) const;
+ viz::Arrow draw(const EdgeData& edge, const VertexData& source, const VertexData& target) const;
+ };
+ std::optional<Arrow> arrow = Arrow {};
+
+
+ void draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const;
+ };
+
+
+ struct GraphVisu
+ {
+ std::optional<VertexVisu> vertex = VertexVisu {};
+ std::optional<EdgeVisu> edge = EdgeVisu {};
+
+
+ void draw(viz::Layer& layer, const GuiGraph& graph);
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 09a1b2b1..6c60fac3 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -21,6 +21,10 @@
#include "EdgeTableWidget.h"
+#include "utils.h"
+
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <QHeaderView>
@@ -33,17 +37,23 @@ namespace armarx::nav::locgrapheditor
setColumnCount(columns.size());
setHorizontalHeaderLabels(columns);
horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setResizeMode(1, QHeaderView::Stretch);
horizontalHeader()->setVisible(true);
setEditTriggers(QAbstractItemView::NoEditTriggers);
setSortingEnabled(true);
+ QString styleSheet = this->styleSheet();
+ ARMARX_IMPORTANT << VAROUT(styleSheet.toStdString());
+ styleSheet = styleSheet + "\n" + "selection-background-color: orange;";
+ this->setStyleSheet(styleSheet);
+
setContextMenuPolicy(Qt::CustomContextMenu);
}
QTableWidgetItem*
- EdgeTableWidget::addEdge(graph::Graph::Edge edge)
+ EdgeTableWidget::addEdge(graph::Graph::ConstEdge edge)
{
int row = rowCount();
setRowCount(row + 1);
@@ -73,4 +83,11 @@ namespace armarx::nav::locgrapheditor
}
+ QList<QTableWidgetItem*>
+ EdgeTableWidget::selectedEdgeItems()
+ {
+ return utils::getSelectedItemsOfColumn(this, 0);
+ }
+
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index 0991665d..bf6a496f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -42,7 +42,7 @@ namespace armarx::nav::locgrapheditor
EdgeTableWidget();
- QTableWidgetItem* addEdge(graph::Graph::Edge edge);
+ QTableWidgetItem* addEdge(graph::Graph::ConstEdge edge);
void updateEdge(GuiGraph::Edge edge);
@@ -51,6 +51,9 @@ namespace armarx::nav::locgrapheditor
void clearEdges();
+ QList<QTableWidgetItem*> selectedEdgeItems();
+
+
private slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index e79324ea..d34dab04 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -68,18 +68,11 @@ namespace armarx::nav::locgrapheditor
pos->setSuffix(" mm");
pos->setMinimum(-1e6);
pos->setMaximum(+1e6);
- pos->setSingleStep(10);
- pos->setValue(0);
- }
- for (QDoubleSpinBox* angle : _angleSpinBoxes())
- {
- angle->setSuffix("");
- angle->setMinimum(-360);
- angle->setMaximum(+360);
- angle->setValue(0);
+ pos->setSingleStep(50);
}
for (QDoubleSpinBox* spinBox : _allSpinBoxes())
{
+ spinBox->setValue(0);
spinBox->setAlignment(Qt::AlignRight);
}
@@ -107,21 +100,38 @@ namespace armarx::nav::locgrapheditor
for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
{
- connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
+ connect(angleUnit, &QRadioButton::toggled,
+ this, &This::_updateAngleUnit);
+ }
+
+ connect(frame, &QLineEdit::editingFinished,
+ this, &This::_updateVertexAttribs);
+ for (QDoubleSpinBox* spinBox : _allSpinBoxes())
+ {
+ connect(spinBox, QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
+ this, &This::_updateVertexAttribs);
}
+
angleUnitDeg->click();
}
+ std::optional<GuiGraph::Vertex> VertexDataWidget::vertex()
+ {
+ return _vertex;
+ }
+
+
void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
{
- setFromVertex(vertex);
+ _vertex = vertex;
+ _setFromVertex(vertex);
setEnabled(true);
}
- void VertexDataWidget::setFromVertex(const GuiGraph::Vertex& vertex)
+ void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
{
const VertexData& attrib = vertex.attrib();
const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
@@ -133,7 +143,7 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::getToVertex(GuiGraph::Vertex& vertex)
+ void VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
{
VertexData& attrib = vertex.attrib();
@@ -181,33 +191,60 @@ namespace armarx::nav::locgrapheditor
void VertexDataWidget::_updateAngleUnit()
{
- QString suffix;
- if (angleUnitDeg->isChecked())
- {
- suffix = " \u00b0";
- }
- else if (angleUnitRad->isChecked())
+ std::function<double(double)> convertValue;
+ QString suffix = " \u00b0";
+ double min = -360;
+ double max = +360;
+ double step = 5.;
+ int decimals = 2;
+
+ if (angleUnitRad->isChecked())
{
+ convertValue = [](double deg)
+ {
+ return simox::math::deg_to_rad(deg);
+ };
suffix = " rad";
+ min = simox::math::deg_to_rad(min);
+ max = simox::math::deg_to_rad(max);
+ step = simox::math::deg_to_rad(step);
+ decimals = 3;
}
else
{
- return;
+ ARMARX_CHECK(angleUnitDeg->isChecked());
+ convertValue = [](double rad)
+ {
+ return simox::math::rad_to_deg(rad);
+ };
}
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
+ angle->blockSignals(true);
+
angle->setSuffix(suffix);
+ angle->setMinimum(min);
+ angle->setMaximum(max);
+ angle->setSingleStep(step);
+ angle->setDecimals(decimals);
+ }
+ if (_vertex.has_value())
+ {
+ _setRpyRad(simox::math::mat4f_to_rpy(_vertex->attrib().getPose().cast<qreal>()));
+ }
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ angle->blockSignals(false);
}
}
void VertexDataWidget::_updateVertexAttribs()
{
- if (vertex.has_value())
+ if (_vertex.has_value())
{
- getToVertex(vertex.value());
-
- emit vertexDataChanged(vertex.value());
+ _getToVertex(_vertex.value());
+ emit vertexDataChanged();
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index 75d17e33..64b7971e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -46,11 +46,9 @@ namespace armarx::nav::locgrapheditor
VertexDataWidget();
- void setVertex(GuiGraph::Vertex vertex);
-
- void setFromVertex(const GuiGraph::Vertex& vertex);
- void getToVertex(GuiGraph::Vertex& vertex);
+ std::optional<GuiGraph::Vertex> vertex();
+ void setVertex(GuiGraph::Vertex vertex);
Eigen::Vector3d xyz() const;
Eigen::Vector3d rpyDeg() const;
@@ -59,7 +57,7 @@ namespace armarx::nav::locgrapheditor
signals:
- void vertexDataChanged(GuiGraph::Vertex vertex);
+ void vertexDataChanged();
private slots:
@@ -70,6 +68,9 @@ namespace armarx::nav::locgrapheditor
private:
+ void _setFromVertex(const GuiGraph::Vertex& vertex);
+ void _getToVertex(GuiGraph::Vertex& vertex);
+
std::vector<QDoubleSpinBox*> _positionSpinBoxes();
std::vector<QDoubleSpinBox*> _angleSpinBoxes();
std::vector<QDoubleSpinBox*> _allSpinBoxes();
@@ -84,7 +85,7 @@ namespace armarx::nav::locgrapheditor
private:
- std::optional<GuiGraph::Vertex> vertex;
+ std::optional<GuiGraph::Vertex> _vertex;
QLineEdit* locationID = nullptr;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 7df6d1b7..c06f36de 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -20,6 +20,7 @@
*/
#include "VertexTableWidget.h"
+#include "utils.h"
#include <QHeaderView>
@@ -29,7 +30,7 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget::VertexTableWidget()
{
- QStringList columns{"Name", "X", "Y", "Yaw"};
+ QStringList columns{"Name", "X [mm]", "Y [mm]", "Yaw [\u00b0]"};
setColumnCount(columns.size());
setHorizontalHeaderLabels(columns);
horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
@@ -45,20 +46,18 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem*
- VertexTableWidget::addVertex(graph::Graph::Vertex vertex)
+ VertexTableWidget::addVertex(graph::Graph::ConstVertex vertex)
{
- Eigen::Matrix4f pose = vertex.attrib().getPose();
-
- char format = 'f';
- const int precision = 2;
+ (void) vertex;
int row = rowCount();
setRowCount(row + 1);
- setItem(row, 0, new QTableWidgetItem {QString::fromStdString(vertex.attrib().getName())});
- setItem(row, 1, new QTableWidgetItem {QString::number(qreal(pose(0, 3)), format, precision)});
- setItem(row, 2, new QTableWidgetItem {QString::number(qreal(pose(1, 3)), format, precision)});
- setItem(row, 3, new QTableWidgetItem {QString::number(qreal(getYawAngleDegree(pose)), format, precision)});
+ // Just fill with vanilla items, they will get values in the update.
+ for (int col = 0; col < 4; ++col)
+ {
+ setItem(row, col, new QTableWidgetItem {});
+ }
for (int col = 1; col <= 3; ++col)
{
item(row, col)->setTextAlignment(Qt::AlignRight | Qt::AlignVCenter);
@@ -71,11 +70,20 @@ namespace armarx::nav::locgrapheditor
void
VertexTableWidget::updateVertex(GuiGraph::Vertex vertex)
{
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+ char format = 'f';
+ const int precision = 2;
+
+ int row = this->row(vertex.attrib().tableWidgetItem);
+ item(row, 0)->setText(QString::fromStdString(vertex.attrib().getName()));
+ item(row, 1)->setText(QString::number(pose(0, 3), format, precision));
+ item(row, 2)->setText(QString::number(pose(1, 3), format, precision));
+ item(row, 3)->setText(QString::number(getYawAngleDegree(pose), format, precision));
+
+
QColor bgColor = vertex.attrib().highlighted ? bgColorSelected : bgColorDefault;
QFont font;
font.setBold(vertex.attrib().highlighted);
-
- int row = this->row(vertex.attrib().tableWidgetItem);
for (int col = 0; col < 4; ++col)
{
auto* item = this->item(row, col);
@@ -87,27 +95,10 @@ namespace armarx::nav::locgrapheditor
}
-
QList<QTableWidgetItem*>
- VertexTableWidget::selectedEdgeItems()
+ VertexTableWidget::selectedVertexItems()
{
- // Only return items from the first column (and avoid duplicates).
- std::set<QTableWidgetItem*> set;
- for (QTableWidgetItem* selected : selectedItems())
- {
- if (column(selected) != 0)
- {
- selected = item(row(selected), 0);
- }
- set.insert(selected);
- }
-
- QList<QTableWidgetItem*> list;
- for (auto i : set)
- {
- list.append(i);
- }
- return list;
+ return utils::getSelectedItemsOfColumn(this, 0);
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 6a8f24a7..7fea8530 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -42,12 +42,12 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
- QTableWidgetItem* addVertex(graph::Graph::Vertex vertex);
+ QTableWidgetItem* addVertex(graph::Graph::ConstVertex vertex);
void updateVertex(GuiGraph::Vertex vertex);
- QList<QTableWidgetItem*> selectedEdgeItems();
+ QList<QTableWidgetItem*> selectedVertexItems();
public slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
index 059cb71c..fd026201 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
@@ -28,6 +28,6 @@ namespace armarx::nav::locgrapheditor
{
const QColor default_colors::tableBackgroundDefault = QColor::fromRgb(255, 255, 255);
- const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(128, 196, 255);
+ const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(255, 210, 160);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
index d66a24e5..32c33382 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -11,7 +11,7 @@ namespace armarx::nav::locgrapheditor::graph_scene
{
QGraphicsEllipseItem*
- Scene::addVertex(const graph::Graph::Vertex& vertex)
+ Scene::addVertex(graph::Graph::ConstVertex vertex)
{
const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
@@ -36,7 +36,7 @@ namespace armarx::nav::locgrapheditor::graph_scene
QGraphicsLineItem*
- Scene::addEdge(const graph::Graph::Edge& edge)
+ Scene::addEdge(graph::Graph::ConstEdge edge)
{
semrel::ShapeID sourceID = edge.sourceObjectID();
semrel::ShapeID targetID = edge.targetObjectID();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
index f1b1ac37..95965d34 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -24,8 +24,8 @@ namespace armarx::nav::locgrapheditor::graph_scene
using QGraphicsScene::QGraphicsScene;
- QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
- QGraphicsLineItem* addEdge(const graph::Graph::Edge& Edge);
+ QGraphicsEllipseItem* addVertex(graph::Graph::ConstVertex vertex);
+ QGraphicsLineItem* addEdge(graph::Graph::ConstEdge Edge);
void updateVertex(GuiGraph::Vertex& vertex);
void updateEdge(GuiGraph::Edge& edge);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
index e05f7e6b..7cdcbcd8 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
@@ -48,6 +48,51 @@ namespace armarx::nav::locgrapheditor::graph_scene
// Initial transform.
transform();
+
+#if 0
+ auto eventFilter = [this](QObject* obj, QEvent* event) -> bool
+ {
+ if (obj == this->view.view && event->type() == QEvent::MouseButtonPress)
+ {
+ QMouseEvent* me = static_cast<QMouseEvent*>(event);
+ if (me->button() == Qt::LeftButton)
+ {
+ QPointF scenePoint = this->view.view->mapToScene(me->pos());
+ scenePoint.setY(- scenePoint.y()); // not sure why
+
+ float minDist = std::numeric_limits<float>::max();
+ auto bestIt = this->vertices.cend();
+
+ for (auto it = this->vertices.cbegin(); it != this->vertices.cend(); ++it)
+ {
+ float deltaX = it->second.pose->position->x - scenePoint.x();
+ float deltaY = it->second.pose->position->y - scenePoint.y();
+ float dist = std::sqrt(deltaX * deltaX + deltaY * deltaY);
+
+ if (dist < minDist)
+ {
+ minDist = dist;
+ bestIt = it;
+ }
+ }
+
+ if (bestIt != this->vertices.cend())
+ {
+ this->highlightVertex(bestIt->first);
+ }
+ }
+ }
+ else if (event->type() == QEvent::Resize)
+ {
+ this->adjustView();
+ }
+ else
+ {
+ return false;
+ }
+ };
+ _view->installEventFilter(new simox::gui::FunctionalEventFilter(eventFilter));
+#endif
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
new file mode 100644
index 00000000..c81e5509
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -0,0 +1,53 @@
+/*
+ * 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/>.
+ *
+ * @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 "utils.h"
+
+#include <QTableWidget>
+
+#include <set>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ QList<QTableWidgetItem*>
+ utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
+ {
+ std::set<QTableWidgetItem*> set;
+ for (QTableWidgetItem* selected : widget->selectedItems())
+ {
+ if (widget->column(selected) != column)
+ {
+ selected = widget->item(widget->row(selected), 0);
+ }
+ set.insert(selected);
+ }
+
+ QList<QTableWidgetItem*> list;
+ for (auto i : set)
+ {
+ list.append(i);
+ }
+ return list;
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
new file mode 100644
index 00000000..8c344f01
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -0,0 +1,35 @@
+/*
+ * 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/>.
+ *
+ * @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 <QList>
+
+class QTableWidget;
+class QTableWidgetItem;
+
+
+namespace armarx::nav::locgrapheditor::utils
+{
+
+ QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
+
+}
--
GitLab
From 7a3a5ca026c84b5191789c84436c8742e139fd89 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 23 Aug 2021 18:51:37 +0200
Subject: [PATCH 17/33] Add "add edges" context menu
---
.../LocationGraphEditorWidget.ui | 51 ++--
.../LocationGraphEditorWidgetController.cpp | 238 ++----------------
.../LocationGraphEditorWidgetController.h | 5 +-
.../gui-plugins/LocationGraphEditor/Visu.cpp | 1 +
.../widgets/EdgeTableWidget.cpp | 10 +-
.../widgets/VertexTableWidget.cpp | 133 +++++++++-
.../widgets/VertexTableWidget.h | 7 +
7 files changed, 188 insertions(+), 257 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index a25894c7..63dbb942 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -58,31 +58,6 @@
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
- <widget class="QFrame" name="graphFrame">
- <property name="frameShape">
- <enum>QFrame::NoFrame</enum>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout_4">
- <property name="spacing">
- <number>0</number>
- </property>
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item>
- <layout class="QVBoxLayout" name="graphSceneLayout"/>
- </item>
- </layout>
- </widget>
<widget class="QWidget" name="tables" native="true">
<layout class="QVBoxLayout" name="verticalLayout">
<property name="leftMargin">
@@ -186,6 +161,32 @@
</item>
</layout>
</widget>
+
+ <widget class="QFrame" name="graphFrame">
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_4">
+ <property name="spacing">
+ <number>0</number>
+ </property>
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <layout class="QVBoxLayout" name="graphSceneLayout"/>
+ </item>
+ </layout>
+ </widget>
</widget>
</item>
<item>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 59de014f..649b6631 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -147,20 +147,8 @@ namespace armarx::nav::locgrapheditor
connect(view.edgeTable, &EdgeTableWidget::itemSelectionChanged,
this, &This::updateEdgeHighlighting);
-
-#if 0
- // Tables
- connect(view.vertexTable, &QTableWidget::cellDoubleClicked, this, &This::vertexTableDoubleClicked);
- connect(view.vertexTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetVerticesCustomContextMenu);
- connect(view.edgeTable, &QTableWidget::cellDoubleClicked, this, &This::edgeTableDoubleClicked);
- connect(view.edgeTable, &QTableWidget::customContextMenuRequested, this, &This::tableWidgetEdgesCustomContextMenu);
-
- connect(widget.btnAdd, &QPushButton::pressed, this, &This::addNewGraphVertex);
- connect(widget.btnAddEdge, &QPushButton::pressed, this, &This::addNewEdgeBoth);
- connect(widget.btnAddEdgeStartEnd, &QPushButton::pressed, this, &This::addNewEdgeStartEnd);
- connect(widget.btnAddEdgeEndStart, &QPushButton::pressed, this, &This::addNewEdgeEndStart);
- connect(widget.btnEdit, &QPushButton::pressed, this, &This::editGraphVertex);
-#endif
+ connect(view.vertexTable, &VertexTableWidget::newEdgesRequested,
+ this, &This::addEdges);
}
@@ -478,6 +466,19 @@ namespace armarx::nav::locgrapheditor
}
+ void LocationGraphEditorWidgetController::addEdges(
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
+ {
+ std::stringstream ss;
+ ss << "Adding edges ...";
+ for (const auto& [source, target] : vertexItems)
+ {
+ ss << "\n- " << source->text() << " to " << target->text();
+ }
+ ARMARX_IMPORTANT << ss.str();
+ }
+
+
void LocationGraphEditorWidgetController::resetHighlighting()
{
for (auto vertex : model.graph.vertices())
@@ -548,213 +549,4 @@ namespace armarx::nav::locgrapheditor
view.vertexData->setVertex(vertex);
}
-
-#if 0
- bool LocationGraphEditorWidgetController::addNewEdge(const std::string& fromId, const std::string& toId)
- {
- std::string errorMsg;
-
- if (!graphSeg->hasEntityById(fromId))
- {
- errorMsg = "start vertex with Id '" + fromId + "' not found in segment";
- }
- else if (!graphSeg->hasEntityById(toId))
- {
- errorMsg = "end vertex with Id '" + toId + "' not found in segment";
-
- }
- else if (fromId == toId)
- {
- errorMsg = "starting and ending vertex are the same";
- }
- else
- {
- auto fromVertex = graphSeg->getVertexById(fromId);
- for (const auto& adjacent : fromVertex->getAdjacentVertices())
- {
- if (toId == adjacent->getId())
- {
- errorMsg = "edge '" + fromVertex->getName() + "' -> '" + adjacent->getName() + "' already exists";
- break;
- }
- }
- }
-
- if (errorMsg.empty())
- {
- widget.labelAddEdgeStatus->setText(QString::fromStdString("Ok"));
- graphSeg->addEdge(fromId, toId);
- graphVertexPoseResolver->forceRefetch(fromId);
- graphVertexPoseResolver->forceRefetch(toId);
- addEdge(fromId, toId);
- updateVertex(fromId);
- updateVertex(toId);
- widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : lime; }");
- }
- else
- {
- ARMARX_WARNING << errorMsg;
- widget.labelAddEdgeStatus->setText(QString::fromStdString(errorMsg));
- widget.labelAddEdgeStatus->setStyleSheet("QLabel { background-color : orange; }");
- }
-
- return errorMsg.empty();
- }
-
-
- void LocationGraphEditorWidgetController::addNewEdgeBoth()
- {
- std::string startId = widget.editStartVertexId->text().toStdString();
- std::string endId = widget.editEndVertexId->text().toStdString();
- addNewEdge(startId, endId);
- addNewEdge(endId, startId);
- }
-
-
- void LocationGraphEditorWidgetController::addNewEdgeStartEnd()
- {
- std::string startId = widget.editStartVertexId->text().toStdString();
- std::string endId = widget.editEndVertexId->text().toStdString();
- addNewEdge(startId, endId);
- }
-
-
- void LocationGraphEditorWidgetController::addNewEdgeEndStart()
- {
- std::string startId = widget.editEndVertexId->text().toStdString();
- std::string endId = widget.editStartVertexId->text().toStdString();
- addNewEdge(startId, endId);
- }
-
-
-
- void LocationGraphEditorWidgetController::addKitchenGraph()
- {
- std::string scene {"GraphKitchen"};
- graphSeg->clearScene(scene);
-
- // if you insist on hardcoding scenes, use these convenience functions for improved readability:
- auto addVertex = [&](const std::string & name, float x, float y, float angle)
- {
- graphSeg->addVertex(new ::memoryx::GraphVertex {x, y, angle, name, scene});
- ARMARX_INFO_S << "added vertex '" << name << "' at (" << x << ", " << y << ", " << angle << "rad)";
- };
-
- auto addEdges = [&](const std::string & vertexFromName, const std::string & vertexToName, bool bidirectional)
- {
- auto vertexFrom = graphSeg->getVertexFromSceneByName(scene, vertexFromName);
- auto vertexTo = graphSeg->getVertexFromSceneByName(scene, vertexToName);
- ARMARX_CHECK(vertexFrom);
- ARMARX_CHECK(vertexTo);
- ARMARX_INFO_S << "'" << vertexFrom->getName() << "' -> '" << vertexTo->getName() << "', status: " << graphSeg->addEdge(vertexFrom->getId(), vertexTo->getId());
- if (bidirectional)
- {
- ARMARX_INFO_S << "'" << vertexTo->getName() << "' -> '" << vertexFrom->getName() << "', status: " << graphSeg->addEdge(vertexTo->getId(), vertexFrom->getId());
- }
- };
-
- // ex
- addVertex("initialvertex", 2900, 7000, 0);
- addVertex("sideboard", 3400, 7000, 0);
- addEdges("initialvertex", "sideboard", true);
- }
-
-
- void LocationGraphEditorWidgetController::addNewGraphVertex()
- {
- Eigen::Matrix4f mat;
- Eigen::Vector3f rpy;
- Eigen::Vector3f pos;
- rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
- VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
- VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
- pos << widget.spinBoxX->value(), widget.spinBoxY->value(), widget.spinBoxZ->value();
- VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
- armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
- widget.editFrameName->text().toStdString(),
- widget.editAgentName->text().toStdString());
- memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
- widget.editVertexName->text().toStdString(),
- widget.editSceneName->text().toStdString());
- auto entityId = graphSeg->addVertex(vertex);
- graphVertexPoseResolver->forceRefetch(entityId);
- vertex->setId(entityId);
- if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
- {
- widget.editVertexId->setText(QString::fromStdString(entityId));
- addVertex(vertex);
- }
- }
-
-
- void LocationGraphEditorWidgetController::editGraphVertex()
- {
- Eigen::Matrix4f mat;
- Eigen::Vector3f rpy;
- Eigen::Vector3f pos;
- rpy << VirtualRobot::MathTools::deg2rad(widget.spinBoxRoll->value()),
- VirtualRobot::MathTools::deg2rad(widget.spinBoxPitch->value()),
- VirtualRobot::MathTools::deg2rad(widget.spinBoxYaw->value());
- pos << widget.spinBoxX->value(),
- widget.spinBoxY->value(),
- widget.spinBoxZ->value();
- VirtualRobot::MathTools::posrpy2eigen4f(pos, rpy, mat);
- armarx::FramedPosePtr pose = new armarx::FramedPose(mat,
- widget.editFrameName->text().toStdString(),
- widget.editAgentName->text().toStdString());
- memoryx::GraphVertexPtr vertex = new memoryx::GraphVertex(pose,
- widget.editVertexName->text().toStdString(),
- widget.editSceneName->text().toStdString());
- auto id = widget.editVertexId->text().toStdString();
-
- vertex->setId(id);
- graphSeg->updateEntity(id, vertex);
- graphVertexPoseResolver->forceRefetch(id);
- if (widget.graphsComboBox->currentText().toStdString() == widget.editSceneName->text().toStdString())
- {
- addVertex(vertex);
- }
- }
-
-
- void LocationGraphEditorWidgetController::tableWidgetVerticesCustomContextMenu(QPoint pos)
- {
- int row = view.vertexTable->rowAt(pos.y());
- auto vertexIt = std::find_if(vertices.begin(), vertices.end(), [&](const std::pair<std::string, VertexData>& d)
- {
- return d.attrib().tableWidgetVerticesIndex == row;
- });
- QMenu menu;
- QAction* deleteAction = menu.addAction("Delete Vertex");
-
- if (menu.exec(QCursor::pos()) == deleteAction)
- {
- ARMARX_CHECK(vertexIt != vertices.end());
- graphSeg->removeVertex(vertexIt->second.vertex->getId());
- loadGraph();
- }
- }
-
-
- void LocationGraphEditorWidgetController::tableWidgetEdgesCustomContextMenu(QPoint pos)
- {
- int row = view.edgeTable->rowAt(pos.y());
- auto edgeIt = std::find_if(edges.begin(), edges.end(), [&](const std::pair<EdgeId, EdgeData>& d)
- {
- return d.attrib().tableWidgetEdgesIndex == row;
- });
- QMenu menu;
- QAction* deleteAction = menu.addAction("Delete Edge");
-
- if (menu.exec(QCursor::pos()) == deleteAction)
- {
- ARMARX_CHECK(edgeIt != edges.end());
- graphSeg->removeEdge(edgeIt->first.first, edgeIt->first.attrib());
- loadGraph();
- }
- }
-
-
-#endif
-
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index fe161065..0fb66749 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -133,7 +133,8 @@ namespace armarx::nav::locgrapheditor
private slots:
- // Data
+ // Model
+
void queryGraphs();
void updateGraphList();
void loadGraph();
@@ -145,6 +146,8 @@ namespace armarx::nav::locgrapheditor
void clearEdges();
void clearGraph();
+ void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
+
// View & Tables
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
index d978ffb6..dfe8587a 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -86,6 +86,7 @@ namespace armarx::nav::locgrapheditor
if (edge.highlighted)
{
arrow.color(colorHighlighted);
+ arrow.width(width * 1.5f);
}
return arrow;
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 6c60fac3..45d852df 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -20,11 +20,8 @@
*/
#include "EdgeTableWidget.h"
-
#include "utils.h"
-#include <ArmarXCore/core/logging/Logging.h>
-
#include <QHeaderView>
@@ -43,10 +40,11 @@ namespace armarx::nav::locgrapheditor
setEditTriggers(QAbstractItemView::NoEditTriggers);
setSortingEnabled(true);
+ setAlternatingRowColors(true);
+
QString styleSheet = this->styleSheet();
- ARMARX_IMPORTANT << VAROUT(styleSheet.toStdString());
- styleSheet = styleSheet + "\n" + "selection-background-color: orange;";
- this->setStyleSheet(styleSheet);
+ styleSheet = styleSheet + "\n" + "selection-background-color: #FF8000;";
+ setStyleSheet(styleSheet);
setContextMenuPolicy(Qt::CustomContextMenu);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index c06f36de..692e18b8 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -22,7 +22,9 @@
#include "VertexTableWidget.h"
#include "utils.h"
+#include <QAction>
#include <QHeaderView>
+#include <QMenu>
namespace armarx::nav::locgrapheditor
@@ -39,9 +41,15 @@ namespace armarx::nav::locgrapheditor
setEditTriggers(QAbstractItemView::NoEditTriggers);
setSortingEnabled(true);
- setContextMenuPolicy(Qt::CustomContextMenu);
-
setAlternatingRowColors(true);
+
+ QString styleSheet = this->styleSheet();
+ styleSheet = styleSheet + "\n" + "selection-background-color: #FF8000;";
+ setStyleSheet(styleSheet);
+
+ setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(this, &This::customContextMenuRequested,
+ this, &This::makeContextMenu);
}
@@ -101,4 +109,125 @@ namespace armarx::nav::locgrapheditor
return utils::getSelectedItemsOfColumn(this, 0);
}
+
+ void VertexTableWidget::makeContextMenu(QPoint pos)
+ {
+ QList<QTableWidgetItem*> items = selectedVertexItems();
+
+ QMenu menu;
+ if (items.size() == 0)
+ {
+ QAction* action = menu.addAction("No locations selected");
+ action->setEnabled(false);
+ }
+
+ // <- https://unicode-table.com/de/2190/
+ const QString arrowLeft = QStringLiteral("â†");
+ // -> https://unicode-table.com/de/2192/
+ const QString arrowRight = QStringLiteral("\u2192");
+ // <-> https://unicode-table.com/de/21C4/
+ const QString arrowBoth = QStringLiteral("⇄");
+
+ // Partners selected
+ if (items.size() == 2)
+ {
+ menu.addSection("Selected pair of locations");
+
+ // Generate actions for connecting these two nodes.
+ std::sort(items.begin(), items.end(), [](QTableWidgetItem* first, QTableWidgetItem* second)
+ {
+ return first->text() < second->text();
+ });
+ QTableWidgetItem* first = items[0];
+ QTableWidgetItem* second = items[1];
+
+ connect(menu.addAction("Add edge '" + first->text() + "' " + arrowRight + " '" + second->text() + "'"),
+ &QAction::triggered, [this, first, second]()
+ {
+ emit newEdgesRequested({{first, second}});
+ });
+ connect(menu.addAction("Add edge '" + first->text() + "' " + arrowLeft + " '" + second->text() + "'"),
+ &QAction::triggered, [this, first, second]()
+ {
+ emit newEdgesRequested({{second, first}});
+ });
+ connect(menu.addAction("Add edges '" + first->text() + "' " + arrowBoth + " '" + second->text() + "'"),
+ &QAction::triggered, [this, first, second]()
+ {
+ emit newEdgesRequested({{first, second}, {second, first}});
+ });
+ }
+
+ // Partners via context menu
+ if (items.size() > 0)
+ {
+ QString edges = items.size() == 1
+ ? "edge"
+ : "edges";
+ QString desc = items.size() == 1
+ ? "'" + items[0]->text() + "'"
+ : QString::number(items.size()) + " locations";
+
+ if (items.size() == 1)
+ {
+ // QAction* deleteAction = menu.addAction("Delete location '" + );
+ menu.addSection("Selected single location " + desc);
+ }
+ else
+ {
+ menu.addSection("Selected bulk of " + desc);
+ }
+
+ using Item = QTableWidgetItem;
+ using ListOfEdges = QList<QPair<Item*, Item*>>;
+ using AppendFunc = std::function<void(ListOfEdges& edges, Item* selected, Item* action)>;
+
+ auto addBulkActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
+ {
+ if (items.size() == rowCount())
+ {
+ QAction* a = submenu->addAction("No other locations");
+ a->setDisabled(true);
+ }
+ for (int row = 0; row < rowCount(); ++row)
+ {
+ QTableWidgetItem* action = this->item(row, 0);
+ if (items.count(action) == 0) // Do no generate self-edges
+ {
+ QAction* a = submenu->addAction(action->text());
+ connect(a, &QAction::triggered,
+ [this, &items, action, appendFunc]()
+ {
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges;
+ for (auto* selected : items)
+ {
+ appendFunc(edges, selected, action);
+ }
+ emit newEdgesRequested(edges);
+ });
+ }
+ }
+ };
+
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowRight + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action)
+ {
+ edges.append({selected, action});
+ });
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowLeft + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action)
+ {
+ edges.append({action, selected});
+ });
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowBoth + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action)
+ {
+ edges.append({selected, action});
+ edges.append({action, selected});
+ });
+ }
+
+ menu.exec(mapToGlobal(pos));
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 7fea8530..6d9a227f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -50,8 +50,15 @@ namespace armarx::nav::locgrapheditor
QList<QTableWidgetItem*> selectedVertexItems();
+ signals:
+
+ void newEdgesRequested(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges);
+
+
public slots:
+ void makeContextMenu(QPoint pos);
+
private slots:
--
GitLab
From c18706c873fc7af3a41070c17500832466762b9c Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 24 Aug 2021 08:12:19 +0200
Subject: [PATCH 18/33] Implement adding edges
---
.../LocationGraphEditorWidgetController.cpp | 69 +++++++++++++------
.../LocationGraphEditorWidgetController.h | 2 +
.../widgets/EdgeTableWidget.cpp | 9 ++-
.../widgets/EdgeTableWidget.h | 11 ++-
.../widgets/graph_scene/Scene.cpp | 27 +++-----
.../widgets/graph_scene/Scene.h | 23 ++++++-
6 files changed, 97 insertions(+), 44 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 649b6631..50e45488 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -341,21 +341,59 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Edge
LocationGraphEditorWidgetController::addEdge(graph::Graph::ConstEdge edge)
{
- ARMARX_CHECK(not model.graph.hasEdge(edge.sourceObjectID(), edge.targetObjectID()))
- << "Edge must not exist before being added: '"
- << edge.source().attrib().getName() << "' -> '"
- << edge.target().attrib().getName() << "'";
-
auto source = model.graph.vertex(edge.sourceObjectID());
auto target = model.graph.vertex(edge.targetObjectID());
- EdgeData attrib { edge.attrib() };
- attrib.tableWidgetItem = view.edgeTable->addEdge(edge);
- attrib.graphicsItem = view.graph->scene()->addEdge(edge);
- return model.graph.addEdge(source, target, attrib);
+ return addEdge(source, target, { edge.attrib() });
+ }
+
+
+ GuiGraph::Edge
+ LocationGraphEditorWidgetController::addEdge(
+ GuiGraph::ConstVertex source,
+ GuiGraph::ConstVertex target,
+ const EdgeData& defaultAttribs)
+ {
+ ARMARX_CHECK(not model.graph.hasEdge(source.objectID(), target.objectID()))
+ << "Edge must not exist before being added: '"
+ << source.attrib().getName() << "' -> '"
+ << target.attrib().getName() << "'";
+
+ GuiGraph::Edge edge = model.graph.addEdge(source, target, defaultAttribs);
+ edge.attrib().tableWidgetItem = view.edgeTable->addEdge(edge);
+ edge.attrib().graphicsItem = view.graph->scene()->addEdge(edge);
+ return edge;
}
+ void LocationGraphEditorWidgetController::addEdges(
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
+ {
+ if (/* DISABLES CODE */ (false))
+ {
+ std::stringstream ss;
+ ss << "Adding edges ...";
+ for (const auto& [source, target] : vertexItems)
+ {
+ ss << "\n- " << source->text() << " to " << target->text();
+ }
+ ARMARX_IMPORTANT << ss.str();
+ }
+
+ const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
+ for (const auto& [sourceItem, targetItem] : vertexItems)
+ {
+ GuiGraph::Vertex source = itemToVertexMap.at(sourceItem);
+ GuiGraph::Vertex target = itemToVertexMap.at(targetItem);
+ if (not model.graph.hasEdge(source, target))
+ {
+ addEdge(source, target, {});
+ }
+ }
+
+ emit graphChanged();
+ }
+
void LocationGraphEditorWidgetController::updateGraphView()
{
@@ -466,19 +504,6 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::addEdges(
- QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
- {
- std::stringstream ss;
- ss << "Adding edges ...";
- for (const auto& [source, target] : vertexItems)
- {
- ss << "\n- " << source->text() << " to " << target->text();
- }
- ARMARX_IMPORTANT << ss.str();
- }
-
-
void LocationGraphEditorWidgetController::resetHighlighting()
{
for (auto vertex : model.graph.vertices())
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 0fb66749..3a7d9dd3 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -142,6 +142,8 @@ namespace armarx::nav::locgrapheditor
void setGraph(graph::Graph& nav);
GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
GuiGraph::Edge addEdge(graph::Graph::ConstEdge edge);
+ GuiGraph::Edge addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target,
+ const EdgeData& defaultAttribs);
void clearEdges();
void clearGraph();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 45d852df..9e7a16ff 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -51,12 +51,15 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem*
- EdgeTableWidget::addEdge(graph::Graph::ConstEdge edge)
+ EdgeTableWidget::addEdge(
+ const graph::VertexAttribs& sourceAttrib,
+ const graph::VertexAttribs& targetAttrib)
{
int row = rowCount();
setRowCount(row + 1);
- setItem(row, 0, new QTableWidgetItem {QString::fromStdString(edge.source().attrib().getName())});
- setItem(row, 1, new QTableWidgetItem {QString::fromStdString(edge.target().attrib().getName())});
+
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(sourceAttrib.getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::fromStdString(targetAttrib.getName())});
return item(row, 0);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index bf6a496f..296e6a6f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -42,7 +42,16 @@ namespace armarx::nav::locgrapheditor
EdgeTableWidget();
- QTableWidgetItem* addEdge(graph::Graph::ConstEdge edge);
+ template <class EdgeT>
+ QTableWidgetItem* addEdge(const EdgeT& edge)
+ {
+ return addEdge(edge.source().attrib(), edge.target().attrib());
+ }
+
+ QTableWidgetItem*
+ addEdge(const graph::VertexAttribs& sourceAttrib,
+ const graph::VertexAttribs& targetAttrib);
+
void updateEdge(GuiGraph::Edge edge);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
index 32c33382..cfa598c2 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -11,38 +11,34 @@ namespace armarx::nav::locgrapheditor::graph_scene
{
QGraphicsEllipseItem*
- Scene::addVertex(graph::Graph::ConstVertex vertex)
+ Scene::addVertex(semrel::ShapeID vertexID, const graph::VertexAttribs& attrib)
{
- const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
-
- // To capture by copy
- semrel::ShapeID vertexID = vertex.objectID();
+ const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
{
[this, vertexID]() { emit vertexSelected(vertexID); },
- pose(0, 3), - pose(1, 3),
- 1, 1
+ pose(0, 3),
+ - pose(1, 3),
+ 2, 2
};
addItem(item);
// setToolTip on graphicsItem does not work
item->setZValue(std::numeric_limits<qreal>::max());
// dynamic_cast<QGraphicsItem*>(item)->setToolTip(QString::fromStdString("Vertex '" + name + "'"));
- item->setToolTip(QString::fromStdString("Vertex '" + vertex.attrib().getName() + "'"));
+ item->setToolTip(QString::fromStdString("Vertex '" + attrib.getName() + "'"));
return item;
}
QGraphicsLineItem*
- Scene::addEdge(graph::Graph::ConstEdge edge)
+ Scene::addEdge(semrel::ShapeID sourceID, const graph::VertexAttribs& sourceAttrib,
+ semrel::ShapeID targetID, const graph::VertexAttribs& targetAttrib)
{
- semrel::ShapeID sourceID = edge.sourceObjectID();
- semrel::ShapeID targetID = edge.targetObjectID();
-
- Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
- Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
+ Eigen::Matrix4d sourcePose = sourceAttrib.getPose().cast<qreal>();
+ Eigen::Matrix4d targetPose = targetAttrib.getPose().cast<qreal>();
GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
{
@@ -54,8 +50,7 @@ namespace armarx::nav::locgrapheditor::graph_scene
// setToolTip on item does not work
std::stringstream toolTip;
- toolTip << "Edge '" << edge.source().attrib().getName()
- << "' -> '" << edge.target().attrib().getName();
+ toolTip << "Edge '" << sourceAttrib.getName() << "' -> '" << targetAttrib.getName();
item->setToolTip(QString::fromStdString(toolTip.str()));
return item;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
index 95965d34..6afc6a7f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -24,8 +24,27 @@ namespace armarx::nav::locgrapheditor::graph_scene
using QGraphicsScene::QGraphicsScene;
- QGraphicsEllipseItem* addVertex(graph::Graph::ConstVertex vertex);
- QGraphicsLineItem* addEdge(graph::Graph::ConstEdge Edge);
+ template <class VertexT>
+ QGraphicsEllipseItem*
+ addVertex(const VertexT& vertex)
+ {
+ return addVertex(vertex.objectID(), vertex.attrib());
+ }
+ QGraphicsEllipseItem*
+ addVertex(semrel::ShapeID vertexID, const graph::VertexAttribs& attribs);
+
+
+ template <class EdgeT>
+ QGraphicsLineItem*
+ addEdge(const EdgeT& edge)
+ {
+ return addEdge(edge.sourceObjectID(), edge.source().attrib(),
+ edge.targetObjectID(), edge.target().attrib());
+ }
+ QGraphicsLineItem*
+ addEdge(semrel::ShapeID sourceID, const graph::VertexAttribs& sourceAttrib,
+ semrel::ShapeID targetID, const graph::VertexAttribs& targetAttrib);
+
void updateVertex(GuiGraph::Vertex& vertex);
void updateEdge(GuiGraph::Edge& edge);
--
GitLab
From d0e0efc0b963122cdf46c328047503f846c276ab Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 24 Aug 2021 13:44:09 +0200
Subject: [PATCH 19/33] Fix re-loading
---
.../LocationGraphEditorWidget.ui | 15 ++-
.../LocationGraphEditorWidgetController.cpp | 110 ++++++++++++++----
.../LocationGraphEditorWidgetController.h | 8 +-
.../widgets/EdgeTableWidget.cpp | 7 ++
.../widgets/EdgeTableWidget.h | 2 +-
.../widgets/VertexDataWidget.cpp | 11 ++
.../widgets/VertexDataWidget.h | 1 +
.../widgets/VertexTableWidget.cpp | 12 ++
.../widgets/VertexTableWidget.h | 2 +
.../widgets/graph_scene/Scene.cpp | 3 +-
10 files changed, 144 insertions(+), 27 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 63dbb942..161687fa 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -50,6 +50,16 @@
</property>
</widget>
</item>
+ <item>
+ <widget class="QPushButton" name="commitGraphButton">
+ <property name="toolTip">
+ <string>Draws the selected scene</string>
+ </property>
+ <property name="text">
+ <string>Commit</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -161,8 +171,7 @@
</item>
</layout>
</widget>
-
- <widget class="QFrame" name="graphFrame">
+ <widget class="QFrame" name="graphFrame">
<property name="frameShape">
<enum>QFrame::NoFrame</enum>
</property>
@@ -186,7 +195,7 @@
<layout class="QVBoxLayout" name="graphSceneLayout"/>
</item>
</layout>
- </widget>
+ </widget>
</widget>
</item>
<item>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 50e45488..77332863 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -127,6 +127,7 @@ namespace armarx::nav::locgrapheditor
connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
+ connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commitGraph);
// Update views
@@ -286,12 +287,20 @@ namespace armarx::nav::locgrapheditor
fromAron(dto, nav);
}
+ model.graphEntityID = entityID;
setGraph(nav);
}
+ void LocationGraphEditorWidgetController::commitGraph()
+ {
+
+ }
+
+
void LocationGraphEditorWidgetController::setGraph(graph::Graph& nav)
{
+#if 0
// Store vertex highlighting.
std::set<std::string> highlightedVertices;
for (auto vertex : model.graph.vertices())
@@ -301,8 +310,12 @@ namespace armarx::nav::locgrapheditor
highlightedVertices.insert(vertex.attrib().getName());
}
}
+#endif
+
+ // Build the gui graph (model).
+ const bool initialBlocked = this->signalsBlocked();
+ blockSignals(true);
- // Build graph.
clearGraph();
for (auto vertex : nav.vertices())
{
@@ -313,6 +326,7 @@ namespace armarx::nav::locgrapheditor
addEdge(edge);
}
+#if 0
// Restore vertex highlighting.
for (auto vertex : model.graph.vertices())
{
@@ -321,7 +335,11 @@ namespace armarx::nav::locgrapheditor
vertex.attrib().highlighted = true;
}
}
+#endif
+
+ blockSignals(initialBlocked);
+ // Trigger a view update.
emit graphChanged();
}
@@ -464,43 +482,89 @@ namespace armarx::nav::locgrapheditor
}
+ void LocationGraphEditorWidgetController::clearGraph()
+ {
+ const bool initialBlocked = this->signalsBlocked();
+ blockSignals(true);
+
+ clearEdges();
+ clearVertices();
+
+ blockSignals(initialBlocked);
+
+ // Clear data structure
+ ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
+ ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
+
+ emit graphChanged();
+ }
+
+
void LocationGraphEditorWidgetController::clearEdges()
{
- // Remove from graphics scene
- for (auto edge : model.graph.edges())
+ // Remove in reverse order to be more array-friendly.
+ std::vector<GuiGraph::Edge> edges { model.graph.edges().begin(), model.graph.edges().end() };
+ for (auto it = edges.rbegin(); it != edges.rend(); ++it)
{
- view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
+ GuiGraph::Edge edge = *it;
+ removeEdge(edge);
}
- // Clear table widget
- view.edgeTable->clearContents();
- view.edgeTable->setRowCount(0);
+ ARMARX_CHECK_EQUAL(view.edgeTable->rowCount(), 0);
+ ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
- // Clear data structure
- while (model.graph.edges().begin() != model.graph.edges().end())
+ emit graphChanged();
+ }
+
+
+ void LocationGraphEditorWidgetController::clearVertices()
+ {
+ ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0)
+ << "The graph may not have any edges when clearing the vertices.";
+
+ // Remove in reverse order to be more array-friendly.
+ std::vector<GuiGraph::Vertex> vertices { model.graph.vertices().begin(), model.graph.vertices().end() };
+ for (auto it = vertices.rbegin(); it != vertices.rend(); ++it)
{
- model.graph.removeEdge(*model.graph.edges().begin());
+ GuiGraph::Vertex vertex = *it;
+ removeVertex(vertex);
}
+ ARMARX_CHECK_EQUAL(view.vertexTable->rowCount(), 0);
+ ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
+
emit graphChanged();
}
- void LocationGraphEditorWidgetController::clearGraph()
+ void LocationGraphEditorWidgetController::removeEdge(GuiGraph::Edge& edge)
{
- // Clear scene
- view.graph->scene()->clear();
+ // Remove view elements
+ view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
+ view.edgeTable->removeEdge(edge);
- // Clear table widgets
- view.edgeTable->clearContents();
- view.edgeTable->setRowCount(0);
- view.vertexTable->clearContents();
- view.vertexTable->setRowCount(0);
+ // Remove from model
+ model.graph.removeEdge(edge);
+ }
- // Clear data structure
- model.graph.clear();
- emit graphChanged();
+ void LocationGraphEditorWidgetController::removeVertex(GuiGraph::Vertex& vertex)
+ {
+ ARMARX_CHECK_EQUAL(vertex.inDegree(), 0)
+ << "A vertex may not have any edges before being removed. " << vertex.attrib().getName();
+ ARMARX_CHECK_EQUAL(vertex.outDegree(), 0)
+ << "A vertex may not have any edges before being removed. " << vertex.attrib().getName();
+
+ // Remove view elements
+ view.graph->scene()->removeVertex(vertex.attrib().graphicsItem);
+ view.vertexTable->removeVertex(vertex);
+ if (view.vertexData->vertex().has_value() and view.vertexData->vertex().value() == vertex)
+ {
+ view.vertexData->clearVertex();
+ }
+
+ // Remove from model
+ model.graph.removeVertex(vertex);
}
@@ -562,6 +626,10 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
{
+ if (vertexItem == nullptr)
+ {
+ view.vertexData->clearVertex();
+ }
if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
{
selectVertex(vertex.value());
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 3a7d9dd3..825ea418 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -138,6 +138,7 @@ namespace armarx::nav::locgrapheditor
void queryGraphs();
void updateGraphList();
void loadGraph();
+ void commitGraph();
void setGraph(graph::Graph& nav);
GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
@@ -145,8 +146,12 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Edge addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target,
const EdgeData& defaultAttribs);
- void clearEdges();
void clearGraph();
+ void clearEdges();
+ void clearVertices();
+
+ void removeEdge(GuiGraph::Edge& edge);
+ void removeVertex(GuiGraph::Vertex& vertex);
void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
@@ -197,6 +202,7 @@ namespace armarx::nav::locgrapheditor
struct Model
{
armem::wm::Memory memory;
+ armem::MemoryID graphEntityID;
GuiGraph graph;
};
Model model;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 9e7a16ff..081091c5 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -84,6 +84,13 @@ namespace armarx::nav::locgrapheditor
}
+ void EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
+ {
+ this->removeRow(row(edge.attrib().tableWidgetItem));
+ edge.attrib().tableWidgetItem = nullptr;
+ }
+
+
QList<QTableWidgetItem*>
EdgeTableWidget::selectedEdgeItems()
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index 296e6a6f..0cd2cbee 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -55,7 +55,7 @@ namespace armarx::nav::locgrapheditor
void updateEdge(GuiGraph::Edge edge);
- void removeEdge(GuiGraph::Edge edge);
+ void removeEdge(GuiGraph::Edge& edge);
void clearEdges();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index d34dab04..ba63ea0f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -125,9 +125,20 @@ namespace armarx::nav::locgrapheditor
void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
{
+ blockSignals(true);
+
_vertex = vertex;
_setFromVertex(vertex);
setEnabled(true);
+
+ blockSignals(false);
+ }
+
+
+ void VertexDataWidget::clearVertex()
+ {
+ _vertex = std::nullopt;
+ setEnabled(false);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index 64b7971e..cb14f0c5 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -49,6 +49,7 @@ namespace armarx::nav::locgrapheditor
std::optional<GuiGraph::Vertex> vertex();
void setVertex(GuiGraph::Vertex vertex);
+ void clearVertex();
Eigen::Vector3d xyz() const;
Eigen::Vector3d rpyDeg() const;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 692e18b8..6722f828 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -103,6 +103,18 @@ namespace armarx::nav::locgrapheditor
}
+ void
+ VertexTableWidget::removeVertex(GuiGraph::Vertex& vertex)
+ {
+ if (currentItem() == vertex.attrib().tableWidgetItem)
+ {
+ setCurrentItem(nullptr);
+ }
+ this->removeRow(row(vertex.attrib().tableWidgetItem));
+ vertex.attrib().tableWidgetItem = nullptr;
+ }
+
+
QList<QTableWidgetItem*>
VertexTableWidget::selectedVertexItems()
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 6d9a227f..379c224d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -46,6 +46,8 @@ namespace armarx::nav::locgrapheditor
void updateVertex(GuiGraph::Vertex vertex);
+ void removeVertex(GuiGraph::Vertex& vertex);
+
QList<QTableWidgetItem*> selectedVertexItems();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
index cfa598c2..330a15dc 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -79,7 +79,8 @@ namespace armarx::nav::locgrapheditor::graph_scene
void Scene::updateEdge(GuiGraph::Edge& edge)
{
QGraphicsLineItem* item = edge.attrib().graphicsItem;
- ARMARX_CHECK_NOT_NULL(item);
+ ARMARX_CHECK_NOT_NULL(item)
+ << edge.source().attrib().getName() << " -> " << edge.target().attrib().getName();
Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
--
GitLab
From e68b583ed84a914025067b8243df569ed6803062 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 24 Aug 2021 15:03:46 +0200
Subject: [PATCH 20/33] Implement edge removal, fix commit
---
.../LocationGraphEditorWidgetController.cpp | 150 +++++++++++++-----
.../LocationGraphEditorWidgetController.h | 1 +
.../widgets/EdgeTableWidget.cpp | 38 +++++
.../widgets/EdgeTableWidget.h | 10 +-
.../widgets/VertexDataWidget.cpp | 16 +-
.../widgets/VertexTableWidget.cpp | 19 +--
.../widgets/VertexTableWidget.h | 5 +-
.../widgets/graph_scene/ControlWidget.cpp | 3 +-
.../LocationGraphEditor/widgets/utils.cpp | 5 +
.../LocationGraphEditor/widgets/utils.h | 9 ++
10 files changed, 188 insertions(+), 68 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 77332863..85663ec8 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -53,6 +53,7 @@
#include <QMouseEvent>
#include <QObject>
#include <QPushButton>
+#include <QSignalBlocker>
// std
#include <sstream>
@@ -150,6 +151,8 @@ namespace armarx::nav::locgrapheditor
connect(view.vertexTable, &VertexTableWidget::newEdgesRequested,
this, &This::addEdges);
+ connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested,
+ this, &This::removeEdges);
}
@@ -210,16 +213,7 @@ namespace armarx::nav::locgrapheditor
{
remote.connect(*this);
- if (/* DISABLES CODE */ (true))
- {
- widget.sceneGroupBox->setEnabled(true);
- widget.sceneGroupBox->setTitle("Scenes from graph memory segment");
- }
- else
- {
- widget.sceneGroupBox->setEnabled(false);
- widget.sceneGroupBox->setTitle("Scenes from graph memory segment (No graph memory segment available)");
- }
+ widget.sceneGroupBox->setEnabled(true);
emit connected();
}
@@ -257,23 +251,41 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::updateGraphList()
{
+ QString previousText = widget.graphsComboBox->currentText();
widget.graphsComboBox->clear();
- bool enable = false;
- model.memory.forEachEntity([this, &enable](const armem::wm::Entity& entity)
+ int i = 0;
+ int previousIndex = -1;
+ model.memory.forEachEntity([&](const armem::wm::Entity& entity)
{
- widget.graphsComboBox->addItem(QString::fromStdString(entity.id().str()));
- enable = true;
+ QString text = QString::fromStdString(entity.id().str());
+ widget.graphsComboBox->addItem(text);
+ if (previousIndex < 0 and text == previousText)
+ {
+ previousIndex = i;
+ }
+ ++i;
});
- widget.loadGraphButton->setEnabled(enable);
+ if (previousIndex >= 0)
+ {
+ widget.graphsComboBox->setCurrentIndex(previousIndex);
+ }
+ widget.loadGraphButton->setEnabled(widget.graphsComboBox->count() > 0);
}
void LocationGraphEditorWidgetController::loadGraph()
{
const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
+
+ queryGraphs(); // Refresh local memory.
+
const armem::wm::EntityInstance* instance = model.memory.findLatestInstance(entityID);
- if (not instance)
+ if (instance)
+ {
+ widget.statusLabel->setText(QString::fromStdString("Loaded snapshot " + instance->id().getEntitySnapshotID().str()));
+ }
+ else
{
std::stringstream ss;
ss << "No latest instance of entity " << entityID << " in memory.";
@@ -288,13 +300,33 @@ namespace armarx::nav::locgrapheditor
}
model.graphEntityID = entityID;
+ ARMARX_VERBOSE << "Loading graph " << nav.str();
setGraph(nav);
}
void LocationGraphEditorWidgetController::commitGraph()
{
+ nav::graph::arondto::Graph dto;
+ {
+ nav::graph::Graph nav = fromGuiGraph(model.graph);
+ toAron(dto, nav);
+ }
+ armem::EntityUpdate update;
+ update.entityID = model.graphEntityID;
+ update.timeCreated = armem::Time::now();
+ update.instancesData = {dto.toAron()};
+
+ armem::EntityUpdateResult result = remote.graphWriter.commit(update);
+ if (result.success)
+ {
+ widget.statusLabel->setText(QString::fromStdString("Committed snapshot " + result.snapshotID.str()));
+ }
+ else
+ {
+ widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
+ }
}
@@ -313,31 +345,31 @@ namespace armarx::nav::locgrapheditor
#endif
// Build the gui graph (model).
- const bool initialBlocked = this->signalsBlocked();
- blockSignals(true);
-
- clearGraph();
- for (auto vertex : nav.vertices())
- {
- addVertex(vertex);
- }
- for (auto edge : nav.edges())
{
- addEdge(edge);
- }
+ QSignalBlocker blocker(this);
+
+ clearGraph();
+ for (auto vertex : nav.vertices())
+ {
+ addVertex(vertex);
+ }
+ for (auto edge : nav.edges())
+ {
+ addEdge(edge);
+ }
#if 0
- // Restore vertex highlighting.
- for (auto vertex : model.graph.vertices())
- {
- if (highlightedVertices.count(vertex.attrib().getName()))
+ // Restore vertex highlighting.
+ for (auto vertex : model.graph.vertices())
{
- vertex.attrib().highlighted = true;
+ if (highlightedVertices.count(vertex.attrib().getName()))
+ {
+ vertex.attrib().highlighted = true;
+ }
}
- }
#endif
- blockSignals(initialBlocked);
+ }
// Trigger a view update.
emit graphChanged();
@@ -413,6 +445,35 @@ namespace armarx::nav::locgrapheditor
}
+ void LocationGraphEditorWidgetController::removeEdges(
+ QList<QTableWidgetItem*> edgeItems)
+ {
+ if (/* DISABLES CODE */ (true))
+ {
+ std::stringstream ss;
+ ss << "Remoiving edges ...";
+ for (const auto& edgeItem : edgeItems)
+ {
+ ss << "\n- " << edgeItem->text();
+ }
+ ARMARX_IMPORTANT << ss.str();
+ }
+
+ {
+ QSignalBlocker blocker(this);
+
+ const auto itemToEdgeMap = model.graph.getTableItemToEdgeMap();
+ for (const auto& edgeItem : edgeItems)
+ {
+ GuiGraph::Edge edge = itemToEdgeMap.at(edgeItem);
+ removeEdge(edge);
+ }
+ }
+
+ emit graphChanged();
+ }
+
+
void LocationGraphEditorWidgetController::updateGraphView()
{
for (auto vertex : model.graph.vertices())
@@ -484,13 +545,11 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::clearGraph()
{
- const bool initialBlocked = this->signalsBlocked();
- blockSignals(true);
-
- clearEdges();
- clearVertices();
-
- blockSignals(initialBlocked);
+ {
+ QSignalBlocker blocker(this);
+ clearEdges();
+ clearVertices();
+ }
// Clear data structure
ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
@@ -539,12 +598,21 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::removeEdge(GuiGraph::Edge& edge)
{
+ ARMARX_CHECK(model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
+ << "Cannot remove edge that does not exist.";
+
// Remove view elements
view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
+
+ const int rows = view.edgeTable->rowCount();
view.edgeTable->removeEdge(edge);
+ ARMARX_CHECK_EQUAL(view.edgeTable->rowCount(), rows - 1);
// Remove from model
model.graph.removeEdge(edge);
+
+ ARMARX_CHECK(not model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
+ << edge.sourceDescriptor() << " -> " << edge.targetDescriptor();
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 825ea418..ff1177db 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -154,6 +154,7 @@ namespace armarx::nav::locgrapheditor
void removeVertex(GuiGraph::Vertex& vertex);
void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
+ void removeEdges(QList<QTableWidgetItem*> edgeItems);
// View & Tables
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 081091c5..21aba203 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -22,7 +22,9 @@
#include "EdgeTableWidget.h"
#include "utils.h"
+#include <QAction>
#include <QHeaderView>
+#include <QMenu>
namespace armarx::nav::locgrapheditor
@@ -47,6 +49,8 @@ namespace armarx::nav::locgrapheditor
setStyleSheet(styleSheet);
setContextMenuPolicy(Qt::CustomContextMenu);
+ connect(this, &This::customContextMenuRequested,
+ this, &This::makeContextMenu);
}
@@ -98,4 +102,38 @@ namespace armarx::nav::locgrapheditor
}
+ void EdgeTableWidget::makeContextMenu(QPoint pos)
+ {
+ QList<QTableWidgetItem*> items = selectedEdgeItems();
+
+ QMenu menu;
+ if (items.size() == 0)
+ {
+ QAction* action = menu.addAction("No edges selected");
+ action->setEnabled(false);
+ }
+ else
+ {
+ QString desc;
+ if (items.size() == 1)
+ {
+ desc = "edge '" + items[0]->text() + "' " + utils::arrowRight
+ + " '" + item(row(items[0]), 1)->text() + "'";
+ }
+ else
+ {
+ desc = QString::number(items.size()) + " edges";
+ }
+
+ menu.addSection("Selected " + desc);
+ connect(menu.addAction("Remove " + desc),
+ &QAction::triggered, [this, &items]()
+ {
+ emit edgeRemovalRequested(items);
+ });
+ }
+
+ menu.exec(mapToGlobal(pos));
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index 0cd2cbee..2e745a1e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -25,6 +25,7 @@
#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
#include <QColor>
+#include <QList>
#include <QTableWidget>
@@ -63,7 +64,14 @@ namespace armarx::nav::locgrapheditor
QList<QTableWidgetItem*> selectedEdgeItems();
- private slots:
+ signals:
+
+ void edgeRemovalRequested(QList<QTableWidgetItem*> edgeItems);
+
+
+ public slots:
+
+ void makeContextMenu(QPoint pos);
public:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index ba63ea0f..7119c030 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -30,7 +30,9 @@
#include <QFormLayout>
#include <QHBoxLayout>
#include <QLineEdit>
+#include <QList>
#include <QRadioButton>
+#include <QSignalBlocker>
#include <SimoxUtility/math/convert/deg_to_rad.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
@@ -125,13 +127,11 @@ namespace armarx::nav::locgrapheditor
void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
{
- blockSignals(true);
+ QSignalBlocker blocker(this);
_vertex = vertex;
_setFromVertex(vertex);
setEnabled(true);
-
- blockSignals(false);
}
@@ -229,9 +229,11 @@ namespace armarx::nav::locgrapheditor
return simox::math::rad_to_deg(rad);
};
}
+
+ QList<QSignalBlocker>> blockers;
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
- angle->blockSignals(true);
+ blockers.append(QSignalBlocker(angle));
angle->setSuffix(suffix);
angle->setMinimum(min);
@@ -243,10 +245,8 @@ namespace armarx::nav::locgrapheditor
{
_setRpyRad(simox::math::mat4f_to_rpy(_vertex->attrib().getPose().cast<qreal>()));
}
- for (QDoubleSpinBox* angle : _angleSpinBoxes())
- {
- angle->blockSignals(false);
- }
+
+ // blockers will disable blocking for all spin boxes on destruction.
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 6722f828..2d1e965e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -133,13 +133,6 @@ namespace armarx::nav::locgrapheditor
action->setEnabled(false);
}
- // <- https://unicode-table.com/de/2190/
- const QString arrowLeft = QStringLiteral("â†");
- // -> https://unicode-table.com/de/2192/
- const QString arrowRight = QStringLiteral("\u2192");
- // <-> https://unicode-table.com/de/21C4/
- const QString arrowBoth = QStringLiteral("⇄");
-
// Partners selected
if (items.size() == 2)
{
@@ -153,17 +146,17 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem* first = items[0];
QTableWidgetItem* second = items[1];
- connect(menu.addAction("Add edge '" + first->text() + "' " + arrowRight + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edge '" + first->text() + "' " + utils::arrowRight + " '" + second->text() + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{first, second}});
});
- connect(menu.addAction("Add edge '" + first->text() + "' " + arrowLeft + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edge '" + first->text() + "' " + utils::arrowLeft + " '" + second->text() + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{second, first}});
});
- connect(menu.addAction("Add edges '" + first->text() + "' " + arrowBoth + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edges '" + first->text() + "' " + utils::arrowBoth + " '" + second->text() + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{first, second}, {second, first}});
@@ -221,17 +214,17 @@ namespace armarx::nav::locgrapheditor
}
};
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowRight + " ..."),
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
});
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowLeft + " ..."),
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({action, selected});
});
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + arrowBoth + " ..."),
+ addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 379c224d..de609ce0 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -25,6 +25,8 @@
#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
#include <QColor>
+#include <QList>
+#include <QPair>
#include <QTableWidget>
@@ -62,9 +64,6 @@ namespace armarx::nav::locgrapheditor
void makeContextMenu(QPoint pos);
- private slots:
-
-
public:
QColor bgColorDefault = default_colors::tableBackgroundDefault;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
index 5549b56f..d6dcf9b7 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
@@ -1,6 +1,6 @@
#include "ControlWidget.h"
-#include <ArmarXCore/core/logging/Logging.h>
+// #include <ArmarXCore/core/logging/Logging.h>
#include <QDoubleSpinBox>
#include <QFrame>
@@ -67,7 +67,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
connect(_angle.turnClockwise, &QPushButton::pressed, [this]()
{
qreal newangle = std::fmod(angle() + _angle.rotateStepSize, 360.0);
- ARMARX_IMPORTANT << VAROUT(newangle);
setAngle(newangle);
});
connect(_angle.turnCounterClockwise, &QPushButton::pressed, [this]()
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
index c81e5509..e61b79b9 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -29,6 +29,11 @@
namespace armarx::nav::locgrapheditor
{
+ const QString utils::arrowLeft = QStringLiteral("â†");
+ const QString utils::arrowRight = QStringLiteral("\u2192");
+ const QString utils::arrowBoth = QStringLiteral("⇄");
+
+
QList<QTableWidgetItem*>
utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
index 8c344f01..44eb497a 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -32,4 +32,13 @@ namespace armarx::nav::locgrapheditor::utils
QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
+
+ // <- https://unicode-table.com/de/2190/
+ extern const QString arrowLeft;
+ // -> https://unicode-table.com/de/2192/
+ extern const QString arrowRight;
+ // <-> https://unicode-table.com/de/21C4/
+ extern const QString arrowBoth;
+
+
}
--
GitLab
From c96e887abbf38e04a276e3d18ca648c346b46bba Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 24 Aug 2021 18:00:30 +0200
Subject: [PATCH 21/33] Add basic changed tracking and message box when loading
while having changes
---
source/armarx/navigation/graph/Graph.cpp | 17 +-
source/armarx/navigation/graph/Visu.cpp | 48 ++--
source/armarx/navigation/graph/Visu.h | 8 +-
.../LocationGraphEditor/GuiGraph.cpp | 16 ++
.../LocationGraphEditor/GuiGraph.h | 7 +
.../LocationGraphEditorWidgetController.cpp | 220 ++++++++++++++----
.../LocationGraphEditorWidgetController.h | 10 +-
.../widgets/EdgeTableWidget.cpp | 34 ++-
.../widgets/VertexDataWidget.cpp | 62 ++---
.../widgets/VertexDataWidget.h | 1 +
.../widgets/VertexTableWidget.cpp | 78 +++++--
.../widgets/VertexTableWidget.h | 5 +
.../LocationGraphEditor/widgets/utils.cpp | 7 +-
.../LocationGraphEditor/widgets/utils.h | 6 +-
14 files changed, 384 insertions(+), 135 deletions(-)
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index f8086dce..48c0f4dd 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -22,13 +22,15 @@
#include "Graph.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
namespace armarx::nav::graph
{
std::string VertexAttribs::getName() const
{
- return aron.locationID.entityName;
+ return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
}
Eigen::Matrix4f VertexAttribs::getPose() const
@@ -52,12 +54,18 @@ namespace armarx::nav
dto = {};
for (auto vertex : bo.vertices())
{
- dto.vertices.push_back(vertex.attrib().aron);
+ auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
+ v.vertexID = static_cast<long>(vertex.objectID());
}
+ ARMARX_CHECK_EQUAL(dto.vertices.size(), bo.numVertices());
+
for (auto edge : bo.edges())
{
- dto.edges.push_back(edge.attrib().aron);
+ auto& e = dto.edges.emplace_back(edge.attrib().aron);
+ e.sourceVertexID = static_cast<long>(edge.sourceObjectID());
+ e.targetVertexID = static_cast<long>(edge.targetObjectID());
}
+ ARMARX_CHECK_EQUAL(dto.edges.size(), bo.numEdges());
}
@@ -69,11 +77,14 @@ namespace armarx::nav
auto v = bo.addVertex(semrel::ShapeID(vertex.vertexID));
v.attrib().aron = vertex;
}
+ ARMARX_CHECK_EQUAL(bo.numVertices(), dto.vertices.size());
+
for (const arondto::Edge& edge : dto.edges)
{
auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID), semrel::ShapeID(edge.targetVertexID));
e.attrib().aron = edge;
}
+ ARMARX_CHECK_EQUAL(bo.numEdges(), dto.edges.size());
}
}
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
index 62ede64f..3d98b5d7 100644
--- a/source/armarx/navigation/graph/Visu.cpp
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -13,7 +13,6 @@
* 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
@@ -33,49 +32,58 @@
namespace armarx::nav::graph
{
- viz::Pose VertexVisu::Pose::draw(Graph::ConstVertex vertex) const
+ viz::Pose VertexVisu::Pose::draw(const VertexAttribs& attribs) const
{
- const arondto::Vertex& aron = vertex.attrib().aron;
- return viz::Pose(aron.locationID.entityName)
- .pose(aron.globalRobotPose)
- .scale(scale);
+ return viz::Pose(attribs.getName()).pose(attribs.getPose()).scale(scale);
}
- viz::Arrow VertexVisu::ForwardArrow::draw(Graph::ConstVertex vertex) const
+ viz::Arrow VertexVisu::ForwardArrow::draw(const VertexAttribs& attribs) const
{
- const arondto::Vertex& aron = vertex.attrib().aron;
- return viz::Arrow(aron.locationID.entityName + " forward")
- .fromTo(simox::math::position(aron.globalRobotPose),
- simox::math::transform_position(aron.globalRobotPose, length * Eigen::Vector3f::UnitY()))
+ return viz::Arrow(attribs.getName() + " forward")
+ .fromTo(simox::math::position(attribs.getPose()),
+ simox::math::transform_position(attribs.getPose(), length * Eigen::Vector3f::UnitY()))
.color(color)
.width(width);
}
void VertexVisu::draw(viz::Layer& layer, Graph::ConstVertex vertex) const
+ {
+ draw(layer, vertex.attrib());
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, const VertexAttribs& attribs) const
{
if (pose.has_value())
{
- layer.add(pose->draw(vertex));
+ layer.add(pose->draw(attribs));
}
if (forwardArrow.has_value())
{
- layer.add(forwardArrow->draw(vertex));
+ layer.add(forwardArrow->draw(attribs));
}
}
viz::Arrow EdgeVisu::Arrow::draw(Graph::ConstEdge edge) const
{
- const auto& sourceAron = edge.source().attrib().aron;
- const auto& targetAron = edge.target().attrib().aron;
- return viz::Arrow(sourceAron.locationID.entityName + " -> " +
- targetAron.locationID.entityName)
- .fromTo(simox::math::position(sourceAron.globalRobotPose),
- simox::math::position(targetAron.globalRobotPose))
+ return draw(edge.attrib(), edge.source().attrib(), edge.target().attrib());
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(
+ const EdgeAttribs& edge,
+ const VertexAttribs& source,
+ const VertexAttribs& target) const
+ {
+ (void) edge;
+ return viz::Arrow(source.getName() + " -> " + target.getName())
+ .fromTo(simox::math::position(source.getPose()),
+ simox::math::position(target.getPose()))
.width(width)
- .color(color);
+ .color(color);
}
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
index 6e2516bc..f38719a9 100644
--- a/source/armarx/navigation/graph/Visu.h
+++ b/source/armarx/navigation/graph/Visu.h
@@ -13,7 +13,6 @@
* 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
@@ -35,7 +34,6 @@ namespace armarx::viz
class Layer;
class Pose;
}
-
namespace armarx::nav::graph
{
@@ -45,7 +43,7 @@ namespace armarx::nav::graph
{
float scale = 1.0;
- viz::Pose draw(Graph::ConstVertex vertex) const;
+ viz::Pose draw(const VertexAttribs& attribs) const;
};
std::optional<Pose> pose = Pose {};
@@ -55,12 +53,13 @@ namespace armarx::nav::graph
float length = 100.0;
simox::Color color = simox::Color::cyan(220);
- viz::Arrow draw(Graph::ConstVertex vertex) const;
+ viz::Arrow draw(const VertexAttribs& attribs) const;
};
std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
void draw(viz::Layer& layer, Graph::ConstVertex vertex) const;
+ void draw(viz::Layer& layer, const VertexAttribs& attribs) const;
};
@@ -72,6 +71,7 @@ namespace armarx::nav::graph
simox::Color color = simox::Color::azure(196);
viz::Arrow draw(Graph::ConstEdge edge) const;
+ viz::Arrow draw(const EdgeAttribs& edge, const VertexAttribs& source, const VertexAttribs& target) const;
};
std::optional<Arrow> arrow = Arrow {};
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
index 49361f44..7cd38cc0 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -29,6 +29,22 @@
namespace armarx::nav::locgrapheditor
{
+ bool GuiGraph::hasChanged() const
+ {
+ if (attrib().edgesChanged)
+ {
+ return true;
+ }
+ for (auto vertex : vertices())
+ {
+ if (vertex.attrib().changed)
+ {
+ return true;
+ }
+ }
+ return false;
+ }
+
std::optional<GuiGraph::Vertex>
GuiGraph::getVertexFromTableItem(QTableWidgetItem* item)
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
index 33f4ddf5..561f4f58 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -51,6 +51,9 @@ namespace armarx::nav::locgrapheditor
/// Whether the node is highlighted.
bool highlighted = false;
+
+ /// Whether the vertex was changed since loading or committing.
+ bool changed = false;
};
@@ -73,6 +76,8 @@ namespace armarx::nav::locgrapheditor
struct GraphData : public nav::graph::GraphAttribs
{
+ /// Whether the graph structure was changed since loading or committing.
+ bool edgesChanged = false;
};
@@ -83,6 +88,8 @@ namespace armarx::nav::locgrapheditor
using RelationGraph::RelationGraph;
+ bool hasChanged() const;
+
std::optional<Vertex> getVertexFromTableItem(QTableWidgetItem* item);
std::map<QTableWidgetItem*, Vertex> getTableItemToVertexMap();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 85663ec8..3f4a7113 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -33,11 +33,12 @@
#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
#include <armarx/navigation/location/constants.h>
-#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -50,6 +51,7 @@
#include <QLabel>
#include <QLineEdit>
#include <QMenu>
+#include <QMessageBox>
#include <QMouseEvent>
#include <QObject>
#include <QPushButton>
@@ -128,7 +130,7 @@ namespace armarx::nav::locgrapheditor
connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
- connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commitGraph);
+ connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commit);
// Update views
@@ -212,8 +214,12 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::onConnectComponent()
{
remote.connect(*this);
-
- widget.sceneGroupBox->setEnabled(true);
+ {
+ std::stringstream ss;
+ ss << "Navigation Graphs (Entities from Core Segment " << nav::graph::coreSegmentID << ")";
+ widget.sceneGroupBox->setTitle(QString::fromStdString(ss.str()));
+ widget.sceneGroupBox->setEnabled(true);
+ }
emit connected();
}
@@ -255,11 +261,16 @@ namespace armarx::nav::locgrapheditor
widget.graphsComboBox->clear();
int i = 0;
- int previousIndex = -1;
+ int previousIndex = -1; // To keep selection.
model.memory.forEachEntity([&](const armem::wm::Entity& entity)
{
- QString text = QString::fromStdString(entity.id().str());
- widget.graphsComboBox->addItem(text);
+ bool hasChanged = (entity.id() == model.graphEntityID
+ ? model.graph.hasChanged()
+ : false);
+ QString text = getGraphDisplayName(entity.id(), hasChanged);
+ QString id = QString::fromStdString(entity.id().str());
+
+ widget.graphsComboBox->addItem(text, id);
if (previousIndex < 0 and text == previousText)
{
previousIndex = i;
@@ -276,9 +287,44 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::loadGraph()
{
- const armem::MemoryID entityID = armem::MemoryID::fromString(widget.graphsComboBox->currentText().toStdString());
+ if (model.graph.hasChanged())
+ {
+ QMessageBox msgBox;
+ msgBox.setText("The current graph and/or locations have uncommitted changes. ");
+ msgBox.setInformativeText("Do you want to discard them?");
+ QStringList detailLines;
+ if (model.graph.attrib().edgesChanged)
+ {
+ detailLines.append("Graph edges have changed.");
+ }
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().changed)
+ {
+ detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) + " has changed.");
+ }
+ }
+ msgBox.setDetailedText(detailLines.join("\n"));
+ msgBox.setStandardButtons(QMessageBox::Discard | QMessageBox::Cancel);
+ msgBox.setDefaultButton(QMessageBox::Cancel);
- queryGraphs(); // Refresh local memory.
+ int ret = msgBox.exec();
+ switch (ret)
+ {
+ case QMessageBox::Discard:
+ // Ok go.
+ break;
+ case QMessageBox::Cancel:
+ // Abort loading.
+ return;
+ }
+ }
+
+ const armem::MemoryID entityID = armem::MemoryID::fromString(
+ widget.graphsComboBox->currentData().toString().toStdString());
+
+ // Refresh local memory.
+ queryGraphs();
const armem::wm::EntityInstance* instance = model.memory.findLatestInstance(entityID);
if (instance)
@@ -305,7 +351,99 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::commitGraph()
+ void
+ LocationGraphEditorWidgetController::commit()
+ {
+ armem::CommitResult locResults = commitLocations();
+ armem::EntityUpdateResult graphResult = commitGraph();
+
+ {
+ std::stringstream ss;
+ if (locResults.allSuccess())
+ {
+ ss << "Committed " << locResults.results.size() << " location snapshots";
+ if (graphResult.success)
+ {
+ ss << " and 1 graph snapshot " << graphResult.snapshotID;
+ }
+ else
+ {
+ ss << " but failed to commit graph: \n" << graphResult.errorMessage;
+ }
+ }
+ else
+ {
+ int numLocs = static_cast<int>(locResults.results.size());
+ int numSuccess = 0;
+ for (const auto& r : locResults.results)
+ {
+ numSuccess += int(r.success);
+ }
+ int numFailed = numLocs - numSuccess;
+
+ if (graphResult.success)
+ {
+ ss << "Committed 1 graph snapshot " << graphResult.snapshotID;
+ ss << " and " << numSuccess << " locations, but failed to commit "
+ << numFailed << " locations: \n" << locResults.allErrorMessages();
+ }
+ else
+ {
+ ss << "Failed to commit graph and " << numFailed << " of " << numLocs << " locations: \n";
+ ss << graphResult.errorMessage << "\n";
+ ss << locResults.allErrorMessages();
+ }
+ }
+
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+
+ // `changed` flags may have changed
+ emit graphChanged();
+ }
+
+
+ armem::CommitResult
+ LocationGraphEditorWidgetController::commitLocations()
+ {
+ armem::Commit commit;
+
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().changed)
+ {
+ armem::EntityUpdate& update = commit.add();
+ fromAron(vertex.attrib().aron.locationID, update.entityID);
+ update.timeCreated = armem::Time::now();
+
+ nav::loc::arondto::Location dto;
+ dto.globalRobotPose = vertex.attrib().getPose();
+ update.instancesData = {dto.toAron()};
+ }
+ }
+
+ armem::CommitResult result = remote.locationWriter.commit(commit);
+ auto it = result.results.begin();
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().changed)
+ {
+ ARMARX_CHECK(it != result.results.end());
+ if (it->success)
+ {
+ // Only clear dirty flag when update was successful.
+ vertex.attrib().changed = false;
+ }
+ ++it;
+ }
+ }
+
+ return result;
+ }
+
+
+ armem::EntityUpdateResult
+ LocationGraphEditorWidgetController::commitGraph()
{
nav::graph::arondto::Graph dto;
{
@@ -318,37 +456,19 @@ namespace armarx::nav::locgrapheditor
update.timeCreated = armem::Time::now();
update.instancesData = {dto.toAron()};
- armem::EntityUpdateResult result = remote.graphWriter.commit(update);
- if (result.success)
- {
- widget.statusLabel->setText(QString::fromStdString("Committed snapshot " + result.snapshotID.str()));
- }
- else
- {
- widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
- }
+ return remote.graphWriter.commit(update);
}
void LocationGraphEditorWidgetController::setGraph(graph::Graph& nav)
{
-#if 0
- // Store vertex highlighting.
- std::set<std::string> highlightedVertices;
- for (auto vertex : model.graph.vertices())
- {
- if (vertex.attrib().highlighted)
- {
- highlightedVertices.insert(vertex.attrib().getName());
- }
- }
-#endif
-
// Build the gui graph (model).
{
QSignalBlocker blocker(this);
clearGraph();
+ model.graph.attrib().edgesChanged = false;
+
for (auto vertex : nav.vertices())
{
addVertex(vertex);
@@ -357,18 +477,6 @@ namespace armarx::nav::locgrapheditor
{
addEdge(edge);
}
-
-#if 0
- // Restore vertex highlighting.
- for (auto vertex : model.graph.vertices())
- {
- if (highlightedVertices.count(vertex.attrib().getName()))
- {
- vertex.attrib().highlighted = true;
- }
- }
-#endif
-
}
// Trigger a view update.
@@ -441,6 +549,8 @@ namespace armarx::nav::locgrapheditor
}
}
+ model.graph.attrib().edgesChanged = true;
+
emit graphChanged();
}
@@ -448,7 +558,7 @@ namespace armarx::nav::locgrapheditor
void LocationGraphEditorWidgetController::removeEdges(
QList<QTableWidgetItem*> edgeItems)
{
- if (/* DISABLES CODE */ (true))
+ if (/* DISABLES CODE */ (false))
{
std::stringstream ss;
ss << "Remoiving edges ...";
@@ -458,7 +568,6 @@ namespace armarx::nav::locgrapheditor
}
ARMARX_IMPORTANT << ss.str();
}
-
{
QSignalBlocker blocker(this);
@@ -470,6 +579,8 @@ namespace armarx::nav::locgrapheditor
}
}
+ model.graph.attrib().edgesChanged = true;
+
emit graphChanged();
}
@@ -484,6 +595,13 @@ namespace armarx::nav::locgrapheditor
{
updateEdgeView(edge);
}
+
+ int index = widget.graphsComboBox->findData(QString::fromStdString(model.graphEntityID.str()));
+ if (index >= 0)
+ {
+ widget.graphsComboBox->setItemText(index, getGraphDisplayName(model.graphEntityID, model.graph.hasChanged()));
+ }
+
updateArViz();
}
@@ -710,4 +828,16 @@ namespace armarx::nav::locgrapheditor
view.vertexData->setVertex(vertex);
}
+
+ QString LocationGraphEditorWidgetController::getGraphDisplayName(
+ const armem::MemoryID& entityID, bool changed) const
+ {
+ QString name = QString::fromStdString(entityID.providerSegmentName + "/" + entityID.entityName);
+ if (changed)
+ {
+ name += "*";
+ }
+ return name;
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index ff1177db..e4de7136 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -138,7 +138,6 @@ namespace armarx::nav::locgrapheditor
void queryGraphs();
void updateGraphList();
void loadGraph();
- void commitGraph();
void setGraph(graph::Graph& nav);
GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
@@ -157,6 +156,10 @@ namespace armarx::nav::locgrapheditor
void removeEdges(QList<QTableWidgetItem*> edgeItems);
+ void commit();
+ armem::CommitResult commitLocations();
+ armem::EntityUpdateResult commitGraph();
+
// View & Tables
void updateGraphView();
@@ -175,6 +178,11 @@ namespace armarx::nav::locgrapheditor
void updateEdgeHighlighting();
+ private:
+
+ QString getGraphDisplayName(const armem::MemoryID& entityID, bool changed = false) const;
+
+
private:
/// Widget Form
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 21aba203..adecbca4 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -59,13 +59,21 @@ namespace armarx::nav::locgrapheditor
const graph::VertexAttribs& sourceAttrib,
const graph::VertexAttribs& targetAttrib)
{
- int row = rowCount();
- setRowCount(row + 1);
+ QTableWidgetItem* result = nullptr;
- setItem(row, 0, new QTableWidgetItem {QString::fromStdString(sourceAttrib.getName())});
- setItem(row, 1, new QTableWidgetItem {QString::fromStdString(targetAttrib.getName())});
+ setSortingEnabled(false);
+ {
+ int row = rowCount();
+ setRowCount(row + 1);
+
+ setItem(row, 0, new QTableWidgetItem {QString::fromStdString(sourceAttrib.getName())});
+ setItem(row, 1, new QTableWidgetItem {QString::fromStdString(targetAttrib.getName())});
+
+ result = item(row, 0);
+ }
+ setSortingEnabled(true);
- return item(row, 0);
+ return result;
}
@@ -76,15 +84,19 @@ namespace armarx::nav::locgrapheditor
QFont font;
font.setBold(edge.attrib().highlighted);
- int row = this->row(edge.attrib().tableWidgetItem);
- for (int col = 0; col < 2; ++col)
+ setSortingEnabled(false);
{
- auto* item = this->item(row, col);
- ARMARX_CHECK_NOT_NULL(item);
+ int row = this->row(edge.attrib().tableWidgetItem);
+ for (int col = 0; col < 2; ++col)
+ {
+ auto* item = this->item(row, col);
+ ARMARX_CHECK_NOT_NULL(item);
- item->setData(Qt::BackgroundRole, bgColor);
- item->setFont(font);
+ item->setData(Qt::BackgroundRole, bgColor);
+ item->setFont(font);
+ }
}
+ setSortingEnabled(true);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index 7119c030..2aceb2bf 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -48,6 +48,8 @@ namespace armarx::nav::locgrapheditor
{
setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+ name = new QLineEdit(this);
+ name->setReadOnly(true);
locationID = new QLineEdit(this);
locationID->setReadOnly(true);
@@ -81,7 +83,8 @@ namespace armarx::nav::locgrapheditor
QFormLayout* layout = new QFormLayout();
this->setLayout(layout);
- layout->addRow("Location ID", locationID);
+ layout->addRow("Name", name);
+ layout->addRow("Entity ID", locationID);
layout->addRow("Frame", frame);
layout->addRow("X", x);
layout->addRow("Y", y);
@@ -142,30 +145,6 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
- {
- const VertexData& attrib = vertex.attrib();
- const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
-
- locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
- frame->setText("<WIP>");
- _setXyz(simox::math::position(pose));
- _setRpyRad(simox::math::mat4f_to_rpy(pose));
- }
-
-
- void VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
- {
- VertexData& attrib = vertex.attrib();
-
- // locationID is read-only
- // frame->setText("<WIP>"); // WIP
-
- Eigen::Matrix4d pose = simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad()));
- attrib.setPose(pose.cast<float>());
- }
-
-
Eigen::Vector3d VertexDataWidget::xyz() const
{
return { x->value(), y->value(), z->value() };
@@ -200,6 +179,32 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
+ {
+ const VertexData& attrib = vertex.attrib();
+ const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
+
+ name->setText(QString::fromStdString(attrib.getName()));
+ locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
+ frame->setText("<WIP>");
+ _setXyz(simox::math::position(pose));
+ _setRpyRad(simox::math::mat4f_to_rpy(pose));
+ }
+
+
+ void VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
+ {
+ VertexData& attrib = vertex.attrib();
+
+ // name is read-only
+ // locationID is read-only
+ // frame->setText("<WIP>"); // WIP
+
+ Eigen::Matrix4d pose = simox::math::pose(xyz(), simox::math::rpy_to_mat3f(rpyRad()));
+ attrib.setPose(pose.cast<float>());
+ }
+
+
void VertexDataWidget::_updateAngleUnit()
{
std::function<double(double)> convertValue;
@@ -230,10 +235,10 @@ namespace armarx::nav::locgrapheditor
};
}
- QList<QSignalBlocker>> blockers;
+ std::vector<QSignalBlocker> blockers;
for (QDoubleSpinBox* angle : _angleSpinBoxes())
{
- blockers.append(QSignalBlocker(angle));
+ blockers.emplace_back(angle);
angle->setSuffix(suffix);
angle->setMinimum(min);
@@ -252,9 +257,10 @@ namespace armarx::nav::locgrapheditor
void VertexDataWidget::_updateVertexAttribs()
{
- if (_vertex.has_value())
+ if (not signalsBlocked() and _vertex.has_value())
{
_getToVertex(_vertex.value());
+ _vertex->attrib().changed = true;
emit vertexDataChanged();
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index cb14f0c5..e16c824e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -89,6 +89,7 @@ namespace armarx::nav::locgrapheditor
std::optional<GuiGraph::Vertex> _vertex;
+ QLineEdit* name = nullptr;
QLineEdit* locationID = nullptr;
QLineEdit* frame = nullptr;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 2d1e965e..908ea14f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -22,6 +22,9 @@
#include "VertexTableWidget.h"
#include "utils.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
#include <QAction>
#include <QHeaderView>
#include <QMenu>
@@ -58,20 +61,36 @@ namespace armarx::nav::locgrapheditor
{
(void) vertex;
- int row = rowCount();
- setRowCount(row + 1);
+ // We need to disable sorting to prevent the new row from being moved away.
+ setSortingEnabled(false);
- // Just fill with vanilla items, they will get values in the update.
- for (int col = 0; col < 4; ++col)
- {
- setItem(row, col, new QTableWidgetItem {});
- }
- for (int col = 1; col <= 3; ++col)
+ QTableWidgetItem* result = nullptr;
{
- item(row, col)->setTextAlignment(Qt::AlignRight | Qt::AlignVCenter);
+ int row = rowCount();
+ setRowCount(row + 1);
+
+ for (int col = 0; col < 4; ++col)
+ {
+ // Just fill with vanilla items, they will get values in the update.
+ QTableWidgetItem* item = new QTableWidgetItem {};
+ setItem(row, col, item);
+
+ if (col >= 1)
+ {
+ item->setTextAlignment(Qt::AlignRight | Qt::AlignVCenter);
+ }
+ if (col == 0)
+ {
+ result = item;
+ }
+ }
}
- return item(row, 0);
+ // Enable sorting - `row` could now be moved away.
+ setSortingEnabled(true);
+
+ ARMARX_CHECK_NOT_NULL(result);
+ return result;
}
@@ -82,12 +101,24 @@ namespace armarx::nav::locgrapheditor
char format = 'f';
const int precision = 2;
+ // Changing the values may trigger a re-sort.
+ setSortingEnabled(false);
+
+
int row = this->row(vertex.attrib().tableWidgetItem);
- item(row, 0)->setText(QString::fromStdString(vertex.attrib().getName()));
+
+ QString displayName = QString::fromStdString(vertex.attrib().getName());
+ if (vertex.attrib().changed)
+ {
+ displayName += "*";
+ }
+ item(row, 0)->setText(displayName);
+ item(row, 0)->setData(Qt::UserRole, QString::fromStdString(vertex.attrib().getName()));
item(row, 1)->setText(QString::number(pose(0, 3), format, precision));
item(row, 2)->setText(QString::number(pose(1, 3), format, precision));
item(row, 3)->setText(QString::number(getYawAngleDegree(pose), format, precision));
+ setSortingEnabled(true);
QColor bgColor = vertex.attrib().highlighted ? bgColorSelected : bgColorDefault;
QFont font;
@@ -110,8 +141,13 @@ namespace armarx::nav::locgrapheditor
{
setCurrentItem(nullptr);
}
- this->removeRow(row(vertex.attrib().tableWidgetItem));
+
+ const int numRows = rowCount();
+ int row = this->row(vertex.attrib().tableWidgetItem);
+ removeRow(row);
vertex.attrib().tableWidgetItem = nullptr;
+
+ ARMARX_CHECK_EQUAL(rowCount(), numRows - 1);
}
@@ -122,6 +158,12 @@ namespace armarx::nav::locgrapheditor
}
+ QString VertexTableWidget::_nameOf(QTableWidgetItem* item)
+ {
+ return item->data(Qt::UserRole).toString();
+ }
+
+
void VertexTableWidget::makeContextMenu(QPoint pos)
{
QList<QTableWidgetItem*> items = selectedVertexItems();
@@ -141,22 +183,22 @@ namespace armarx::nav::locgrapheditor
// Generate actions for connecting these two nodes.
std::sort(items.begin(), items.end(), [](QTableWidgetItem* first, QTableWidgetItem* second)
{
- return first->text() < second->text();
+ return _nameOf(first) < _nameOf(second);
});
QTableWidgetItem* first = items[0];
QTableWidgetItem* second = items[1];
- connect(menu.addAction("Add edge '" + first->text() + "' " + utils::arrowRight + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowRight + " '" + _nameOf(second) + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{first, second}});
});
- connect(menu.addAction("Add edge '" + first->text() + "' " + utils::arrowLeft + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowLeft + " '" + _nameOf(second) + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{second, first}});
});
- connect(menu.addAction("Add edges '" + first->text() + "' " + utils::arrowBoth + " '" + second->text() + "'"),
+ connect(menu.addAction("Add edges '" + _nameOf(first) + "' " + utils::arrowBoth + " '" + _nameOf(second) + "'"),
&QAction::triggered, [this, first, second]()
{
emit newEdgesRequested({{first, second}, {second, first}});
@@ -170,7 +212,7 @@ namespace armarx::nav::locgrapheditor
? "edge"
: "edges";
QString desc = items.size() == 1
- ? "'" + items[0]->text() + "'"
+ ? "'" + _nameOf(items[0]) + "'"
: QString::number(items.size()) + " locations";
if (items.size() == 1)
@@ -199,7 +241,7 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem* action = this->item(row, 0);
if (items.count(action) == 0) // Do no generate self-edges
{
- QAction* a = submenu->addAction(action->text());
+ QAction* a = submenu->addAction(_nameOf(action));
connect(a, &QAction::triggered,
[this, &items, action, appendFunc]()
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index de609ce0..e782867f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -64,6 +64,11 @@ namespace armarx::nav::locgrapheditor
void makeContextMenu(QPoint pos);
+ private:
+
+ static QString _nameOf(QTableWidgetItem* item);
+
+
public:
QColor bgColorDefault = default_colors::tableBackgroundDefault;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
index e61b79b9..1a101fff 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -29,9 +29,12 @@
namespace armarx::nav::locgrapheditor
{
- const QString utils::arrowLeft = QStringLiteral("â†");
+ // <- https://unicode-table.com/de/2190/
+ const QString utils::arrowLeft = QStringLiteral("\u2190");
+ // -> https://unicode-table.com/de/2192/
const QString utils::arrowRight = QStringLiteral("\u2192");
- const QString utils::arrowBoth = QStringLiteral("⇄");
+ // <-> https://unicode-table.com/de/21C4/
+ const QString utils::arrowBoth = QStringLiteral("\u21C4");
QList<QTableWidgetItem*>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
index 44eb497a..ad866a66 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -33,11 +33,11 @@ namespace armarx::nav::locgrapheditor::utils
QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
- // <- https://unicode-table.com/de/2190/
+ /// <-
extern const QString arrowLeft;
- // -> https://unicode-table.com/de/2192/
+ /// ->
extern const QString arrowRight;
- // <-> https://unicode-table.com/de/21C4/
+ /// <->
extern const QString arrowBoth;
--
GitLab
From 1dd5dc6e25ef8c10a23904578dc8d80d8f62ff86 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 25 Aug 2021 15:12:05 +0200
Subject: [PATCH 22/33] Implement adding vertices and creating new graphs;
remove robot pose from graph aron
---
.../GraphImportExport/GraphImportExport.cpp | 3 +-
source/armarx/navigation/graph/Graph.cpp | 34 +-
source/armarx/navigation/graph/Graph.h | 11 +
source/armarx/navigation/graph/aron/Graph.xml | 5 +-
.../LocationGraphEditor/CMakeLists.txt | 2 +
.../LocationGraphEditorWidget.ui | 28 +-
.../LocationGraphEditorWidgetController.cpp | 579 +++++++++++-------
.../LocationGraphEditorWidgetController.h | 104 ++--
.../widgets/EdgeTableWidget.cpp | 13 +-
.../widgets/NewEntityIdDialog.cpp | 129 ++++
.../widgets/NewEntityIdDialog.h | 64 ++
.../widgets/VertexTableWidget.cpp | 58 +-
.../widgets/VertexTableWidget.h | 10 +-
.../LocationGraphEditor/widgets/utils.cpp | 1 +
.../LocationGraphEditor/widgets/utils.h | 12 +-
15 files changed, 748 insertions(+), 305 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 96a69c23..36936fee 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -26,7 +26,6 @@
#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
-#include <armarx/navigation/graph/Writer.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
@@ -373,7 +372,7 @@ namespace armarx::nav
nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
- vertex.attrib().aron.globalRobotPose = globalNodePose->toEigen();
+ vertex.attrib().setPose(globalNodePose->toEigen());
nextVertexID++;
}
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index 48c0f4dd..f34cb169 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -22,8 +22,14 @@
#include "Graph.h"
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+
+
namespace armarx::nav::graph
{
@@ -33,14 +39,20 @@ namespace armarx::nav::graph
return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
}
+ bool VertexAttribs::hasPose() const
+ {
+ return _pose.has_value();
+ }
+
Eigen::Matrix4f VertexAttribs::getPose() const
{
- return aron.globalRobotPose;
+ ARMARX_CHECK(_pose.has_value());
+ return _pose.value();
}
void VertexAttribs::setPose(const Eigen::Matrix4f& pose)
{
- this->aron.globalRobotPose = pose;
+ this->_pose = pose;
}
}
@@ -87,4 +99,22 @@ namespace armarx::nav
ARMARX_CHECK_EQUAL(bo.numEdges(), dto.edges.size());
}
+
+ void graph::resolveLocations(Graph& bo, armem::wm::Memory& locationMemory)
+ {
+ for (graph::Graph::Vertex vertex : bo.vertices())
+ {
+ armem::MemoryID locationID;
+ fromAron(vertex.attrib().aron.locationID, locationID);
+
+ armem::wm::EntityInstance* instance = locationMemory.findLatestInstance(locationID);
+ if (instance)
+ {
+ nav::loc::arondto::Location dto;
+ dto.fromAron(instance->data());
+ vertex.attrib().setPose(dto.globalRobotPose);
+ }
+ }
+ }
+
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index d895dd63..aa216d69 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -24,6 +24,8 @@
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
@@ -36,13 +38,21 @@ namespace armarx::nav::graph
std::string getName() const;
+ bool hasPose() const;
Eigen::Matrix4f getPose() const;
void setPose(const Eigen::Matrix4f& pose);
+
+ private:
+
+ std::optional<Eigen::Matrix4f> _pose;
+
};
+
struct EdgeAttribs
{
armarx::nav::graph::arondto::Edge aron;
};
+
struct GraphAttribs
{
};
@@ -52,5 +62,6 @@ namespace armarx::nav::graph
void toAron(arondto::Graph& dto, const Graph& bo);
void fromAron(const arondto::Graph& dto, Graph& bo);
+ void resolveLocations(Graph& bo, armem::wm::Memory& locationMemory);
}
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
index 18e1c847..fe09dcc0 100644
--- a/source/armarx/navigation/graph/aron/Graph.xml
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -19,9 +19,10 @@
<armarx::armem::arondto::MemoryID />
</ObjectChild>
- <ObjectChild key='globalRobotPose'>
+ <!-- Only stored in Location -->
+ <!--ObjectChild key='globalRobotPose'>
<Pose />
- </ObjectChild>
+ </ObjectChild-->
</Object>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 0820ee1b..e345f39d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -20,6 +20,7 @@ set(SOURCES
widgets/default_colors.cpp
widgets/utils.cpp
widgets/EdgeTableWidget.cpp
+ widgets/NewEntityIdDialog.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
@@ -37,6 +38,7 @@ set(HEADERS
widgets/default_colors.h
widgets/utils.h
widgets/EdgeTableWidget.h
+ widgets/NewEntityIdDialog.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 161687fa..88fa0f4c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -15,18 +15,28 @@
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
- <widget class="QGroupBox" name="sceneGroupBox">
+ <widget class="QGroupBox" name="graphGroupBox">
<property name="title">
<string>Navigation Graphs from Navigation Graph Segment</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
<item>
- <widget class="QPushButton" name="refreshGraphsButton">
+ <widget class="QPushButton" name="createGraphButton">
<property name="toolTip">
<string>Reloads the list of scenes from the memory</string>
</property>
<property name="text">
- <string>Refresh List</string>
+ <string>Create New Graph</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="queryGraphsButton">
+ <property name="toolTip">
+ <string>Reloads the list of scenes from the memory</string>
+ </property>
+ <property name="text">
+ <string>Query</string>
</property>
</widget>
</item>
@@ -110,6 +120,14 @@
</layout>
</widget>
</item>
+ <item>
+ <widget class="QGroupBox" name="robotVisuGroupBox">
+ <property name="title">
+ <string>Robot Visu</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_5"/>
+ </widget>
+ </item>
</layout>
</widget>
<widget class="QWidget" name="verticalLayout_13Widget">
@@ -117,7 +135,7 @@
<item>
<widget class="QGroupBox" name="locationsTableGroupBox">
<property name="title">
- <string>Locations (Graph Vertices)</string>
+ <string>Locations (right click for options)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_7">
<property name="leftMargin">
@@ -142,7 +160,7 @@
<item>
<widget class="QGroupBox" name="edgesTableGroupBox">
<property name="title">
- <string>Graph Edges</string>
+ <string>Graph Edges (right click for options)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_8">
<property name="leftMargin">
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index 3f4a7113..abe9388f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -23,8 +23,9 @@
#include <VirtualRobot/VirtualRobot.h>
#include "LocationGraphEditorWidgetController.h"
-#include "Visu.h"
+#include "widgets/utils.h"
#include "widgets/EdgeTableWidget.h"
+#include "widgets/NewEntityIdDialog.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
#include "widgets/graph_scene/Scene.h"
@@ -42,8 +43,7 @@
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SimoxUtility/color/Color.h>
+#include <ArmarXCore/core/logging/Logging.h>
// Qt headers
#include <QDialog>
@@ -67,30 +67,6 @@ static const QString SETTING_LAST_SCENE = "lastScene";
namespace armarx::nav::locgrapheditor
{
-
- /**
- * @brief Returns the name used on the debug layer.
- * @param edge The edge.
- * @return The name used on the debug layer.
- */
- std::string iceName(const GuiGraph::Edge& edge)
- {
- std::stringstream ss;
- ss << "edge " << std::to_string(edge.sourceObjectID()) << " -> " << std::to_string(edge.targetObjectID());
- return ss.str();
- }
-
- /**
- * @brief iceName Returns the name used on the debug layer.
- * @param vertexName The vertex.
- * @return The name used on the debug layer.
- */
- std::string iceName(const GuiGraph::Vertex& vertex)
- {
- return std::to_string(vertex.objectID());
- }
-
-
QString LocationGraphEditorWidgetController::GetWidgetName()
{
return "Navigation.LocationGraphEditor";
@@ -110,31 +86,49 @@ namespace armarx::nav::locgrapheditor
widget.loadGraphButton->setEnabled(false);
- view.edgeTable = new EdgeTableWidget();
- view.vertexTable = new VertexTableWidget();
view.vertexData = new VertexDataWidget();
view.vertexData->setEnabled(false); // Enable on first selection of vertex.
- widget.edgesTableGroupBox->layout()->addWidget(view.edgeTable);
- widget.locationsTableGroupBox->layout()->addWidget(view.vertexTable);
+ view.vertexTable = new VertexTableWidget();
+ view.edgeTable = new EdgeTableWidget();
+
widget.locationDataGroupBox->layout()->addWidget(view.vertexData);
+ if (QBoxLayout* layout = dynamic_cast<QBoxLayout*>(widget.locationDataGroupBox->layout()))
+ {
+ layout->addStretch();
+ }
+
+ widget.locationsTableGroupBox->layout()->addWidget(view.vertexTable);
+ widget.edgesTableGroupBox->layout()->addWidget(view.edgeTable);
view.graph = new GraphSceneWidget();
widget.graphSceneLayout->addWidget(view.graph);
- // Widgets -> This
-
- // Memory Access
connect(this, &This::connected, this, &This::queryGraphs);
- connect(this, &This::memoryDataChanged, this, &This::updateGraphList);
- connect(widget.refreshGraphsButton, &QPushButton::pressed, this, &This::updateGraphList);
- connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
- connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commit);
+ connect(widget.createGraphButton, &QPushButton::pressed,
+ this, &This::createGraphDialog);
+
+ connect(widget.queryGraphsButton, &QPushButton::pressed,
+ this, &This::queryGraphs);
+ connect(this, &This::locationMemoryChanged,
+ this, &This::memoryChanged);
+ connect(this, &This::graphMemoryChanged,
+ this, &This::memoryChanged);
- // Update views
- connect(this, &This::graphChanged, this, &This::updateGraphView);
+ connect(this, &This::graphMemoryChanged,
+ this, &This::updateGraphList);
+
+ connect(widget.loadGraphButton, &QPushButton::pressed,
+ this, &This::loadGraph);
+ connect(widget.commitGraphButton, &QPushButton::pressed,
+ this, &This::commit);
+
+
+ // View updates
+ connect(this, &This::graphChanged,
+ this, &This::updateGraphView);
connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
this, &This::updateGraphView);
@@ -151,8 +145,15 @@ namespace armarx::nav::locgrapheditor
connect(view.edgeTable, &EdgeTableWidget::itemSelectionChanged,
this, &This::updateEdgeHighlighting);
+ // Graph modification
+
+ connect(view.vertexTable, &VertexTableWidget::newVertexRequested,
+ this, &This::createVertexDialog);
connect(view.vertexTable, &VertexTableWidget::newEdgesRequested,
this, &This::addEdges);
+ connect(view.vertexTable, &VertexTableWidget::edgeRemovalRequested,
+ this, &This::removeEdgesOfVertex);
+
connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested,
this, &This::removeEdges);
}
@@ -166,18 +167,18 @@ namespace armarx::nav::locgrapheditor
QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
{
- if (not dialog)
+ if (not configDialog)
{
- dialog = new SimpleConfigDialog(parent);
- dialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
+ configDialog = new SimpleConfigDialog(parent);
+ configDialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
}
- return qobject_cast<SimpleConfigDialog*>(dialog);
+ return qobject_cast<SimpleConfigDialog*>(configDialog);
}
void LocationGraphEditorWidgetController::configured()
{
- remote.memoryNameSystemName = dialog->getProxyName("MemoryNameSystem");
+ remote.memoryNameSystemName = configDialog->getProxyName("MemoryNameSystem");
}
@@ -217,8 +218,8 @@ namespace armarx::nav::locgrapheditor
{
std::stringstream ss;
ss << "Navigation Graphs (Entities from Core Segment " << nav::graph::coreSegmentID << ")";
- widget.sceneGroupBox->setTitle(QString::fromStdString(ss.str()));
- widget.sceneGroupBox->setEnabled(true);
+ widget.graphGroupBox->setTitle(QString::fromStdString(ss.str()));
+ widget.graphGroupBox->setEnabled(true);
}
emit connected();
@@ -240,18 +241,49 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::queryGraphs()
+ void LocationGraphEditorWidgetController::queryMemory()
{
- armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
+ armem::client::QueryResult locResult = queryLocations();
+ armem::client::QueryResult graphResult = queryGraphs();
+
+ if (not (locResult.success and graphResult.success))
+ {
+ QStringList status;
+ std::stringstream ss;
+ if (not locResult.success)
+ {
+ status.append(QString::fromStdString(locResult.errorMessage));
+ }
+ if (not graphResult.success)
+ {
+ status.append(QString::fromStdString(graphResult.errorMessage));
+ }
+ widget.statusLabel->setText(status.join("\n"));
+ }
+ }
+
+
+ armem::client::QueryResult LocationGraphEditorWidgetController::queryLocations()
+ {
+ armem::client::QueryResult result = remote.locationReader.getLatestSnapshotsIn(nav::loc::coreSegmentID);
if (result.success)
{
- model.memory = std::move(result.memory);
- emit memoryDataChanged();
+ model.locationsMemory = std::move(result.memory);
+ emit locationMemoryChanged();
}
- else
+ return result;
+ }
+
+
+ armem::client::QueryResult LocationGraphEditorWidgetController::queryGraphs()
+ {
+ armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
+ if (result.success)
{
- widget.statusLabel->setText(QString::fromStdString(result.errorMessage));
+ model.graphMemory = std::move(result.memory);
+ emit graphMemoryChanged();
}
+ return result;
}
@@ -262,7 +294,7 @@ namespace armarx::nav::locgrapheditor
int i = 0;
int previousIndex = -1; // To keep selection.
- model.memory.forEachEntity([&](const armem::wm::Entity& entity)
+ model.graphMemory.forEachEntity([&](const armem::wm::Entity& entity)
{
bool hasChanged = (entity.id() == model.graphEntityID
? model.graph.hasChanged()
@@ -289,34 +321,9 @@ namespace armarx::nav::locgrapheditor
{
if (model.graph.hasChanged())
{
- QMessageBox msgBox;
- msgBox.setText("The current graph and/or locations have uncommitted changes. ");
- msgBox.setInformativeText("Do you want to discard them?");
- QStringList detailLines;
- if (model.graph.attrib().edgesChanged)
- {
- detailLines.append("Graph edges have changed.");
- }
- for (auto vertex : model.graph.vertices())
- {
- if (vertex.attrib().changed)
- {
- detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) + " has changed.");
- }
- }
- msgBox.setDetailedText(detailLines.join("\n"));
- msgBox.setStandardButtons(QMessageBox::Discard | QMessageBox::Cancel);
- msgBox.setDefaultButton(QMessageBox::Cancel);
-
- int ret = msgBox.exec();
- switch (ret)
+ if (not loadGraphDialog())
{
- case QMessageBox::Discard:
- // Ok go.
- break;
- case QMessageBox::Cancel:
- // Abort loading.
- return;
+ return;
}
}
@@ -324,9 +331,9 @@ namespace armarx::nav::locgrapheditor
widget.graphsComboBox->currentData().toString().toStdString());
// Refresh local memory.
- queryGraphs();
+ queryMemory();
- const armem::wm::EntityInstance* instance = model.memory.findLatestInstance(entityID);
+ const armem::wm::EntityInstance* instance = model.graphMemory.findLatestInstance(entityID);
if (instance)
{
widget.statusLabel->setText(QString::fromStdString("Loaded snapshot " + instance->id().getEntitySnapshotID().str()));
@@ -341,9 +348,32 @@ namespace armarx::nav::locgrapheditor
nav::graph::Graph nav;
{
nav::graph::arondto::Graph dto;
+ ARMARX_CHECK_NOT_NULL(instance->data());
dto.fromAron(instance->data());
fromAron(dto, nav);
}
+ // Resolve locations and remove vertices which could not be resolved.
+ {
+ resolveLocations(nav, model.locationsMemory);
+ std::vector<nav::graph::Graph::Vertex> remove;
+ for (nav::graph::Graph::Vertex vertex : nav.vertices())
+ {
+ if (not vertex.attrib().hasPose())
+ {
+ remove.push_back(vertex);
+ }
+ }
+ if (not remove.empty())
+ {
+ for (nav::graph::Graph::Vertex vertex : remove)
+ {
+ nav.removeVertex(vertex.objectID());
+ }
+ std::stringstream ss;
+ ss << "Dropped " << remove.size() << " locations which could not be resolved.";
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+ }
model.graphEntityID = entityID;
ARMARX_VERBOSE << "Loading graph " << nav.str();
@@ -351,6 +381,42 @@ namespace armarx::nav::locgrapheditor
}
+
+ bool LocationGraphEditorWidgetController::loadGraphDialog()
+ {
+ ARMARX_CHECK(model.graph.hasChanged());
+
+ QMessageBox msgBox;
+ msgBox.setText("The current graph and/or locations have uncommitted changes. ");
+ msgBox.setInformativeText("Do you want to discard them?");
+ QStringList detailLines;
+ if (model.graph.attrib().edgesChanged)
+ {
+ detailLines.append("Graph edges have changed.");
+ }
+ for (auto vertex : model.graph.vertices())
+ {
+ if (vertex.attrib().changed)
+ {
+ detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) + " has changed.");
+ }
+ }
+ msgBox.setDetailedText(detailLines.join("\n"));
+ msgBox.setStandardButtons(QMessageBox::Discard | QMessageBox::Cancel);
+ msgBox.setDefaultButton(QMessageBox::Cancel);
+
+ switch (msgBox.exec())
+ {
+ case QMessageBox::Discard:
+ return true; // Ok go.
+ case QMessageBox::Cancel:
+ return false; // Abort loading.
+ default:
+ return false; // Something went wrong.
+ }
+ }
+
+
void
LocationGraphEditorWidgetController::commit()
{
@@ -460,7 +526,7 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::setGraph(graph::Graph& nav)
+ void LocationGraphEditorWidgetController::setGraph(const graph::Graph& nav)
{
// Build the gui graph (model).
{
@@ -471,11 +537,14 @@ namespace armarx::nav::locgrapheditor
for (auto vertex : nav.vertices())
{
- addVertex(vertex);
+ ARMARX_CHECK(vertex.attrib().hasPose());
+ addVertex(vertex.objectID(), { vertex.attrib() });
}
for (auto edge : nav.edges())
{
- addEdge(edge);
+ addEdge(model.graph.vertex(edge.sourceObjectID()),
+ model.graph.vertex(edge.targetObjectID()),
+ { edge.attrib() });
}
}
@@ -484,25 +553,19 @@ namespace armarx::nav::locgrapheditor
}
- GuiGraph::Vertex
- LocationGraphEditorWidgetController::addVertex(graph::Graph::ConstVertex vertex)
+ GuiGraph::Vertex LocationGraphEditorWidgetController::addVertex(
+ semrel::ShapeID vertexID,
+ const VertexData& defaultAttribs)
{
- ARMARX_CHECK(not model.graph.hasVertex(vertex.objectID()));
+ ARMARX_CHECK(not model.graph.hasVertex(vertexID));
- VertexData attrib { vertex.attrib() };
- attrib.graphicsItem = view.graph->scene()->addVertex(vertex);
- attrib.tableWidgetItem = view.vertexTable->addVertex(vertex);
- return model.graph.addVertex(vertex.objectID(), attrib);
- }
+ GuiGraph::Vertex vertex = model.graph.addVertex(vertexID, defaultAttribs);
+ vertex.attrib().graphicsItem = view.graph->scene()->addVertex(vertex);
+ vertex.attrib().tableWidgetItem = view.vertexTable->addVertex();
+ emit graphChanged();
- GuiGraph::Edge
- LocationGraphEditorWidgetController::addEdge(graph::Graph::ConstEdge edge)
- {
- auto source = model.graph.vertex(edge.sourceObjectID());
- auto target = model.graph.vertex(edge.targetObjectID());
-
- return addEdge(source, target, { edge.attrib() });
+ return vertex;
}
@@ -520,68 +583,10 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Edge edge = model.graph.addEdge(source, target, defaultAttribs);
edge.attrib().tableWidgetItem = view.edgeTable->addEdge(edge);
edge.attrib().graphicsItem = view.graph->scene()->addEdge(edge);
- return edge;
- }
-
-
- void LocationGraphEditorWidgetController::addEdges(
- QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
- {
- if (/* DISABLES CODE */ (false))
- {
- std::stringstream ss;
- ss << "Adding edges ...";
- for (const auto& [source, target] : vertexItems)
- {
- ss << "\n- " << source->text() << " to " << target->text();
- }
- ARMARX_IMPORTANT << ss.str();
- }
-
- const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
- for (const auto& [sourceItem, targetItem] : vertexItems)
- {
- GuiGraph::Vertex source = itemToVertexMap.at(sourceItem);
- GuiGraph::Vertex target = itemToVertexMap.at(targetItem);
- if (not model.graph.hasEdge(source, target))
- {
- addEdge(source, target, {});
- }
- }
-
- model.graph.attrib().edgesChanged = true;
emit graphChanged();
- }
-
-
- void LocationGraphEditorWidgetController::removeEdges(
- QList<QTableWidgetItem*> edgeItems)
- {
- if (/* DISABLES CODE */ (false))
- {
- std::stringstream ss;
- ss << "Remoiving edges ...";
- for (const auto& edgeItem : edgeItems)
- {
- ss << "\n- " << edgeItem->text();
- }
- ARMARX_IMPORTANT << ss.str();
- }
- {
- QSignalBlocker blocker(this);
-
- const auto itemToEdgeMap = model.graph.getTableItemToEdgeMap();
- for (const auto& edgeItem : edgeItems)
- {
- GuiGraph::Edge edge = itemToEdgeMap.at(edgeItem);
- removeEdge(edge);
- }
- }
- model.graph.attrib().edgesChanged = true;
-
- emit graphChanged();
+ return edge;
}
@@ -606,8 +611,7 @@ namespace armarx::nav::locgrapheditor
}
- void
- LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
+ void LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
{
view.graph->scene()->updateVertex(vertex);
view.vertexTable->updateVertex(vertex);
@@ -653,14 +657,68 @@ namespace armarx::nav::locgrapheditor
{
if (remote.arviz)
{
- viz::Layer layer = remote.arviz->layer(widget.graphsComboBox->currentText().toStdString());
- GraphVisu visu;
- visu.draw(layer, model.graph);
+ const QString layerName = getGraphDisplayName(model.graphEntityID, false);
+ viz::Layer layer = remote.arviz->layer(layerName.toStdString());
+ model.visu.draw(layer, model.graph);
remote.arviz->commit(layer);
}
}
+ template <class T>
+ static
+ void updateElementHighlighting(
+ QList<QTableWidgetItem*> selectedItems,
+ std::map<QTableWidgetItem*, T>&& itemToElementMap)
+ {
+ for (QTableWidgetItem* selected : selectedItems)
+ {
+ if (auto it = itemToElementMap.find(selected); it != itemToElementMap.end())
+ {
+ it->second.attrib().highlighted = true;
+ itemToElementMap.erase(it);
+ }
+ }
+ for (auto& [_, unselected] : itemToElementMap)
+ {
+ unselected.attrib().highlighted = false;
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ {
+ updateElementHighlighting(view.vertexTable->selectedVertexItems(), model.graph.getTableItemToVertexMap());
+ emit graphChanged();
+ }
+
+
+ void LocationGraphEditorWidgetController::updateEdgeHighlighting()
+ {
+ updateElementHighlighting(view.edgeTable->selectedEdgeItems(), model.graph.getTableItemToEdgeMap());
+ emit graphChanged();
+ }
+
+
+ void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
+ {
+ if (vertexItem == nullptr)
+ {
+ view.vertexData->clearVertex();
+ }
+ if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
+ {
+ selectVertex(vertex.value());
+ }
+ }
+
+
+ void LocationGraphEditorWidgetController::selectVertex(GuiGraph::Vertex vertex)
+ {
+ view.vertexData->setVertex(vertex);
+ }
+
+
void LocationGraphEditorWidgetController::clearGraph()
{
{
@@ -681,10 +739,13 @@ namespace armarx::nav::locgrapheditor
{
// Remove in reverse order to be more array-friendly.
std::vector<GuiGraph::Edge> edges { model.graph.edges().begin(), model.graph.edges().end() };
- for (auto it = edges.rbegin(); it != edges.rend(); ++it)
{
- GuiGraph::Edge edge = *it;
- removeEdge(edge);
+ QSignalBlocker blocker(this);
+ for (auto it = edges.rbegin(); it != edges.rend(); ++it)
+ {
+ GuiGraph::Edge edge = *it;
+ removeEdge(edge);
+ }
}
ARMARX_CHECK_EQUAL(view.edgeTable->rowCount(), 0);
@@ -701,10 +762,13 @@ namespace armarx::nav::locgrapheditor
// Remove in reverse order to be more array-friendly.
std::vector<GuiGraph::Vertex> vertices { model.graph.vertices().begin(), model.graph.vertices().end() };
- for (auto it = vertices.rbegin(); it != vertices.rend(); ++it)
{
- GuiGraph::Vertex vertex = *it;
- removeVertex(vertex);
+ QSignalBlocker blocker(this);
+ for (auto it = vertices.rbegin(); it != vertices.rend(); ++it)
+ {
+ GuiGraph::Vertex vertex = *it;
+ removeVertex(vertex);
+ }
}
ARMARX_CHECK_EQUAL(view.vertexTable->rowCount(), 0);
@@ -720,30 +784,35 @@ namespace armarx::nav::locgrapheditor
<< "Cannot remove edge that does not exist.";
// Remove view elements
+ {
+ QSignalBlocker blocker(view.edgeTable);
+ view.edgeTable->removeEdge(edge);
+ }
view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
- const int rows = view.edgeTable->rowCount();
- view.edgeTable->removeEdge(edge);
- ARMARX_CHECK_EQUAL(view.edgeTable->rowCount(), rows - 1);
-
// Remove from model
model.graph.removeEdge(edge);
ARMARX_CHECK(not model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
<< edge.sourceDescriptor() << " -> " << edge.targetDescriptor();
+
+ emit graphChanged();
}
void LocationGraphEditorWidgetController::removeVertex(GuiGraph::Vertex& vertex)
{
ARMARX_CHECK_EQUAL(vertex.inDegree(), 0)
- << "A vertex may not have any edges before being removed. " << vertex.attrib().getName();
+ << "A vertex must not have any edges before being removed. " << vertex.attrib().getName();
ARMARX_CHECK_EQUAL(vertex.outDegree(), 0)
- << "A vertex may not have any edges before being removed. " << vertex.attrib().getName();
+ << "A vertex must not have any edges before being removed. " << vertex.attrib().getName();
// Remove view elements
+ {
+ QSignalBlocker blocker(view.vertexTable);
+ view.vertexTable->removeVertex(vertex);
+ }
view.graph->scene()->removeVertex(vertex.attrib().graphicsItem);
- view.vertexTable->removeVertex(vertex);
if (view.vertexData->vertex().has_value() and view.vertexData->vertex().value() == vertex)
{
view.vertexData->clearVertex();
@@ -751,81 +820,159 @@ namespace armarx::nav::locgrapheditor
// Remove from model
model.graph.removeVertex(vertex);
+
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::resetHighlighting()
+ void LocationGraphEditorWidgetController::createVertexDialog()
{
- for (auto vertex : model.graph.vertices())
+ NewEntityIdDialog dialog(nav::loc::coreSegmentID, nullptr);
+ switch (dialog.exec())
{
- if (vertex.attrib().highlighted)
- {
- vertex.attrib().highlighted = false;
- }
+ case QDialog::Accepted:
+ // Ok go.
+ break;
+ case QDialog::Rejected:
+ // Abort creation.
+ return;
}
- for (auto edge : model.graph.edges())
+
+ // Find free vertex ID
+ semrel::ShapeID vertexID { 0 };
+ while (model.graph.hasVertex(vertexID))
{
- if (edge.attrib().highlighted)
- {
- edge.attrib().highlighted = false;
- }
+ ++vertexID;
}
- emit graphChanged();
+ // Initiaize attributes
+ VertexData attribs;
+ toAron(attribs.aron.locationID, dialog.entityID());
+ attribs.setPose(Eigen::Matrix4f::Identity());
+ attribs.changed = true;
+
+ addVertex(vertexID, attribs);
}
- template <class T>
- static
- void updateElementHighlighting(
- QList<QTableWidgetItem*> selectedItems,
- std::map<QTableWidgetItem*, T>&& itemToElementMap)
+ void LocationGraphEditorWidgetController::addEdges(
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
{
- for (QTableWidgetItem* selected : selectedItems)
+ const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
{
- if (auto it = itemToElementMap.find(selected); it != itemToElementMap.end())
+ QSignalBlocker blocker(this);
+
+ for (const auto& [sourceItem, targetItem] : vertexItems)
{
- it->second.attrib().highlighted = true;
- itemToElementMap.erase(it);
+ GuiGraph::Vertex source = itemToVertexMap.at(sourceItem);
+ GuiGraph::Vertex target = itemToVertexMap.at(targetItem);
+ if (not model.graph.hasEdge(source, target))
+ {
+ addEdge(source, target, {});
+ }
}
}
- for (auto& [_, unselected] : itemToElementMap)
- {
- unselected.attrib().highlighted = false;
- }
+
+ model.graph.attrib().edgesChanged = true;
+ emit graphChanged();
}
- void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ void LocationGraphEditorWidgetController::removeEdges(
+ QList<QTableWidgetItem*> edgeItems)
{
- updateElementHighlighting(view.vertexTable->selectedVertexItems(), model.graph.getTableItemToVertexMap());
+ const auto itemToEdgeMap = model.graph.getTableItemToEdgeMap();
+ {
+ QSignalBlocker blocker(this);
+
+ for (const auto& edgeItem : edgeItems)
+ {
+ GuiGraph::Edge edge = itemToEdgeMap.at(edgeItem);
+ removeEdge(edge);
+ }
+ }
+
+ model.graph.attrib().edgesChanged = true;
emit graphChanged();
}
- void LocationGraphEditorWidgetController::updateEdgeHighlighting()
+ void LocationGraphEditorWidgetController::removeEdgesOfVertex(
+ QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction)
{
- updateElementHighlighting(view.edgeTable->selectedEdgeItems(), model.graph.getTableItemToEdgeMap());
+ auto removeEdges = [this](auto&& range)
+ {
+ for (auto edge : range)
+ {
+ removeEdge(edge);
+ }
+ };
+ {
+ QSignalBlocker blocker(this);
+
+ const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
+ for (const auto& vertexItem : vertexItems)
+ {
+ GuiGraph::Vertex vertex = itemToVertexMap.at(vertexItem);
+ switch (direction)
+ {
+ case utils::EdgeDirection::To:
+ removeEdges(vertex.inEdges());
+ break;
+
+ case utils::EdgeDirection::From:
+ removeEdges(vertex.outEdges());
+ break;
+
+ case utils::EdgeDirection::Bidirectional:
+ removeEdges(vertex.inEdges());
+ removeEdges(vertex.outEdges());
+ break;
+ }
+ }
+ }
+
+ model.graph.attrib().edgesChanged = true;
emit graphChanged();
}
- void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
+ void LocationGraphEditorWidgetController::createGraphDialog()
{
- if (vertexItem == nullptr)
+ NewEntityIdDialog dialog(nav::graph::coreSegmentID, nullptr);
+ switch (dialog.exec())
{
- view.vertexData->clearVertex();
+ case QDialog::Accepted:
+ break;
+ case QDialog::Rejected:
+ return;
}
- if (auto vertex = model.graph.getVertexFromTableItem(vertexItem))
+
+ // Find free vertex ID
+ semrel::ShapeID vertexID { 0 };
+ while (model.graph.hasVertex(vertexID))
{
- selectVertex(vertex.value());
+ ++vertexID;
}
- }
+ const armem::MemoryID entityID = dialog.entityID();
- void LocationGraphEditorWidgetController::selectVertex(GuiGraph::Vertex vertex)
- {
- view.vertexData->setVertex(vertex);
+ const bool hasChanged = true;
+ QString text = getGraphDisplayName(entityID, hasChanged);
+ QString data = QString::fromStdString(entityID.str());
+ widget.graphsComboBox->addItem(text, data);
+ widget.graphsComboBox->setCurrentIndex(widget.graphsComboBox->count() - 1);
+
+ {
+ QSignalBlocker blocker(this);
+ clearEdges();
+ // clearVertices();
+ }
+ model.graph.attrib().edgesChanged = true;
+ model.graphEntityID = entityID;
+
+ emit graphChanged();
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index e4de7136..32ae0ccd 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -21,8 +21,8 @@
*/
#pragma once
-#include "FunctionalEventFilter.h"
#include "GuiGraph.h"
+#include "Visu.h"
#include "widgets/graph_scene.h"
#include <armarx/navigation/graph/Graph.h>
@@ -52,6 +52,10 @@ namespace armarx::viz
{
class Client;
}
+namespace armarx::nav::locgrapheditor::utils
+{
+ enum class EdgeDirection;
+}
namespace armarx::nav::locgrapheditor
{
class EdgeTableWidget;
@@ -127,59 +131,73 @@ namespace armarx::nav::locgrapheditor
signals:
void connected();
- void memoryDataChanged();
+
+ void locationMemoryChanged();
+ void graphMemoryChanged();
+ void memoryChanged();
+
void graphChanged();
private slots:
- // Model
+ // Load
+ void queryMemory();
+ armem::client::QueryResult queryLocations();
+ armem::client::QueryResult queryGraphs();
- void queryGraphs();
- void updateGraphList();
void loadGraph();
+ bool loadGraphDialog();
- void setGraph(graph::Graph& nav);
- GuiGraph::Vertex addVertex(graph::Graph::ConstVertex vertex);
- GuiGraph::Edge addEdge(graph::Graph::ConstEdge edge);
- GuiGraph::Edge addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target,
- const EdgeData& defaultAttribs);
-
- void clearGraph();
- void clearEdges();
- void clearVertices();
-
- void removeEdge(GuiGraph::Edge& edge);
- void removeVertex(GuiGraph::Vertex& vertex);
-
- void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
- void removeEdges(QList<QTableWidgetItem*> edgeItems);
-
-
+ // Commit
void commit();
armem::CommitResult commitLocations();
armem::EntityUpdateResult commitGraph();
- // View & Tables
+ // View & Tables
+ void updateGraphList();
void updateGraphView();
void updateVertexView(GuiGraph::Vertex vertex);
void updateEdgeView(GuiGraph::Edge edge);
void updateArViz();
- // Selection
-
+ // Selection & Highlighting
void selectVertex(QTableWidgetItem* vertexItem);
void selectVertex(GuiGraph::Vertex vertex);
-
- void resetHighlighting();
void updateVertexHighlighting();
void updateEdgeHighlighting();
+ // Graph modification triggered by user interaction
+ void createVertexDialog();
+
+ void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
+ void removeEdges(QList<QTableWidgetItem*> edgeItems);
+ void removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
+
+ void createGraphDialog();
+
+
private:
+ void setGraph(const graph::Graph& nav);
+
+ GuiGraph::Vertex
+ addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs);
+
+ GuiGraph::Edge
+ addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target, const EdgeData& defaultAttribs);
+
+ void removeVertex(GuiGraph::Vertex& vertex);
+ void removeEdge(GuiGraph::Edge& edge);
+
+ void clearGraph();
+ void clearEdges();
+ void clearVertices();
+
+
QString getGraphDisplayName(const armem::MemoryID& entityID, bool changed = false) const;
@@ -187,7 +205,7 @@ namespace armarx::nav::locgrapheditor
/// Widget Form
Ui::LocationGraphEditorWidget widget;
- QPointer<SimpleConfigDialog> dialog;
+ QPointer<SimpleConfigDialog> configDialog;
struct Remote
@@ -210,9 +228,13 @@ namespace armarx::nav::locgrapheditor
struct Model
{
- armem::wm::Memory memory;
+ armem::wm::Memory locationsMemory;
+
+ armem::wm::Memory graphMemory;
armem::MemoryID graphEntityID;
GuiGraph graph;
+
+ GraphVisu visu;
};
Model model;
@@ -228,30 +250,6 @@ namespace armarx::nav::locgrapheditor
View view;
- // Non-refactored
-
-#if 0
- private slots:
-
- void tableWidgetVerticesCustomContextMenu(QPoint pos);
- void tableWidgetEdgesCustomContextMenu(QPoint pos);
-
-
- private slots:
-
- /// Add kitchen graph (H2T Armar3a robot kitchen)
- void addKitchenGraph();
-
- bool addNewEdge(const std::string& from, const std::string& to);
- void addNewEdgeBoth();
- void addNewEdgeStartEnd();
- void addNewEdgeEndStart();
-
- void addNewGraphVertex();
- void editGraphVertex();
-#endif
-
-
private:
QSettings settings;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index adecbca4..93d87392 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -100,9 +100,15 @@ namespace armarx::nav::locgrapheditor
}
- void EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
+ void
+ EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
{
- this->removeRow(row(edge.attrib().tableWidgetItem));
+ if (currentItem() == edge.attrib().tableWidgetItem)
+ {
+ setCurrentItem(nullptr);
+ }
+
+ removeRow(row(edge.attrib().tableWidgetItem));
edge.attrib().tableWidgetItem = nullptr;
}
@@ -114,7 +120,8 @@ namespace armarx::nav::locgrapheditor
}
- void EdgeTableWidget::makeContextMenu(QPoint pos)
+ void
+ EdgeTableWidget::makeContextMenu(QPoint pos)
{
QList<QTableWidgetItem*> items = selectedEdgeItems();
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
new file mode 100644
index 00000000..381556b4
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
@@ -0,0 +1,129 @@
+/*
+ * 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/>.
+ *
+ * @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 "NewEntityIdDialog.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <QDialogButtonBox>
+#include <QGridLayout>
+#include <QLineEdit>
+#include <QLabel>
+#include <QPushButton>
+#include <QVBoxLayout>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ NewEntityIdDialog::NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent) :
+ QDialog(parent),
+ coreSegmentID(std::make_unique<armem::MemoryID>(coreSegmentID))
+ {
+ _providerSegmentName = new QLineEdit(this);
+ _entityName = new QLineEdit(this);
+
+ _buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok
+ | QDialogButtonBox::Cancel, this);
+
+ QLabel* instruction = new QLabel("Enter provider segment name and entity name for new entity:");
+ QFont font = instruction->font();
+ font.setBold(true);
+ instruction->setFont(font);
+
+
+ QGridLayout* grid = new QGridLayout();
+ int col = 0;
+
+ grid->addWidget(new QLabel("Core Segment ID"), 0, col);
+ grid->addWidget(new QLabel(QString::fromStdString(coreSegmentID.str()) + "/"), 1, col);
+ ++col;
+
+ grid->addWidget(new QLabel("Provider Segment Name"), 0, col);
+ grid->addWidget(_providerSegmentName, 1, col);
+ ++col;
+
+ grid->addWidget(new QLabel("/"), 1, col);
+ ++col;
+
+ grid->addWidget(new QLabel("Entity Name"), 0, col);
+ grid->addWidget(_entityName, 1, col);
+ ++col;
+
+
+ QVBoxLayout* layout = new QVBoxLayout();
+ setLayout(layout);
+ layout->addWidget(instruction);
+ layout->addLayout(grid);
+ layout->addWidget(_buttonBox);
+
+
+ connect(_buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept);
+ connect(_buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject);
+
+ auto enableOkIfReady = [this]()
+ {
+ _buttonBox->button(QDialogButtonBox::Ok)
+ ->setEnabled(_providerSegmentName->text().size() > 0 and _entityName->text().size() > 0);
+ };
+ connect(_providerSegmentName, &QLineEdit::textChanged, this, enableOkIfReady);
+ connect(_entityName, &QLineEdit::textChanged, this, enableOkIfReady);
+
+ enableOkIfReady();
+ }
+
+
+ NewEntityIdDialog::~NewEntityIdDialog()
+ {
+ }
+
+
+ QString NewEntityIdDialog::providerSegmentName() const
+ {
+ return _providerSegmentName->text();
+ }
+
+
+ QString NewEntityIdDialog::entityName() const
+ {
+ return _entityName->text();
+ }
+
+
+ armem::MemoryID NewEntityIdDialog::entityIDWithoutCoreSegmentID() const
+ {
+ armem::MemoryID id;
+ id.providerSegmentName = providerSegmentName().toStdString();
+ id.entityName = entityName().toStdString();
+ return id;
+ }
+
+ armem::MemoryID NewEntityIdDialog::entityID() const
+ {
+ armem::MemoryID entityID = entityIDWithoutCoreSegmentID();
+ ARMARX_CHECK_NOT_NULL(coreSegmentID);
+ entityID.setCoreSegmentID(*coreSegmentID);
+ return entityID;
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
new file mode 100644
index 00000000..18659cda
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
@@ -0,0 +1,64 @@
+/*
+ * 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/>.
+ *
+ * @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 <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+#include <QDialog>
+
+#include <memory>
+
+
+class QLineEdit;
+class QDialogButtonBox;
+
+
+namespace armarx::nav::locgrapheditor
+{
+ class NewEntityIdDialog : public QDialog
+ {
+ public:
+
+ NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent = nullptr);
+ virtual ~NewEntityIdDialog() override;
+
+ QString providerSegmentName() const;
+ QString entityName() const;
+
+
+ armem::MemoryID entityIDWithoutCoreSegmentID() const;
+ armem::MemoryID entityID() const;
+
+
+ private:
+
+ QLineEdit* _providerSegmentName = nullptr;
+ QLineEdit* _entityName = nullptr;
+
+ QDialogButtonBox* _buttonBox = nullptr;
+
+
+ std::unique_ptr<armem::MemoryID> coreSegmentID;
+
+ };
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index 908ea14f..b1a5ceed 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -57,10 +57,8 @@ namespace armarx::nav::locgrapheditor
QTableWidgetItem*
- VertexTableWidget::addVertex(graph::Graph::ConstVertex vertex)
+ VertexTableWidget::addVertex()
{
- (void) vertex;
-
// We need to disable sorting to prevent the new row from being moved away.
setSortingEnabled(false);
@@ -142,12 +140,8 @@ namespace armarx::nav::locgrapheditor
setCurrentItem(nullptr);
}
- const int numRows = rowCount();
- int row = this->row(vertex.attrib().tableWidgetItem);
- removeRow(row);
+ removeRow(row(vertex.attrib().tableWidgetItem));
vertex.attrib().tableWidgetItem = nullptr;
-
- ARMARX_CHECK_EQUAL(rowCount(), numRows - 1);
}
@@ -158,20 +152,22 @@ namespace armarx::nav::locgrapheditor
}
- QString VertexTableWidget::_nameOf(QTableWidgetItem* item)
+ QString
+ VertexTableWidget::_nameOf(QTableWidgetItem* item)
{
return item->data(Qt::UserRole).toString();
}
- void VertexTableWidget::makeContextMenu(QPoint pos)
+ void
+ VertexTableWidget::makeContextMenu(QPoint pos)
{
QList<QTableWidgetItem*> items = selectedVertexItems();
QMenu menu;
if (items.size() == 0)
{
- QAction* action = menu.addAction("No locations selected");
+ QAction* action = menu.addSection("No locations selected");
action->setEnabled(false);
}
@@ -229,7 +225,7 @@ namespace armarx::nav::locgrapheditor
using ListOfEdges = QList<QPair<Item*, Item*>>;
using AppendFunc = std::function<void(ListOfEdges& edges, Item* selected, Item* action)>;
- auto addBulkActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
+ auto addBulkAddEdgeActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
{
if (items.size() == rowCount())
{
@@ -242,8 +238,8 @@ namespace armarx::nav::locgrapheditor
if (items.count(action) == 0) // Do no generate self-edges
{
QAction* a = submenu->addAction(_nameOf(action));
- connect(a, &QAction::triggered,
- [this, &items, action, appendFunc]()
+ connect(a, &QAction::triggered, this,
+ [this, items, action, appendFunc]()
{
QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges;
for (auto* selected : items)
@@ -256,24 +252,52 @@ namespace armarx::nav::locgrapheditor
}
};
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
+ addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
});
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
+ addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({action, selected});
});
- addBulkActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
+ addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
[](ListOfEdges& edges, Item* selected, Item* action)
{
edges.append({selected, action});
edges.append({action, selected});
});
+
+
+ auto connectBulkRemoveEdgeAction = [this, &items](
+ QAction* action, utils::EdgeDirection edgeDirection)
+ {
+ connect(action, &QAction::triggered, this, [this, items, edgeDirection]()
+ {
+ emit edgeRemovalRequested(items, edgeDirection);
+ });
+ };
+
+ connectBulkRemoveEdgeAction(
+ menu.addAction("Remove all edges " + utils::arrowRight + " " + desc),
+ utils::EdgeDirection::To);
+ connectBulkRemoveEdgeAction(
+ menu.addAction("Remove all edges " + utils::arrowLeft + " " + desc),
+ utils::EdgeDirection::From);
+ connectBulkRemoveEdgeAction(
+ menu.addAction("Remove all edges " + utils::arrowBoth + " " + desc),
+ utils::EdgeDirection::Bidirectional);
}
+
+ menu.addSection("Manage Locations");
+ connect(menu.addAction("Create new location ..."), &QAction::triggered, this,
+ [this]()
+ {
+ emit newVertexRequested();
+ });
+
menu.exec(mapToGlobal(pos));
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index e782867f..9224c249 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -30,9 +30,12 @@
#include <QTableWidget>
+namespace armarx::nav::locgrapheditor::utils
+{
+ enum class EdgeDirection;
+}
namespace armarx::nav::locgrapheditor
{
-
class VertexTableWidget : public QTableWidget
{
Q_OBJECT
@@ -44,7 +47,7 @@ namespace armarx::nav::locgrapheditor
VertexTableWidget();
- QTableWidgetItem* addVertex(graph::Graph::ConstVertex vertex);
+ QTableWidgetItem* addVertex();
void updateVertex(GuiGraph::Vertex vertex);
@@ -56,7 +59,10 @@ namespace armarx::nav::locgrapheditor
signals:
+ void newVertexRequested();
+
void newEdgesRequested(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges);
+ void edgeRemovalRequested(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
public slots:
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
index 1a101fff..80b75d16 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -37,6 +37,7 @@ namespace armarx::nav::locgrapheditor
const QString utils::arrowBoth = QStringLiteral("\u21C4");
+
QList<QTableWidgetItem*>
utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
index ad866a66..bd1358b3 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -22,6 +22,7 @@
#pragma once
#include <QList>
+#include <QString>
class QTableWidget;
class QTableWidgetItem;
@@ -30,9 +31,6 @@ class QTableWidgetItem;
namespace armarx::nav::locgrapheditor::utils
{
- QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
-
-
/// <-
extern const QString arrowLeft;
/// ->
@@ -41,4 +39,12 @@ namespace armarx::nav::locgrapheditor::utils
extern const QString arrowBoth;
+ enum class EdgeDirection
+ {
+ From, To, Bidirectional
+ };
+
+
+ QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
+
}
--
GitLab
From 5b5f9f6bc1a57efefeb9d568361865d66c236653 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 25 Aug 2021 16:47:52 +0200
Subject: [PATCH 23/33] Add icon
---
.../LocationGraphEditor/CMakeLists.txt | 6 +-
.../gui-plugins/LocationGraphEditor/icons.qrc | 5 +
.../LocationGraphEditor/icons/graph_visu.svg | 187 ++++++++++++++++++
3 files changed, 196 insertions(+), 2 deletions(-)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index e345f39d..593f461c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -50,7 +50,9 @@ set(HEADERS
set(GUI_UIS
LocationGraphEditorWidget.ui
)
-
+set(GUI_RCS
+ icons.qrc
+)
# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
set(COMPONENT_LIBS
@@ -67,7 +69,7 @@ set(COMPONENT_LIBS
if(ArmarXGui_FOUND)
- armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "" "${COMPONENT_LIBS}")
+ armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "${GUI_RCS}" "${COMPONENT_LIBS}")
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
new file mode 100644
index 00000000..97d27064
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
@@ -0,0 +1,5 @@
+<RCC>
+ <qresource prefix="/">
+ <file>icons/graph_visu.svg</file>
+ </qresource>
+</RCC>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg
new file mode 100644
index 00000000..6f27578f
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg
@@ -0,0 +1,187 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+ xmlns:dc="http://purl.org/dc/elements/1.1/"
+ xmlns:cc="http://creativecommons.org/ns#"
+ xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+ xmlns:svg="http://www.w3.org/2000/svg"
+ xmlns="http://www.w3.org/2000/svg"
+ xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+ xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+ enable-background="new 0 0 512 512"
+ height="512px"
+ id="Layer_1"
+ version="1.1"
+ viewBox="0 0 512 512"
+ width="512px"
+ xml:space="preserve"
+ inkscape:version="0.48.4 r9939"
+ sodipodi:docname="graph_visu.svg"><metadata
+ id="metadata9"><rdf:RDF><cc:Work
+ rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
+ rdf:resource="http://purl.org/dc/dcmitype/StillImage" /><dc:title></dc:title></cc:Work></rdf:RDF></metadata><defs
+ id="defs7" /><sodipodi:namedview
+ pagecolor="#ffffff"
+ bordercolor="#666666"
+ borderopacity="1"
+ objecttolerance="10"
+ gridtolerance="10"
+ guidetolerance="10"
+ inkscape:pageopacity="0"
+ inkscape:pageshadow="2"
+ inkscape:window-width="1920"
+ inkscape:window-height="1028"
+ id="namedview5"
+ showgrid="false"
+ inkscape:zoom="1.3037281"
+ inkscape:cx="91.880082"
+ inkscape:cy="258.60516"
+ inkscape:window-x="1280"
+ inkscape:window-y="24"
+ inkscape:window-maximized="1"
+ inkscape:current-layer="Layer_1" /><path
+ d="M480,0H32C14.328,0,0,14.312,0,32v352c0,17.688,14.328,32,32,32h192v32h-96c-17.672,0-32,14.312-32,32v32h320v-32 c0-17.688-14.328-32-32-32h-96v-32h192c17.672,0,32-14.312,32-32V32C512,14.312,497.672,0,480,0z M448,352H64V64h384V352z"
+ id="path3" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,-85.183547,-107.15758)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-3"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,-37.951856,-198.87707)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-6"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,11.90517,-61.578511)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-7"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,-129.99558,-14.022576)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-5"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,-134.59777,-179.7013)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-35"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,106.24999,-80.754285)" /><path
+ sodipodi:type="arc"
+ style="fill:#000000;fill-rule:evenodd;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
+ id="path3025-62"
+ sodipodi:cx="129.62825"
+ sodipodi:cy="159.54921"
+ sodipodi:rx="9.9714041"
+ sodipodi:ry="9.5878887"
+ d="m 139.59965,159.54921 a 9.9714041,9.5878887 0 1 1 -19.9428,0 9.9714041,9.5878887 0 1 1 19.9428,0 z"
+ transform="matrix(1.9081592,0,0,1.9844855,108.55109,-189.6727)" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="M 151.45515,193.73856 123.46621,152.64875"
+ id="path3093"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-5"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 125.60728,285.45098 28.30899,-58.83593"
+ id="path3095"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-7"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 215.89558,135.63528 36.86483,101.52004"
+ id="path3097"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-3"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-6"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="M 200.68669,134.66544 170.88058,192.5461"
+ id="path3099"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-3"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 334.9519,239.65934 -57.04596,11.59471"
+ id="path3101"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-35"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-6"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 344.44196,142.14007 -73.72498,97.71484"
+ id="path3103"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-62"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-6"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 228.38971,118.93913 108.52249,6.81816"
+ id="path3105"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-3"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-62"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 355.50054,145.97326 -1.49732,70.87268"
+ id="path3107"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-62"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-35"
+ inkscape:connection-end-point="d4" /><path
+ style="fill:none;stroke:#000000;stroke-width:11;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1;stroke-miterlimit:4;stroke-dasharray:none"
+ d="m 135.4015,296.55275 105.80926,-35.4604"
+ id="path3109"
+ inkscape:connector-type="polyline"
+ inkscape:connector-curvature="3"
+ inkscape:connection-start="#path3025-7"
+ inkscape:connection-start-point="d4"
+ inkscape:connection-end="#path3025-6"
+ inkscape:connection-end-point="d4" /></svg>
\ No newline at end of file
--
GitLab
From def3d91fc0ec4892b2da378e2714df6f000ec74b Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Wed, 25 Aug 2021 16:48:28 +0200
Subject: [PATCH 24/33] Fix / improve Graph creation and visu for different
graphs
---
.../LocationGraphEditorWidgetController.cpp | 126 +++++++++++++-----
.../LocationGraphEditorWidgetController.h | 6 +-
.../gui-plugins/LocationGraphEditor/Visu.cpp | 13 +-
.../gui-plugins/LocationGraphEditor/Visu.h | 14 ++
4 files changed, 127 insertions(+), 32 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index abe9388f..cbd70025 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -40,6 +40,7 @@
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -355,19 +356,19 @@ namespace armarx::nav::locgrapheditor
// Resolve locations and remove vertices which could not be resolved.
{
resolveLocations(nav, model.locationsMemory);
- std::vector<nav::graph::Graph::Vertex> remove;
+ std::vector<semrel::ShapeID> remove;
for (nav::graph::Graph::Vertex vertex : nav.vertices())
{
if (not vertex.attrib().hasPose())
{
- remove.push_back(vertex);
+ remove.push_back(vertex.objectID());
}
}
if (not remove.empty())
{
- for (nav::graph::Graph::Vertex vertex : remove)
+ for (semrel::ShapeID vertexID : remove)
{
- nav.removeVertex(vertex.objectID());
+ nav.removeVertex(nav.vertex(vertexID));
}
std::stringstream ss;
ss << "Dropped " << remove.size() << " locations which could not be resolved.";
@@ -375,9 +376,8 @@ namespace armarx::nav::locgrapheditor
}
}
- model.graphEntityID = entityID;
ARMARX_VERBOSE << "Loading graph " << nav.str();
- setGraph(nav);
+ setGraph(entityID, nav);
}
@@ -503,7 +503,6 @@ namespace armarx::nav::locgrapheditor
++it;
}
}
-
return result;
}
@@ -522,12 +521,32 @@ namespace armarx::nav::locgrapheditor
update.timeCreated = armem::Time::now();
update.instancesData = {dto.toAron()};
- return remote.graphWriter.commit(update);
+ armem::EntityUpdateResult result = remote.graphWriter.commit(update);
+ if (result.success)
+ {
+ // Clear dirty flag
+ model.graph.attrib().edgesChanged = false;
+ }
+ return result;
}
- void LocationGraphEditorWidgetController::setGraph(const graph::Graph& nav)
+ template <class GraphT>
+ semrel::ShapeID findNextFreeVertexID(const GraphT& graph, semrel::ShapeID vertexID)
{
+ while (graph.hasVertex(vertexID))
+ {
+ ++vertexID;
+ }
+ return vertexID;
+ }
+
+ void LocationGraphEditorWidgetController::setGraph(
+ const armem::MemoryID& entityID,
+ const graph::Graph& nav)
+ {
+ model.graphEntityID = entityID;
+
// Build the gui graph (model).
{
QSignalBlocker blocker(this);
@@ -535,11 +554,29 @@ namespace armarx::nav::locgrapheditor
clearGraph();
model.graph.attrib().edgesChanged = false;
+ std::set<armem::MemoryID> coveredLocationIDs;
for (auto vertex : nav.vertices())
{
ARMARX_CHECK(vertex.attrib().hasPose());
addVertex(vertex.objectID(), { vertex.attrib() });
+
+ coveredLocationIDs.insert(aron::fromAron<armem::MemoryID>(vertex.attrib().aron.locationID));
+ }
+ // Add locations which have not been part of graph.
+ // ToDo: This should be an explicit step in the GUI.
+ {
+ semrel::ShapeID vertexID { 0 };
+ model.locationsMemory.forEachInstance([&](const armem::wm::EntityInstance& instance)
+ {
+ if (coveredLocationIDs.count(instance.id().getEntityID()) == 0)
+ {
+ vertexID = findNextFreeVertexID(model.graph, vertexID);
+ addVertex(vertexID, instance);
+ }
+ });
}
+
+
for (auto edge : nav.edges())
{
addEdge(model.graph.vertex(edge.sourceObjectID()),
@@ -553,6 +590,31 @@ namespace armarx::nav::locgrapheditor
}
+ void LocationGraphEditorWidgetController::setEmptyGraph(
+ const armem::MemoryID& entityID)
+ {
+ model.graphEntityID = entityID;
+
+ {
+ QSignalBlocker blocker(this);
+ clearGraph();
+
+ semrel::ShapeID id { 0 };
+ queryLocations();
+ model.locationsMemory.forEachInstance([this, &id](const armem::wm::EntityInstance& instance)
+ {
+ addVertex(id, instance);
+ ++id;
+ });
+ }
+
+ // Mark graph as changed.
+ model.graph.attrib().edgesChanged = true;
+
+ emit graphChanged();
+ }
+
+
GuiGraph::Vertex LocationGraphEditorWidgetController::addVertex(
semrel::ShapeID vertexID,
const VertexData& defaultAttribs)
@@ -569,6 +631,21 @@ namespace armarx::nav::locgrapheditor
}
+ GuiGraph::Vertex
+ LocationGraphEditorWidgetController::addVertex(
+ semrel::ShapeID vertexID,
+ const armem::wm::EntityInstance& locationInstance)
+ {
+ nav::loc::arondto::Location location;
+ location.fromAron(locationInstance.data());
+
+ VertexData attrib;
+ toAron(attrib.aron.locationID, locationInstance.id().getEntityID());
+ attrib.setPose(location.globalRobotPose);
+ return addVertex(vertexID, attrib);
+ }
+
+
GuiGraph::Edge
LocationGraphEditorWidgetController::addEdge(
GuiGraph::ConstVertex source,
@@ -657,10 +734,18 @@ namespace armarx::nav::locgrapheditor
{
if (remote.arviz)
{
+#if 0
const QString layerName = getGraphDisplayName(model.graphEntityID, false);
viz::Layer layer = remote.arviz->layer(layerName.toStdString());
model.visu.draw(layer, model.graph);
remote.arviz->commit(layer);
+#else
+ viz::Layer vertices = remote.arviz->layer("Vertices");
+ viz::Layer edges = remote.arviz->layer("Edges");
+ applyVisu(vertices, model.visu.vertex, model.graph.vertices());
+ applyVisu(edges, model.visu.edge, model.graph.edges());
+ remote.arviz->commit({vertices, edges});
+#endif
}
}
@@ -839,11 +924,7 @@ namespace armarx::nav::locgrapheditor
}
// Find free vertex ID
- semrel::ShapeID vertexID { 0 };
- while (model.graph.hasVertex(vertexID))
- {
- ++vertexID;
- }
+ const semrel::ShapeID vertexID = findNextFreeVertexID(model.graph, semrel::ShapeID{ 0 });
// Initiaize attributes
VertexData attribs;
@@ -949,13 +1030,6 @@ namespace armarx::nav::locgrapheditor
return;
}
- // Find free vertex ID
- semrel::ShapeID vertexID { 0 };
- while (model.graph.hasVertex(vertexID))
- {
- ++vertexID;
- }
-
const armem::MemoryID entityID = dialog.entityID();
const bool hasChanged = true;
@@ -964,15 +1038,7 @@ namespace armarx::nav::locgrapheditor
widget.graphsComboBox->addItem(text, data);
widget.graphsComboBox->setCurrentIndex(widget.graphsComboBox->count() - 1);
- {
- QSignalBlocker blocker(this);
- clearEdges();
- // clearVertices();
- }
- model.graph.attrib().edgesChanged = true;
- model.graphEntityID = entityID;
-
- emit graphChanged();
+ setEmptyGraph(entityID);
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 32ae0ccd..3a3e9e3e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -26,6 +26,7 @@
#include "widgets/graph_scene.h"
#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
@@ -182,10 +183,13 @@ namespace armarx::nav::locgrapheditor
private:
- void setGraph(const graph::Graph& nav);
+ void setGraph(const armem::MemoryID& entityID, const graph::Graph& nav);
+ void setEmptyGraph(const armem::MemoryID& entityID);
GuiGraph::Vertex
addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs);
+ GuiGraph::Vertex
+ addVertex(semrel::ShapeID vertexID, const armem::wm::EntityInstance& locationInstance);
GuiGraph::Edge
addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target, const EdgeData& defaultAttribs);
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
index dfe8587a..5476690f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -58,6 +58,12 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexVisu::draw(viz::Layer& layer, GuiGraph::ConstVertex vertex) const
+ {
+ draw(layer, vertex.attrib());
+ }
+
+
void VertexVisu::draw(viz::Layer& layer, const VertexData& attribs) const
{
if (pose.has_value())
@@ -103,11 +109,15 @@ namespace armarx::nav::locgrapheditor
void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph)
{
+#if 1
+ applyVisu(layer, vertex, graph.vertices());
+ applyVisu(layer, edge, graph.edges());
+#else
if (vertex.has_value())
{
for (GuiGraph::ConstVertex v : graph.vertices())
{
- vertex->draw(layer, v.attrib());
+ vertex->draw(layer, v);
}
}
if (edge.has_value())
@@ -117,6 +127,7 @@ namespace armarx::nav::locgrapheditor
edge->draw(layer, e);
}
}
+#endif
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
index 5e77c8be..9cc9ba2e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -56,6 +56,7 @@ namespace armarx::nav::locgrapheditor
std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+ void draw(viz::Layer& layer, GuiGraph::ConstVertex vertex) const;
void draw(viz::Layer& layer, const VertexData& attribs) const;
};
@@ -85,4 +86,17 @@ namespace armarx::nav::locgrapheditor
void draw(viz::Layer& layer, const GuiGraph& graph);
};
+
+ template <class VisuT, class RangeT>
+ void applyVisu(viz::Layer& layer, const std::optional<VisuT>& visu, RangeT&& range)
+ {
+ if (visu.has_value())
+ {
+ for (auto element : range)
+ {
+ visu->draw(layer, element);
+ }
+ }
+ }
+
}
--
GitLab
From a6198b3e8b85c8d50d2d5dc6e0bc77059258f5f5 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 26 Aug 2021 11:18:59 +0200
Subject: [PATCH 25/33] Add robot visu, fix icon
---
source/armarx/navigation/graph/Graph.cpp | 34 ++-
source/armarx/navigation/graph/Graph.h | 5 +-
source/armarx/navigation/graph/Visu.h | 2 +-
.../LocationGraphEditor/CMakeLists.txt | 4 +
.../LocationGraphEditorWidget.ui | 15 +-
.../LocationGraphEditorWidgetController.cpp | 49 +++--
.../LocationGraphEditorWidgetController.h | 3 +
.../gui-plugins/LocationGraphEditor/icons.qrc | 2 +-
...aph_visu.svg => location_graph_editor.svg} | 0
.../widgets/ConnectDialog.cpp | 28 +++
.../widgets/ConnectDialog.h | 88 ++++++++
.../widgets/RobotVisuWidget.cpp | 208 ++++++++++++++++++
.../widgets/RobotVisuWidget.h | 164 ++++++++++++++
13 files changed, 567 insertions(+), 35 deletions(-)
rename source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/{graph_visu.svg => location_graph_editor.svg} (100%)
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
create mode 100644 source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index f34cb169..04a41807 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -13,7 +13,6 @@
* 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
@@ -100,21 +99,32 @@ namespace armarx::nav
}
- void graph::resolveLocations(Graph& bo, armem::wm::Memory& locationMemory)
+ void graph::resolveLocations(Graph& bo, const armem::wm::Memory& locationMemory)
{
for (graph::Graph::Vertex vertex : bo.vertices())
{
- armem::MemoryID locationID;
- fromAron(vertex.attrib().aron.locationID, locationID);
-
- armem::wm::EntityInstance* instance = locationMemory.findLatestInstance(locationID);
- if (instance)
- {
- nav::loc::arondto::Location dto;
- dto.fromAron(instance->data());
- vertex.attrib().setPose(dto.globalRobotPose);
- }
+ resolveLocation(vertex, locationMemory);
}
}
+
+ bool graph::resolveLocation(Graph::Vertex& vertex, const armem::wm::Memory& locationMemory)
+ {
+ armem::MemoryID locationID;
+ fromAron(vertex.attrib().aron.locationID, locationID);
+
+ if (const armem::wm::EntityInstance* instance = locationMemory.findLatestInstance(locationID))
+ {
+ nav::loc::arondto::Location dto;
+ dto.fromAron(instance->data());
+ vertex.attrib().setPose(dto.globalRobotPose);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index aa216d69..731264d6 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -13,7 +13,6 @@
* 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
@@ -62,6 +61,8 @@ namespace armarx::nav::graph
void toAron(arondto::Graph& dto, const Graph& bo);
void fromAron(const arondto::Graph& dto, Graph& bo);
- void resolveLocations(Graph& bo, armem::wm::Memory& locationMemory);
+
+ void resolveLocations(Graph& bo, const armem::wm::Memory& locationMemory);
+ bool resolveLocation(Graph::Vertex& vertex, const armem::wm::Memory& locationMemory);
}
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
index f38719a9..dffc4b47 100644
--- a/source/armarx/navigation/graph/Visu.h
+++ b/source/armarx/navigation/graph/Visu.h
@@ -51,7 +51,7 @@ namespace armarx::nav::graph
{
float width = 7.5;
float length = 100.0;
- simox::Color color = simox::Color::cyan(220);
+ simox::Color color = simox::Color::green();
viz::Arrow draw(const VertexAttribs& attribs) const;
};
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
index 593f461c..0c9d633d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -19,8 +19,10 @@ set(SOURCES
widgets/default_colors.cpp
widgets/utils.cpp
+ widgets/ConnectDialog.cpp
widgets/EdgeTableWidget.cpp
widgets/NewEntityIdDialog.cpp
+ widgets/RobotVisuWidget.cpp
widgets/VertexDataWidget.cpp
widgets/VertexTableWidget.cpp
@@ -37,8 +39,10 @@ set(HEADERS
widgets/default_colors.h
widgets/utils.h
+ widgets/ConnectDialog.h
widgets/EdgeTableWidget.h
widgets/NewEntityIdDialog.h
+ widgets/RobotVisuWidget.h
widgets/VertexDataWidget.h
widgets/VertexTableWidget.h
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 88fa0f4c..759de91a 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -125,7 +125,20 @@
<property name="title">
<string>Robot Visu</string>
</property>
- <layout class="QVBoxLayout" name="verticalLayout_5"/>
+ <layout class="QVBoxLayout" name="verticalLayout_5">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ </layout>
</widget>
</item>
</layout>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index cbd70025..a79b210a 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -26,6 +26,7 @@
#include "widgets/utils.h"
#include "widgets/EdgeTableWidget.h"
#include "widgets/NewEntityIdDialog.h"
+#include "widgets/RobotVisuWidget.h"
#include "widgets/VertexDataWidget.h"
#include "widgets/VertexTableWidget.h"
#include "widgets/graph_scene/Scene.h"
@@ -74,7 +75,7 @@ namespace armarx::nav::locgrapheditor
}
QIcon LocationGraphEditorWidgetController::GetWidgetIcon()
{
- return QIcon {":// icons/graph_visu.svg"};
+ return QIcon {"://icons/location_graph_editor.svg"};
}
@@ -89,23 +90,27 @@ namespace armarx::nav::locgrapheditor
view.vertexData = new VertexDataWidget();
view.vertexData->setEnabled(false); // Enable on first selection of vertex.
- view.vertexTable = new VertexTableWidget();
- view.edgeTable = new EdgeTableWidget();
-
widget.locationDataGroupBox->layout()->addWidget(view.vertexData);
if (QBoxLayout* layout = dynamic_cast<QBoxLayout*>(widget.locationDataGroupBox->layout()))
{
layout->addStretch();
}
+ view.vertexTable = new VertexTableWidget();
widget.locationsTableGroupBox->layout()->addWidget(view.vertexTable);
+
+ view.edgeTable = new EdgeTableWidget();
widget.edgesTableGroupBox->layout()->addWidget(view.edgeTable);
+ view.robotVisu = new RobotVisuWidget(*this);
+ widget.robotVisuGroupBox->layout()->addWidget(view.robotVisu);
+
view.graph = new GraphSceneWidget();
widget.graphSceneLayout->addWidget(view.graph);
connect(this, &This::connected, this, &This::queryGraphs);
+ connect(this, &This::connected, this, &This::graphChanged);
connect(widget.createGraphButton, &QPushButton::pressed,
this, &This::createGraphDialog);
@@ -126,12 +131,15 @@ namespace armarx::nav::locgrapheditor
connect(widget.commitGraphButton, &QPushButton::pressed,
this, &This::commit);
-
// View updates
connect(this, &This::graphChanged,
this, &This::updateGraphView);
connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
this, &This::updateGraphView);
+ connect(view.robotVisu, &RobotVisuWidget::connected,
+ this, &This::updateGraphView);
+ connect(view.robotVisu, &RobotVisuWidget::settingsChanged,
+ this, &This::updateGraphView);
// Selection
connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
@@ -147,7 +155,6 @@ namespace armarx::nav::locgrapheditor
this, &This::updateEdgeHighlighting);
// Graph modification
-
connect(view.vertexTable, &VertexTableWidget::newVertexRequested,
this, &This::createVertexDialog);
connect(view.vertexTable, &VertexTableWidget::newEdgesRequested,
@@ -734,18 +741,24 @@ namespace armarx::nav::locgrapheditor
{
if (remote.arviz)
{
-#if 0
- const QString layerName = getGraphDisplayName(model.graphEntityID, false);
- viz::Layer layer = remote.arviz->layer(layerName.toStdString());
- model.visu.draw(layer, model.graph);
- remote.arviz->commit(layer);
-#else
- viz::Layer vertices = remote.arviz->layer("Vertices");
- viz::Layer edges = remote.arviz->layer("Edges");
- applyVisu(vertices, model.visu.vertex, model.graph.vertices());
- applyVisu(edges, model.visu.edge, model.graph.edges());
- remote.arviz->commit({vertices, edges});
-#endif
+ std::vector<viz::Layer> layers;
+ {
+ viz::Layer& vertices = layers.emplace_back(remote.arviz->layer("Vertices"));
+ applyVisu(vertices, model.visu.vertex, model.graph.vertices());
+ }
+ {
+ viz::Layer& edges = layers.emplace_back(remote.arviz->layer("Edges"));
+ applyVisu(edges, model.visu.edge, model.graph.edges());
+ }
+ {
+ viz::Layer& robot = layers.emplace_back(remote.arviz->layer("Robot"));
+ if (view.vertexData->vertex().has_value() and view.robotVisu->isEnabled())
+ {
+ robot.add(view.robotVisu->connection().vizRobot("robot")
+ .pose(view.vertexData->vertex()->attrib().getPose()));
+ }
+ }
+ remote.arviz->commit(layers);
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 3a3e9e3e..3746eea6 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -60,6 +60,7 @@ namespace armarx::nav::locgrapheditor::utils
namespace armarx::nav::locgrapheditor
{
class EdgeTableWidget;
+ class RobotVisuWidget;
class VertexDataWidget;
class VertexTableWidget;
@@ -250,6 +251,8 @@ namespace armarx::nav::locgrapheditor
VertexDataWidget* vertexData = nullptr;
GraphSceneWidget* graph = nullptr;
+
+ RobotVisuWidget* robotVisu = nullptr;
};
View view;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
index 97d27064..14569cdf 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
@@ -1,5 +1,5 @@
<RCC>
<qresource prefix="/">
- <file>icons/graph_visu.svg</file>
+ <file>icons/location_graph_editor.svg</file>
</qresource>
</RCC>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/location_graph_editor.svg
similarity index 100%
rename from source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/graph_visu.svg
rename to source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/location_graph_editor.svg
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
new file mode 100644
index 00000000..9ced9d91
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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/>.
+ *
+ * @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 "ConnectDialog.h"
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
new file mode 100644
index 00000000..14157a33
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
@@ -0,0 +1,88 @@
+/*
+ * 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/>.
+ *
+ * @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 <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <QDialog>
+#include <QDialogButtonBox>
+
+
+namespace armarx::nav::locgrapheditor
+{
+ template <class ProxyT>
+ class ConnectDialog : public QDialog
+ {
+ public:
+
+ ConnectDialog(
+ QString searchMask,
+ ManagedIceObject& component,
+ QWidget* parent = nullptr) :
+ QDialog(parent),
+ component(component)
+ {
+ finder = new armarx::IceProxyFinder<ProxyT>();
+ finder->setIceManager(component.getIceManager());
+ finder->setSearchMask(searchMask);
+
+ QDialogButtonBox* buttonBox = new QDialogButtonBox(
+ QDialogButtonBox::Ok | QDialogButtonBox::Cancel, this);
+
+ setLayout(new QVBoxLayout);
+ layout()->addWidget(finder);
+ layout()->addWidget(buttonBox);
+
+ connect(buttonBox, &QDialogButtonBox::accepted, this, &QDialog::accept);
+ connect(buttonBox, &QDialogButtonBox::rejected, this, &QDialog::reject);
+ }
+
+ virtual ~ConnectDialog() override
+ {
+ }
+
+
+ ProxyT getProxy()
+ {
+ QString name = finder->getSelectedProxyName();
+ if (name.size() > 0)
+ {
+ return component.getProxy<ProxyT>(name.toStdString());
+ }
+ else
+ {
+ return nullptr;
+ }
+ }
+
+
+ public:
+
+ ManagedIceObject& component;
+ armarx::IceProxyFinder<ProxyT>* finder = nullptr;
+
+ };
+
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
new file mode 100644
index 00000000..1b7b079d
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
@@ -0,0 +1,208 @@
+/*
+ * 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/>.
+ *
+ * @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 "RobotVisuWidget.h"
+#include "ConnectDialog.h"
+
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <VirtualRobot/Robot.h>
+
+#include <QCheckBox>
+#include <QDialog>
+#include <QDialogButtonBox>
+#include <QPushButton>
+#include <QVBoxLayout>
+
+
+namespace armarx::nav::locgrapheditor
+{
+
+ robotvisu::SettingsWidget::SettingsWidget(QWidget* parent) :
+ QWidget(parent)
+ {
+ _enabled = new QCheckBox("Enabled", this);
+ _enabled->setChecked(true);
+
+ _collisionModel = new QCheckBox("Collision Model", this);
+ _collisionModel->setChecked(false);
+
+
+ setLayout(new QVBoxLayout());
+ layout()->addWidget(_enabled);
+ layout()->addWidget(_collisionModel);
+
+
+ connect(_enabled, &QCheckBox::toggled, this, &This::changed);
+ connect(_collisionModel, &QCheckBox::toggled, this, &This::changed);
+ }
+
+
+ bool robotvisu::SettingsWidget::isEnabled() const
+ {
+ return _enabled->isChecked();
+ }
+
+
+ bool robotvisu::SettingsWidget::useCollisionModel() const
+ {
+ return _collisionModel->isChecked();
+ }
+
+
+ robotvisu::Connection::Connection(
+ RobotStateComponentInterfacePrx robotStateComponent,
+ robotvisu::SettingsWidget* settings) :
+ robotStateComponent(robotStateComponent),
+ _settings(settings)
+ {
+ ARMARX_CHECK_NOT_NULL(robotStateComponent);
+ filename = robotStateComponent->getRobotFilename();
+ robot = RemoteRobot::createLocalClone(
+ robotStateComponent, "", {}, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ ARMARX_CHECK_NOT_NULL(robot);
+ }
+
+
+ Eigen::Matrix4f robotvisu::Connection::getGlobalPose(bool synchronize)
+ {
+ _synchronize(synchronize);
+ return robot->getGlobalPose();
+ }
+
+
+ std::map<std::string, float>
+ robotvisu::Connection::getJointValues(bool synchronize)
+ {
+ _synchronize(synchronize);
+ return robot->getConfig()->getRobotNodeJointValueMap();
+ }
+
+
+ viz::Robot robotvisu::Connection::vizRobot(
+ const std::string& id,
+ bool synchronize)
+ {
+ _synchronize(synchronize);
+ viz::Robot robot = viz::Robot(id)
+ .file("", filename)
+ .pose(getGlobalPose(false))
+ .joints(getJointValues(false));
+
+ if (_settings and _settings->useCollisionModel())
+ {
+ robot.useCollisionModel();
+ }
+ return robot;
+ }
+
+
+ void robotvisu::Connection::_synchronize(bool synchronize)
+ {
+ if (synchronize)
+ {
+ RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
+ }
+ }
+
+
+
+ RobotVisuWidget::RobotVisuWidget(ManagedIceObject& component, QWidget* parent) :
+ QWidget(parent),
+ _component(component)
+ {
+ _statusLabel = new QLabel("Not connected", this);
+ _connectButton = new QPushButton("Connect ...", this);
+
+ _settings = new robotvisu::SettingsWidget();
+ _settings->setEnabled(false);
+
+
+ QBoxLayout* layout = new QVBoxLayout(this);
+ setLayout(layout);
+
+ layout->addWidget(_statusLabel);
+ layout->addWidget(_connectButton);
+ layout->addWidget(_settings);
+
+
+ connect(_connectButton, &QPushButton::pressed,
+ this, &This::connectToRobot);
+
+ connect(_settings, &robotvisu::SettingsWidget::changed,
+ this, &This::settingsChanged);
+
+ connect(this, &This::connected, this, [this]()
+ {
+ const std::string name = connection().robotStateComponent->ice_getIdentity().name;
+ _statusLabel->setText("Connected to '" + QString::fromStdString(name) + "'");
+ });
+ connect(this, &This::connected, this, [this]()
+ {
+ _settings->setEnabled(true);
+ });
+ }
+
+
+ bool RobotVisuWidget::isEnabled() const
+ {
+ return isConnected() and _settings->isEnabled();
+ }
+
+
+ bool RobotVisuWidget::isConnected() const
+ {
+ return _connection.has_value();
+ }
+
+
+ robotvisu::Connection& RobotVisuWidget::connection()
+ {
+ return _connection.value();
+ }
+
+
+ void RobotVisuWidget::connectToRobot()
+ {
+ ConnectDialog<RobotStateComponentInterfacePrx> dialog("*RobotStateComponent", _component, this);
+ switch (dialog.exec())
+ {
+ case QDialog::Accepted:
+ break;
+ case QDialog::Rejected:
+ return;
+ }
+
+ auto robotStateComponent = dialog.getProxy();
+ if (robotStateComponent)
+ {
+ _connection = robotvisu::Connection(robotStateComponent, _settings);
+ emit connected();
+ }
+ }
+
+}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
new file mode 100644
index 00000000..82b7a3ff
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
@@ -0,0 +1,164 @@
+/*
+ * 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/>.
+ *
+ * @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 <RobotAPI/interface/core/RobotState.h>
+
+#include <Ice/ProxyHandle.h>
+
+#include <QWidget>
+
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+
+
+class QCheckBox;
+class QLabel;
+class QPushButton;
+
+
+namespace VirtualRobot
+{
+ using RobotPtr = std::shared_ptr<class Robot>;
+}
+namespace IceProxy::armarx
+{
+ class RobotStateComponentInterface;
+}
+namespace armarx
+{
+ class ManagedIceObject;
+ using RobotStateComponentInterfacePrx = ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface>;
+}
+namespace armarx::viz
+{
+ class Robot;
+}
+
+
+namespace armarx::nav::locgrapheditor::robotvisu
+{
+
+ class SettingsWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = SettingsWidget;
+
+ public:
+
+ SettingsWidget(QWidget* parent = nullptr);
+
+
+ bool isEnabled() const;
+ bool useCollisionModel() const;
+
+
+ signals:
+
+ void changed();
+
+
+ private:
+
+ QCheckBox* _enabled = nullptr;
+ QCheckBox* _collisionModel = nullptr;
+
+ };
+
+
+
+ class Connection
+ {
+ public:
+
+ Connection(RobotStateComponentInterfacePrx robotStateComponent,
+ robotvisu::SettingsWidget* settings);
+
+
+ Eigen::Matrix4f getGlobalPose(bool synchronize = true);
+ std::map<std::string, float> getJointValues(bool synchronize = true);
+
+ viz::Robot vizRobot(const std::string& id, bool synchronize = true);
+
+
+ public:
+
+ RobotStateComponentInterfacePrx robotStateComponent;
+ std::string filename;
+
+ VirtualRobot::RobotPtr robot;
+
+
+ private:
+
+ void _synchronize(bool synchronize);
+ robotvisu::SettingsWidget* _settings = nullptr;
+
+ };
+
+}
+
+namespace armarx::nav::locgrapheditor
+{
+
+ class RobotVisuWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = RobotVisuWidget;
+
+ public:
+
+ RobotVisuWidget(ManagedIceObject& component, QWidget* parent = nullptr);
+
+
+ /// Indicates whether the connection is established and visu is enabled.
+ bool isEnabled() const;
+
+ bool isConnected() const;
+ robotvisu::Connection& connection();
+
+
+ signals:
+
+ void connected();
+ void settingsChanged();
+
+
+ public slots:
+
+ void connectToRobot();
+
+
+ private:
+
+ ManagedIceObject& _component;
+ std::optional<robotvisu::Connection> _connection;
+
+ QLabel* _statusLabel = nullptr;
+ QPushButton* _connectButton = nullptr;
+ robotvisu::SettingsWidget* _settings = nullptr;
+
+ };
+
+}
--
GitLab
From e310936167b29058d46744cfb3668aa3b604c11a Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 26 Aug 2021 13:13:57 +0200
Subject: [PATCH 26/33] Add use current robot pose button
---
.../LocationGraphEditorWidget.ui | 2 +-
.../LocationGraphEditorWidgetController.cpp | 11 +++-
.../widgets/RobotVisuWidget.cpp | 47 +++++++++++-----
.../widgets/RobotVisuWidget.h | 26 +++++----
.../widgets/VertexDataWidget.cpp | 53 +++++++++++++++----
.../widgets/VertexDataWidget.h | 16 ++++++
6 files changed, 119 insertions(+), 36 deletions(-)
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
index 759de91a..db72fa0c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -123,7 +123,7 @@
<item>
<widget class="QGroupBox" name="robotVisuGroupBox">
<property name="title">
- <string>Robot Visu</string>
+ <string>Robot State</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_5">
<property name="leftMargin">
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index a79b210a..ed6c1893 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -138,6 +138,11 @@ namespace armarx::nav::locgrapheditor
this, &This::updateGraphView);
connect(view.robotVisu, &RobotVisuWidget::connected,
this, &This::updateGraphView);
+ connect(view.robotVisu, &RobotVisuWidget::connected,
+ this, [this]()
+ {
+ view.vertexData->setRobotConnection(&view.robotVisu->connection());
+ });
connect(view.robotVisu, &RobotVisuWidget::settingsChanged,
this, &This::updateGraphView);
@@ -434,7 +439,11 @@ namespace armarx::nav::locgrapheditor
std::stringstream ss;
if (locResults.allSuccess())
{
- ss << "Committed " << locResults.results.size() << " location snapshots";
+ ss << "Committed " << locResults.results.size() << " location snapshot";
+ if (locResults.results.size() != 1)
+ {
+ ss << "s"; // plural
+ }
if (graphResult.success)
{
ss << " and 1 graph snapshot " << graphResult.snapshotID;
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
index 1b7b079d..e6699a15 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
@@ -45,10 +45,10 @@ namespace armarx::nav::locgrapheditor
robotvisu::SettingsWidget::SettingsWidget(QWidget* parent) :
QWidget(parent)
{
- _enabled = new QCheckBox("Enabled", this);
+ _enabled = new QCheckBox("Enable Visu", this);
_enabled->setChecked(true);
- _collisionModel = new QCheckBox("Collision Model", this);
+ _collisionModel = new QCheckBox("Use Collision Model", this);
_collisionModel->setChecked(false);
@@ -59,6 +59,9 @@ namespace armarx::nav::locgrapheditor
connect(_enabled, &QCheckBox::toggled, this, &This::changed);
connect(_collisionModel, &QCheckBox::toggled, this, &This::changed);
+
+ const int margin = 0;
+ setContentsMargins(margin, margin, margin, margin);
}
@@ -77,21 +80,26 @@ namespace armarx::nav::locgrapheditor
robotvisu::Connection::Connection(
RobotStateComponentInterfacePrx robotStateComponent,
robotvisu::SettingsWidget* settings) :
- robotStateComponent(robotStateComponent),
+ _robotStateComponent(std::make_unique<RobotStateComponentInterfacePrx>(robotStateComponent)),
_settings(settings)
{
ARMARX_CHECK_NOT_NULL(robotStateComponent);
- filename = robotStateComponent->getRobotFilename();
- robot = RemoteRobot::createLocalClone(
+ _filename = robotStateComponent->getRobotFilename();
+ _robot = RemoteRobot::createLocalClone(
robotStateComponent, "", {}, VirtualRobot::RobotIO::RobotDescription::eStructure);
- ARMARX_CHECK_NOT_NULL(robot);
+ ARMARX_CHECK_NOT_NULL(_robot);
+ }
+
+
+ robotvisu::Connection::~Connection()
+ {
}
Eigen::Matrix4f robotvisu::Connection::getGlobalPose(bool synchronize)
{
_synchronize(synchronize);
- return robot->getGlobalPose();
+ return _robot->getGlobalPose();
}
@@ -99,7 +107,7 @@ namespace armarx::nav::locgrapheditor
robotvisu::Connection::getJointValues(bool synchronize)
{
_synchronize(synchronize);
- return robot->getConfig()->getRobotNodeJointValueMap();
+ return _robot->getConfig()->getRobotNodeJointValueMap();
}
@@ -109,7 +117,7 @@ namespace armarx::nav::locgrapheditor
{
_synchronize(synchronize);
viz::Robot robot = viz::Robot(id)
- .file("", filename)
+ .file("", _filename)
.pose(getGlobalPose(false))
.joints(getJointValues(false));
@@ -121,11 +129,24 @@ namespace armarx::nav::locgrapheditor
}
+ RobotStateComponentInterfacePrx robotvisu::Connection::getRobotStateComponent() const
+ {
+ ARMARX_CHECK_NOT_NULL(_robotStateComponent);
+ return *_robotStateComponent;
+ }
+
+
+ std::string robotvisu::Connection::getConnectedName() const
+ {
+ return getRobotStateComponent()->ice_getIdentity().name;
+ }
+
+
void robotvisu::Connection::_synchronize(bool synchronize)
{
if (synchronize)
{
- RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
+ RemoteRobot::synchronizeLocalClone(_robot, getRobotStateComponent());
}
}
@@ -158,8 +179,8 @@ namespace armarx::nav::locgrapheditor
connect(this, &This::connected, this, [this]()
{
- const std::string name = connection().robotStateComponent->ice_getIdentity().name;
- _statusLabel->setText("Connected to '" + QString::fromStdString(name) + "'");
+ QString name = QString::fromStdString(connection().getConnectedName());
+ _statusLabel->setText("Connected to '" + name + "'");
});
connect(this, &This::connected, this, [this]()
{
@@ -200,7 +221,7 @@ namespace armarx::nav::locgrapheditor
auto robotStateComponent = dialog.getProxy();
if (robotStateComponent)
{
- _connection = robotvisu::Connection(robotStateComponent, _settings);
+ _connection.emplace(robotStateComponent, _settings);
emit connected();
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
index 82b7a3ff..7a926922 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
@@ -21,9 +21,7 @@
#pragma once
-#include <RobotAPI/interface/core/RobotState.h>
-
-#include <Ice/ProxyHandle.h>
+#include <Eigen/Core>
#include <QWidget>
@@ -42,6 +40,10 @@ namespace VirtualRobot
{
using RobotPtr = std::shared_ptr<class Robot>;
}
+namespace IceInternal
+{
+ template<typename T> class ProxyHandle;
+}
namespace IceProxy::armarx
{
class RobotStateComponentInterface;
@@ -94,6 +96,11 @@ namespace armarx::nav::locgrapheditor::robotvisu
Connection(RobotStateComponentInterfacePrx robotStateComponent,
robotvisu::SettingsWidget* settings);
+ ~Connection();
+
+
+ RobotStateComponentInterfacePrx getRobotStateComponent() const;
+ std::string getConnectedName() const;
Eigen::Matrix4f getGlobalPose(bool synchronize = true);
@@ -102,19 +109,16 @@ namespace armarx::nav::locgrapheditor::robotvisu
viz::Robot vizRobot(const std::string& id, bool synchronize = true);
- public:
-
- RobotStateComponentInterfacePrx robotStateComponent;
- std::string filename;
-
- VirtualRobot::RobotPtr robot;
-
-
private:
void _synchronize(bool synchronize);
+
+ std::unique_ptr<RobotStateComponentInterfacePrx> _robotStateComponent;
robotvisu::SettingsWidget* _settings = nullptr;
+ std::string _filename;
+ VirtualRobot::RobotPtr _robot;
+
};
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index 2aceb2bf..d228ec6c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -21,6 +21,7 @@
*/
#include "VertexDataWidget.h"
+#include "RobotVisuWidget.h"
#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
@@ -31,6 +32,7 @@
#include <QHBoxLayout>
#include <QLineEdit>
#include <QList>
+#include <QPushButton>
#include <QRadioButton>
#include <QSignalBlocker>
@@ -66,6 +68,10 @@ namespace armarx::nav::locgrapheditor
angleUnitDeg = new QRadioButton("Degrees");
angleUnitRad = new QRadioButton("Radians");
+ _useCurrentRobotPose = new QPushButton("Use Current Robot Pose");
+ _useCurrentRobotPose->setEnabled(false);
+ _useCurrentRobotPose->setToolTip("Use the robot's current global pose (requires connection to robot state).");
+
for (QDoubleSpinBox* pos : _positionSpinBoxes())
{
@@ -80,6 +86,12 @@ namespace armarx::nav::locgrapheditor
spinBox->setAlignment(Qt::AlignRight);
}
+ QHBoxLayout* angleUnitLayout = new QHBoxLayout();
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ angleUnitLayout->addWidget(angleUnit);
+ }
+
QFormLayout* layout = new QFormLayout();
this->setLayout(layout);
@@ -92,13 +104,8 @@ namespace armarx::nav::locgrapheditor
layout->addRow("Roll", roll);
layout->addRow("Pitch", pitch);
layout->addRow("Yaw", yaw);
-
- QHBoxLayout* angleUnitLayout = new QHBoxLayout();
- for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
- {
- angleUnitLayout->addWidget(angleUnit);
- }
- layout->addRow("", angleUnitLayout);
+ layout->addRow("Angle Units", angleUnitLayout);
+ layout->addRow(_useCurrentRobotPose);
// Connect
@@ -117,6 +124,9 @@ namespace armarx::nav::locgrapheditor
this, &This::_updateVertexAttribs);
}
+ connect(_useCurrentRobotPose, &QPushButton::pressed,
+ this, &This::_setFromCurrentRobotPose);
+
angleUnitDeg->click();
}
@@ -179,16 +189,21 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::setRobotConnection(robotvisu::Connection* connection)
+ {
+ this->_robotConnection = connection;
+ _useCurrentRobotPose->setEnabled(_robotConnection != nullptr);
+ }
+
+
void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
{
const VertexData& attrib = vertex.attrib();
- const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
name->setText(QString::fromStdString(attrib.getName()));
locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
frame->setText("<WIP>");
- _setXyz(simox::math::position(pose));
- _setRpyRad(simox::math::mat4f_to_rpy(pose));
+ _setPose(attrib.getPose().cast<qreal>());
}
@@ -284,6 +299,13 @@ namespace armarx::nav::locgrapheditor
}
+ void VertexDataWidget::_setPose(const Eigen::Matrix4d& pose)
+ {
+ _setXyz(simox::math::position(pose));
+ _setRpyRad(simox::math::mat4f_to_rpy(pose));
+ }
+
+
void VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
{
x->setValue(xyz.x());
@@ -331,4 +353,15 @@ namespace armarx::nav::locgrapheditor
}
}
+
+ void VertexDataWidget::_setFromCurrentRobotPose()
+ {
+ ARMARX_CHECK_NOT_NULL(_robotConnection);
+ {
+ QSignalBlocker blocker(this);
+ _setPose(_robotConnection->getGlobalPose().cast<qreal>());
+ }
+ _updateVertexAttribs();
+ }
+
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index e16c824e..21ff9daf 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -30,9 +30,15 @@
class QLineEdit;
class QDoubleSpinBox;
+class QPushButton;
class QRadioButton;
+
+namespace armarx::nav::locgrapheditor::robotvisu
+{
+ class Connection;
+}
namespace armarx::nav::locgrapheditor
{
@@ -56,6 +62,9 @@ namespace armarx::nav::locgrapheditor
Eigen::Vector3d rpyRad() const;
+ void setRobotConnection(robotvisu::Connection* connection);
+
+
signals:
void vertexDataChanged();
@@ -76,6 +85,7 @@ namespace armarx::nav::locgrapheditor
std::vector<QDoubleSpinBox*> _angleSpinBoxes();
std::vector<QDoubleSpinBox*> _allSpinBoxes();
+ void _setPose(const Eigen::Matrix4d& pose);
void _setXyz(const Eigen::Vector3d& xyz);
Eigen::Vector3d _rpyRaw() const;
@@ -83,6 +93,8 @@ namespace armarx::nav::locgrapheditor
void _setRpyDeg(const Eigen::Vector3d& rpyDeg) const;
void _setRpyRad(const Eigen::Vector3d& rpyRad) const;
+ void _setFromCurrentRobotPose();
+
private:
@@ -104,6 +116,10 @@ namespace armarx::nav::locgrapheditor
QRadioButton* angleUnitDeg = nullptr;
QRadioButton* angleUnitRad = nullptr;
+
+ robotvisu::Connection* _robotConnection = nullptr;
+ QPushButton* _useCurrentRobotPose = nullptr;
+
};
}
--
GitLab
From 1a3bcee9d9632cbc914d26a3bb0cbd407d4721ed Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 26 Aug 2021 15:58:12 +0200
Subject: [PATCH 27/33] Add visualization to NavigationMemory
---
.../NavigationMemory/CMakeLists.txt | 9 +-
.../NavigationMemory/NavigationMemory.cpp | 138 +++++++++---------
.../NavigationMemory/NavigationMemory.h | 84 +++++------
.../components/NavigationMemory/Visu.cpp | 114 +++++++++++++++
.../components/NavigationMemory/Visu.h | 65 +++++++++
source/armarx/navigation/graph/Graph.cpp | 41 ++----
source/armarx/navigation/graph/Graph.h | 37 ++++-
source/armarx/navigation/graph/Visu.cpp | 33 ++++-
source/armarx/navigation/graph/Visu.h | 5 +-
.../LocationGraphEditorWidgetController.cpp | 4 +-
.../gui-plugins/LocationGraphEditor/Visu.cpp | 7 +-
.../gui-plugins/LocationGraphEditor/Visu.h | 2 +-
12 files changed, 374 insertions(+), 165 deletions(-)
create mode 100644 source/Navigation/components/NavigationMemory/Visu.cpp
create mode 100644 source/Navigation/components/NavigationMemory/Visu.h
diff --git a/source/Navigation/components/NavigationMemory/CMakeLists.txt b/source/Navigation/components/NavigationMemory/CMakeLists.txt
index ae5f287c..104cbfc0 100644
--- a/source/Navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/Navigation/components/NavigationMemory/CMakeLists.txt
@@ -20,7 +20,7 @@ armarx_add_component(
ArmarXCore
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
- ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
armem
@@ -36,9 +36,11 @@ armarx_add_component(
SOURCES
NavigationMemory.cpp
+ Visu.cpp
HEADERS
NavigationMemory.h
+ Visu.h
)
@@ -63,10 +65,9 @@ armarx_add_component(
# Add unit tests
-add_subdirectory(test)
+# 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::mynamespace"
+ COMPONENT_NAMESPACE "armarx::nav"
)
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
index 73f3fcfd..e20e0a99 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/Navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -21,6 +21,7 @@
*/
#include "NavigationMemory.h"
+#include "Visu.h"
#include <Navigation/libraries/core/aron/Trajectory.aron.generated.h>
#include <Navigation/libraries/core/aron/Twist.aron.generated.h>
@@ -28,15 +29,12 @@
#include <armarx/navigation/location/constants.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/graph/Graph.h>
-// Include headers you only need in function definitions in the .cpp.
+#include <ArmarXCore/core/time/CycleUtil.h>
-// #include <Eigen/Core>
-// #include <SimoxUtility/color/Color.h>
-
-
-namespace armarx
+namespace armarx::nav
{
armarx::PropertyDefinitionsPtr
@@ -55,14 +53,12 @@ namespace armarx
// def->component(myComponentProxy)
- // 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.");
- // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
-
+ def->optional(properties.locationGraph.visuLocations, "p.locationGraph.visuLocation",
+ "Enable visualization of locations.");
+ def->optional(properties.locationGraph.visuGraphEdges, "p.locationGraph.visuGraphEdges",
+ "Enable visualization of navigation graph edges.");
+ def->optional(properties.locationGraph.visuFrequency, "p.locationGraph.visuFrequency",
+ "Visualization frequeny of locations and graph edges [Hz].");
return def;
}
@@ -77,7 +73,7 @@ namespace armarx
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
- setMemoryName(p.memoryName);
+ setMemoryName(properties.memoryName);
workingMemory.addCoreSegment("Parameterization");
@@ -118,19 +114,17 @@ namespace armarx
}
*/
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
// Setup the remote GUI.
{
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
- */
+
+ tasks.visuTask = new SimpleRunningTask<>([this]()
+ {
+ this->visuRun();
+ }, "Visualization");
+ tasks.visuTask->start();
}
@@ -153,80 +147,92 @@ namespace armarx
}
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
void NavigationMemory::createRemoteGuiTab()
{
using namespace armarx::RemoteGui::Client;
// Setup the widgets.
+ tab.locationGraph.setup(*this);
- tab.boxLayerName.setValue(properties.boxLayerName);
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
+ // Setup the layout.
+ VBoxLayout root = {tab.locationGraph.group, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
- tab.drawBoxes.setLabel("Draw Boxes");
- // Setup the layout.
+ void NavigationMemory::RemoteGui_update()
+ {
+ tab.locationGraph.update(*this);
+ }
+
+ void NavigationMemory::RemoteGuiTab::LocationGraph::setup(NavigationMemory& owner)
+ {
+ using namespace armarx::RemoteGui::Client;
GridLayout grid;
int row = 0;
{
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
+ visuLocations.setValue(owner.properties.locationGraph.visuLocations);
+ visuGraphEdges.setValue(owner.properties.locationGraph.visuLocations);
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+ grid.add(Label("Visualize Locations"), {row, 0}).add(visuLocations, {row, 1});
++row;
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+ grid.add(Label("Visualize Graph Edges"), {row, 0}).add(visuGraphEdges, {row, 1});
++row;
}
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
+ group = GroupBox({grid});
}
- void NavigationMemory::RemoteGui_update()
+ void NavigationMemory::RemoteGuiTab::LocationGraph::update(NavigationMemory& owner)
{
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
+ if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
{
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
-
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- }
- if (tab.drawBoxes.wasClicked())
- {
- // Lock shared variables in methods running in seperate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex, arvizMutex);
- drawBoxes(properties, arviz);
+ std::scoped_lock lock(owner.propertiesMutex);
+ owner.properties.locationGraph.visuLocations = visuLocations.getValue();
+ owner.properties.locationGraph.visuGraphEdges = visuGraphEdges.getValue();
}
}
- */
- /* (Requires the armarx::ArVizComponentPluginUser.)
- void NavigationMemory::drawBoxes(const NavigationMemory::Properties& p, viz::Client& arviz)
+
+ void NavigationMemory::visuRun()
{
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
+ memory::Visu visu {
+ arviz,
+ workingMemory.getCoreSegment(nav::loc::coreSegmentID),
+ workingMemory.getCoreSegment(nav::graph::coreSegmentID)
+ };
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
+ Properties::LocationGraph p;
{
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
+ std::scoped_lock lock(propertiesMutex);
+ p = properties.locationGraph;
+ }
+
+ CycleUtil cycle(static_cast<int>(1000 / p.visuFrequency));
+ while (tasks.visuTask and not tasks.visuTask->isStopped())
+ {
+ {
+ std::scoped_lock lock(propertiesMutex);
+ p = properties.locationGraph;
+ }
+
+ std::vector<viz::Layer> layers;
+
+ // Locations
+ visu.drawLocations(layers, p.visuLocations);
+
+ // Graph Edges
+ visu.drawGraphs(layers, p.visuGraphEdges);
+
+ arviz.commit(layers);
+
+ cycle.waitForCycleDuration();
}
- arviz.commit(layer);
}
- */
+
} // namespace armarx
diff --git a/source/Navigation/components/NavigationMemory/NavigationMemory.h b/source/Navigation/components/NavigationMemory/NavigationMemory.h
index 15d2088a..444868c5 100644
--- a/source/Navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/Navigation/components/NavigationMemory/NavigationMemory.h
@@ -23,20 +23,19 @@
#pragma once
-// #include <mutex>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
-#include <ArmarXCore/core/Component.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
+#include <mutex>
-namespace armarx
+namespace armarx::nav
{
/**
@@ -51,19 +50,20 @@ namespace armarx
* Detailed description of class NavigationMemory.
*/
class NavigationMemory :
- virtual public armarx::Component,
+ virtual public armarx::Component
// , virtual public armarx::DebugObserverComponentPluginUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
- virtual public armarx::armem::server::ComponentPluginUser
-
+ , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
+ , virtual public armarx::armem::server::ComponentPluginUser
{
public:
+
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
protected:
+
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -80,63 +80,59 @@ namespace armarx
void onExitComponent() override;
- /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
- /// This function should be called once in onConnect() or when you
- /// need to re-create the Remote GUI tab.
void createRemoteGuiTab();
-
- /// 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:
- // Private methods go here.
-
- // Forward declare `Properties` if you used it before its defined.
- // struct Properties;
- /* (Requires the armarx::ArVizComponentPluginUser.)
- /// Draw some boxes in ArViz.
- void drawBoxes(const Properties& p, viz::Client& arviz);
- */
+ void visuRun();
private:
- // Private member variables go here.
/// Properties shown in the Scenario GUI.
struct Properties
{
std::string memoryName = "Navigation";
+
+ struct LocationGraph
+ {
+ bool visuLocations = true;
+ bool visuGraphEdges = true;
+ float visuFrequency = 2;
+ };
+ LocationGraph locationGraph;
};
- Properties p;
- /* Use a mutex if you access variables from different threads
- * (e.g. ice functions and RemoteGui_update()).
+ Properties properties;
std::mutex propertiesMutex;
- */
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
/// Tab shown in the Remote GUI.
struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
{
- armarx::RemoteGui::Client::LineEdit boxLayerName;
- armarx::RemoteGui::Client::IntSpinBox numBoxes;
+ struct LocationGraph
+ {
+ armarx::RemoteGui::Client::GroupBox group;
+
+ armarx::RemoteGui::Client::CheckBox visuLocations;
+ armarx::RemoteGui::Client::CheckBox visuGraphEdges;
- armarx::RemoteGui::Client::Button drawBoxes;
+ void setup(NavigationMemory& owner);
+ void update(NavigationMemory& owner);
+ };
+ LocationGraph locationGraph;
};
RemoteGuiTab tab;
- */
- /* (Requires the armarx::ArVizComponentPluginUser.)
- * When used from different threads, an ArViz client needs to be synchronized.
- /// Protects the arviz client inherited from the ArViz plugin.
- std::mutex arvizMutex;
- */
+ struct Tasks
+ {
+ SimpleRunningTask<>::pointer_type visuTask;
+ };
+ Tasks tasks;
+
};
+
} // namespace armarx
diff --git a/source/Navigation/components/NavigationMemory/Visu.cpp b/source/Navigation/components/NavigationMemory/Visu.cpp
new file mode 100644
index 00000000..19e69de3
--- /dev/null
+++ b/source/Navigation/components/NavigationMemory/Visu.cpp
@@ -0,0 +1,114 @@
+/*
+ * 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 Navigation::ArmarXObjects::NavigationMemory
+ * @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 "Visu.h"
+
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
+#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/graph/Visu.h>
+
+#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
+
+
+namespace armarx::nav::memory
+{
+
+ Visu::Visu(viz::Client arviz,
+ const armem::server::wm::CoreSegment& locSegment,
+ const armem::server::wm::CoreSegment& graphSegment) :
+ arviz(arviz),
+ locSegment(locSegment),
+ graphSegment(graphSegment),
+ visu(std::make_unique<graph::GraphVisu>())
+ {
+ }
+
+
+ Visu::~Visu()
+ {
+ }
+
+
+ void Visu::drawLocations(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ using namespace armem::server;
+
+ viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
+ if (enabled)
+ {
+ std::map<armem::MemoryID, loc::arondto::Location> locations;
+ locSegment.doLocked([&]()
+ {
+ locSegment.forEachEntity([&](const wm::Entity& entity)
+ {
+ if (const wm::EntityInstance* instance = entity.findLatestInstance())
+ {
+ locations[entity.id()].fromAron(instance->data());
+ }
+ });
+ });
+
+ for (auto& [id, location] : locations)
+ {
+ visu->vertex->draw(layer, id.str(), location.globalRobotPose);
+ }
+ }
+ }
+
+
+ void Visu::drawGraphs(std::vector<viz::Layer>& layers, bool enabled)
+ {
+ using namespace armem::server;
+
+ std::map<armem::MemoryID, graph::Graph> graphs;
+ graphSegment.doLocked([&]()
+ {
+ graphSegment.forEachEntity([&](const wm::Entity& entity)
+ {
+ graph::Graph& graph = graphs[entity.id()];
+ if (enabled)
+ {
+ if (const wm::EntityInstance* instance = entity.findLatestInstance())
+ {
+ nav::graph::arondto::Graph aron;
+ aron.fromAron(instance->data());
+ fromAron(aron, graph);
+ }
+ }
+ // else: empty layer
+ });
+ });
+
+ for (auto& [id, graph] : graphs)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer(id.str()));
+ if (enabled)
+ {
+ graph::resolveLocations(graph, locSegment);
+ visu->draw(layer, graph);
+ }
+ // else: clear layer
+ }
+ }
+
+}
diff --git a/source/Navigation/components/NavigationMemory/Visu.h b/source/Navigation/components/NavigationMemory/Visu.h
new file mode 100644
index 00000000..c5c9076c
--- /dev/null
+++ b/source/Navigation/components/NavigationMemory/Visu.h
@@ -0,0 +1,65 @@
+/*
+ * 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 Navigation::ArmarXObjects::NavigationMemory
+ * @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 <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+#include <memory>
+#include <vector>
+
+
+namespace armarx::nav::graph
+{
+ class GraphVisu;
+}
+namespace armarx::nav::memory
+{
+
+ class Visu
+ {
+ public:
+
+ Visu(viz::Client arviz,
+ const armem::server::wm::CoreSegment& locSegment,
+ const armem::server::wm::CoreSegment& graphSegment);
+ ~Visu();
+
+
+ void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
+ void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
+
+
+ public:
+
+ viz::Client arviz;
+
+ const armem::server::wm::CoreSegment& locSegment;
+ const armem::server::wm::CoreSegment& graphSegment;
+
+ std::unique_ptr<graph::GraphVisu> visu;
+
+ };
+
+
+}
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index 04a41807..31f6d056 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -25,7 +25,6 @@
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
@@ -60,13 +59,13 @@ namespace armarx::nav::graph
namespace armarx::nav
{
- void graph::toAron(arondto::Graph& dto, const Graph& bo)
+void graph::toAron(arondto::Graph& dto, const Graph& bo)
+{
+ dto = {};
+ for (auto vertex : bo.vertices())
{
- dto = {};
- for (auto vertex : bo.vertices())
- {
- auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
- v.vertexID = static_cast<long>(vertex.objectID());
+ auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
+ v.vertexID = static_cast<long>(vertex.objectID());
}
ARMARX_CHECK_EQUAL(dto.vertices.size(), bo.numVertices());
@@ -99,31 +98,11 @@ namespace armarx::nav
}
- void graph::resolveLocations(Graph& bo, const armem::wm::Memory& locationMemory)
+ void graph::resolveLocation(Graph::Vertex& vertex, const aron::datanavigator::DictNavigatorPtr& locationData)
{
- for (graph::Graph::Vertex vertex : bo.vertices())
- {
- resolveLocation(vertex, locationMemory);
- }
- }
-
-
- bool graph::resolveLocation(Graph::Vertex& vertex, const armem::wm::Memory& locationMemory)
- {
- armem::MemoryID locationID;
- fromAron(vertex.attrib().aron.locationID, locationID);
-
- if (const armem::wm::EntityInstance* instance = locationMemory.findLatestInstance(locationID))
- {
- nav::loc::arondto::Location dto;
- dto.fromAron(instance->data());
- vertex.attrib().setPose(dto.globalRobotPose);
- return true;
- }
- else
- {
- return false;
- }
+ nav::loc::arondto::Location dto;
+ dto.fromAron(locationData);
+ vertex.attrib().setPose(dto.globalRobotPose);
}
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index 731264d6..be40293d 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -24,6 +24,8 @@
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
@@ -62,7 +64,38 @@ namespace armarx::nav::graph
void toAron(arondto::Graph& dto, const Graph& bo);
void fromAron(const arondto::Graph& dto, Graph& bo);
- void resolveLocations(Graph& bo, const armem::wm::Memory& locationMemory);
- bool resolveLocation(Graph::Vertex& vertex, const armem::wm::Memory& locationMemory);
+
+
+ // Location resolution
+
+ void resolveLocation(Graph::Vertex& vertex, const aron::datanavigator::DictNavigatorPtr& locationData);
+
+
+ template <class MemoryContainerT>
+ bool resolveLocation(Graph::Vertex& vertex, const MemoryContainerT& locationContainer)
+ {
+ armem::MemoryID locationID;
+ fromAron(vertex.attrib().aron.locationID, locationID);
+
+ if (const auto* instance = locationContainer.findLatestInstance(locationID))
+ {
+ resolveLocation(vertex, instance->data());
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+
+
+ template <class MemoryContainerT>
+ void resolveLocations(Graph& graph, const MemoryContainerT& locationContainer)
+ {
+ for (graph::Graph::Vertex vertex : graph.vertices())
+ {
+ resolveLocation(vertex, locationContainer);
+ }
+ }
}
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
index 3d98b5d7..6eb4c17b 100644
--- a/source/armarx/navigation/graph/Visu.cpp
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -34,15 +34,26 @@ namespace armarx::nav::graph
viz::Pose VertexVisu::Pose::draw(const VertexAttribs& attribs) const
{
- return viz::Pose(attribs.getName()).pose(attribs.getPose()).scale(scale);
+ return draw(attribs.getName(), attribs.getPose());
+ }
+
+ viz::Pose VertexVisu::Pose::draw(const std::string& name, const Eigen::Matrix4f& pose) const
+ {
+ return viz::Pose(name).pose(pose).scale(scale);
}
viz::Arrow VertexVisu::ForwardArrow::draw(const VertexAttribs& attribs) const
{
- return viz::Arrow(attribs.getName() + " forward")
- .fromTo(simox::math::position(attribs.getPose()),
- simox::math::transform_position(attribs.getPose(), length * Eigen::Vector3f::UnitY()))
+ return draw(attribs.getName(), attribs.getPose());
+ }
+
+
+ viz::Arrow VertexVisu::ForwardArrow::draw(const std::string& name, const Eigen::Matrix4f& pose) const
+ {
+ return viz::Arrow(name + " forward")
+ .fromTo(simox::math::position(pose),
+ simox::math::transform_position(pose, length * Eigen::Vector3f::UnitY()))
.color(color)
.width(width);
}
@@ -56,13 +67,19 @@ namespace armarx::nav::graph
void VertexVisu::draw(viz::Layer& layer, const VertexAttribs& attribs) const
{
- if (pose.has_value())
+ draw(layer, attribs.getName(), attribs.getPose());
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, const std::string& name, const Eigen::Matrix4f& pose) const
+ {
+ if (this->pose.has_value())
{
- layer.add(pose->draw(attribs));
+ layer.add(this->pose->draw(name, pose));
}
if (forwardArrow.has_value())
{
- layer.add(forwardArrow->draw(attribs));
+ layer.add(forwardArrow->draw(name, pose));
}
}
@@ -96,7 +113,7 @@ namespace armarx::nav::graph
}
- void GraphVisu::draw(viz::Layer& layer, const Graph& graph)
+ void GraphVisu::draw(viz::Layer& layer, const Graph& graph) const
{
if (vertex.has_value())
{
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
index dffc4b47..90976f01 100644
--- a/source/armarx/navigation/graph/Visu.h
+++ b/source/armarx/navigation/graph/Visu.h
@@ -44,6 +44,7 @@ namespace armarx::nav::graph
float scale = 1.0;
viz::Pose draw(const VertexAttribs& attribs) const;
+ viz::Pose draw(const std::string& name, const Eigen::Matrix4f& pose) const;
};
std::optional<Pose> pose = Pose {};
@@ -54,12 +55,14 @@ namespace armarx::nav::graph
simox::Color color = simox::Color::green();
viz::Arrow draw(const VertexAttribs& attribs) const;
+ viz::Arrow draw(const std::string& name, const Eigen::Matrix4f& pose) const;
};
std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
void draw(viz::Layer& layer, Graph::ConstVertex vertex) const;
void draw(viz::Layer& layer, const VertexAttribs& attribs) const;
+ void draw(viz::Layer& layer, const std::string& name, const Eigen::Matrix4f& pose) const;
};
@@ -86,7 +89,7 @@ namespace armarx::nav::graph
std::optional<EdgeVisu> edge = EdgeVisu {};
- void draw(viz::Layer& layer, const Graph& graph);
+ void draw(viz::Layer& layer, const Graph& graph) const;
};
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index ed6c1893..c5d86e4d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -752,11 +752,11 @@ namespace armarx::nav::locgrapheditor
{
std::vector<viz::Layer> layers;
{
- viz::Layer& vertices = layers.emplace_back(remote.arviz->layer("Vertices"));
+ viz::Layer& vertices = layers.emplace_back(remote.arviz->layer("Locations"));
applyVisu(vertices, model.visu.vertex, model.graph.vertices());
}
{
- viz::Layer& edges = layers.emplace_back(remote.arviz->layer("Edges"));
+ viz::Layer& edges = layers.emplace_back(remote.arviz->layer("Graph Edges"));
applyVisu(edges, model.visu.edge, model.graph.edges());
}
{
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
index 5476690f..be1323e9 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -107,12 +107,8 @@ namespace armarx::nav::locgrapheditor
}
- void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph)
+ void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph) const
{
-#if 1
- applyVisu(layer, vertex, graph.vertices());
- applyVisu(layer, edge, graph.edges());
-#else
if (vertex.has_value())
{
for (GuiGraph::ConstVertex v : graph.vertices())
@@ -127,7 +123,6 @@ namespace armarx::nav::locgrapheditor
edge->draw(layer, e);
}
}
-#endif
}
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
index 9cc9ba2e..0266cd2f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -83,7 +83,7 @@ namespace armarx::nav::locgrapheditor
std::optional<EdgeVisu> edge = EdgeVisu {};
- void draw(viz::Layer& layer, const GuiGraph& graph);
+ void draw(viz::Layer& layer, const GuiGraph& graph) const;
};
--
GitLab
From ad1bf440d1a3953c59e1c6998ca0b1e730baf8c0 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 31 Aug 2021 22:47:34 +0200
Subject: [PATCH 28/33] fix
---
.../components/NavigationMemory/CMakeLists.txt | 17 ++---------------
1 file changed, 2 insertions(+), 15 deletions(-)
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 104cbfc0..8427c268 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -1,7 +1,3 @@
-set(LIB_NAME NavigationMemory)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
# If your component needs a special ice interface, define it here:
@@ -14,8 +10,8 @@ armarx_set_target("Library: ${LIB_NAME}")
# Add the component
-armarx_add_component(
- COMPONENT_LIBS
+armarx_add_component(NavigationMemory
+ DEPENDENCIES
# ArmarXCore
ArmarXCore
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
@@ -62,12 +58,3 @@ armarx_add_component(
# ARON_FILES
# aron/ExampleData.xml
#)
-
-
-# Add unit tests
-# add_subdirectory(test)
-
-# Generate the application
-armarx_generate_and_add_component_executable(
- COMPONENT_NAMESPACE "armarx::nav"
-)
--
GitLab
From 82177aa00cc4a04ec7c7c170be15cd2a726c1778 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 31 Aug 2021 23:17:36 +0200
Subject: [PATCH 29/33] adding deleted files
---
.../memory/client/parameterization/Writer.cpp | 68 +++++++++++++++++++
.../memory/client/parameterization/Writer.h | 51 ++++++++++++++
2 files changed, 119 insertions(+)
create mode 100644 source/armarx/navigation/memory/client/parameterization/Writer.cpp
create mode 100644 source/armarx/navigation/memory/client/parameterization/Writer.h
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
new file mode 100644
index 00000000..df26a8bc
--- /dev/null
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -0,0 +1,68 @@
+#include "Writer.h"
+
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
+
+#include <Navigation/libraries/core/constants.h>
+
+
+namespace armarx::nav::mem::client::param
+{
+ bool
+ Writer::store(
+ const std::unordered_map<core::StackLayer, aron::datanavigator::DictNavigatorPtr>& stack,
+ const std::string& clientID,
+ const core::TimestampUs& timestamp)
+ {
+ ARMARX_CHECK(not stack.empty());
+
+ armem::Time ts = armem::Time::microSeconds(timestamp.count());
+
+ ARMARX_INFO << "timestamp chrono " << timestamp;
+ ARMARX_INFO << "timestamp " << ts;
+
+ armem::Commit commit;
+
+ for (const auto& [layer, data] : stack)
+ {
+ const std::string layerName = core::StackLayerNames.to_name(layer);
+
+ armem::EntityUpdate update;
+ update.entityID = armem::MemoryID()
+ .withMemoryName(properties().memoryName)
+ .withCoreSegmentName(properties().coreSegmentName)
+ .withProviderSegmentName(clientID)
+ .withEntityName(layerName)
+ .withTimestamp(ts);
+ update.timeCreated = ts;
+ update.instancesData = {data};
+ commit.add(update);
+ }
+
+ std::lock_guard g{memoryWriterMutex()};
+ armem::CommitResult updateResult = memoryWriter().commit(commit);
+
+ ARMARX_DEBUG << updateResult;
+
+ if (not updateResult.allSuccess())
+ {
+ ARMARX_ERROR << updateResult.allErrorMessages();
+ }
+ return updateResult.allSuccess();
+ }
+
+ std::string
+ Writer::propertyPrefix() const
+ {
+ return "mem.nav.param.";
+ }
+
+ Writer::Properties
+ Writer::defaultProperties() const
+ {
+ return Properties{
+ .memoryName = "Navigation",
+ .coreSegmentName = "Parameterization",
+ .providerName = "" // clientId
+ };
+ }
+} // namespace armarx::nav::mem::client::param
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h
new file mode 100644
index 00000000..9c4db3de
--- /dev/null
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -0,0 +1,51 @@
+/**
+ * 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/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
+
+#include <Navigation/libraries/core/constants.h>
+#include <Navigation/libraries/core/types.h>
+
+
+namespace armarx::nav::mem::client::param
+{
+
+ class Writer : virtual public armem::client::util::SimpleWriterBase
+ {
+ public:
+ using armem::client::util::SimpleWriterBase::SimpleWriterBase;
+
+
+ bool store(const std::unordered_map<core::StackLayer,
+ aron::datanavigator::DictNavigatorPtr>& stack,
+ const std::string& clientID,
+ const core::TimestampUs& timestamp);
+
+ std::string propertyPrefix() const override;
+ Properties defaultProperties() const override;
+
+ protected:
+ private:
+ };
+} // namespace armarx::nav::mem::client::param
--
GitLab
From 2f3bd4fc2b550c7f8f0d353e71080c3d89bacbf2 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 31 Aug 2021 23:53:14 +0200
Subject: [PATCH 30/33] multiple fixes: namespaces; includes; registering
decoupled main
---
CMakeLists.txt | 2 +
source/armarx/navigation/CMakeLists.txt | 2 +
.../ExampleClient/ExampleClient.cpp | 258 ++++-----
.../components/ExampleClient/ExampleClient.h | 8 +-
.../GraphImportExport/GraphImportExport.cpp | 172 +++---
.../GraphImportExport/GraphImportExport.h | 41 +-
.../NavigationMemory/CMakeLists.txt | 4 +-
.../NavigationMemory/NavigationMemory.cpp | 32 +-
.../NavigationMemory/NavigationMemory.h | 12 +-
.../components/NavigationMemory/Visu.cpp | 8 +-
.../components/NavigationMemory/Visu.h | 9 +-
.../components/Navigator/Navigator.cpp | 491 ++++++++--------
source/armarx/navigation/graph/CMakeLists.txt | 45 +-
source/armarx/navigation/graph/Graph.cpp | 48 +-
source/armarx/navigation/graph/Graph.h | 27 +-
source/armarx/navigation/graph/Visu.cpp | 67 ++-
source/armarx/navigation/graph/Visu.h | 24 +-
source/armarx/navigation/graph/aron/Graph.xml | 10 +-
source/armarx/navigation/graph/constants.cpp | 4 +-
source/armarx/navigation/graph/constants.h | 5 +-
.../navigation/graph/forward_declarations.h | 4 +-
.../LocationGraphEditor/GraphScene.cpp | 89 +--
.../LocationGraphEditor/GraphScene.h | 48 +-
.../LocationGraphEditor/GuiGraph.cpp | 32 +-
.../LocationGraphEditor/GuiGraph.h | 19 +-
.../LocationGraphEditorWidgetController.cpp | 522 ++++++++++--------
.../LocationGraphEditorWidgetController.h | 63 +--
.../gui-plugins/LocationGraphEditor/Visu.cpp | 46 +-
.../gui-plugins/LocationGraphEditor/Visu.h | 35 +-
.../widgets/ConnectDialog.cpp | 2 +-
.../widgets/ConnectDialog.h | 28 +-
.../widgets/EdgeTableWidget.cpp | 29 +-
.../widgets/EdgeTableWidget.h | 22 +-
.../widgets/NewEntityIdDialog.cpp | 38 +-
.../widgets/NewEntityIdDialog.h | 12 +-
.../widgets/RobotVisuWidget.cpp | 119 ++--
.../widgets/RobotVisuWidget.h | 31 +-
.../widgets/VertexDataWidget.cpp | 119 ++--
.../widgets/VertexDataWidget.h | 15 +-
.../widgets/VertexTableWidget.cpp | 160 +++---
.../widgets/VertexTableWidget.h | 20 +-
.../widgets/default_colors.cpp | 4 +-
.../widgets/default_colors.h | 4 +-
.../LocationGraphEditor/widgets/graph_scene.h | 9 +-
.../widgets/graph_scene/ControlWidget.cpp | 61 +-
.../widgets/graph_scene/ControlWidget.h | 8 +-
.../widgets/graph_scene/Scene.cpp | 95 ++--
.../widgets/graph_scene/Scene.h | 64 +--
.../widgets/graph_scene/Widget.cpp | 33 +-
.../widgets/graph_scene/Widget.h | 8 +-
.../LocationGraphEditor/widgets/utils.cpp | 6 +-
.../LocationGraphEditor/widgets/utils.h | 8 +-
.../armarx/navigation/location/CMakeLists.txt | 29 +-
.../navigation/location/aron/Location.xml | 6 +-
.../armarx/navigation/location/constants.cpp | 4 +-
source/armarx/navigation/location/constants.h | 2 +-
.../armarx/navigation/memory/CMakeLists.txt | 4 +
.../memory/client/parameterization/Writer.cpp | 7 +-
.../memory/client/parameterization/Writer.h | 10 +-
59 files changed, 1586 insertions(+), 1498 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c4dfcbe..151a1979 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,6 +28,8 @@ add_subdirectory(external)
armarx_depends_on(SYSTEM Ceres REQUIRED)
# Optional dependencies
+armarx_depends_on(SYSTEM VTK)
+armarx_depends_on(SYSTEM SemanticObjectRelations QUIET)
#include(FetchContent)
diff --git a/source/armarx/navigation/CMakeLists.txt b/source/armarx/navigation/CMakeLists.txt
index e726ba5d..39a4589e 100644
--- a/source/armarx/navigation/CMakeLists.txt
+++ b/source/armarx/navigation/CMakeLists.txt
@@ -9,6 +9,8 @@ add_subdirectory(trajectory_control)
add_subdirectory(safety_control)
add_subdirectory(client)
add_subdirectory(factories)
+add_subdirectory(graph)
+add_subdirectory(location)
add_subdirectory(memory)
add_subdirectory(server)
diff --git a/source/armarx/navigation/components/ExampleClient/ExampleClient.cpp b/source/armarx/navigation/components/ExampleClient/ExampleClient.cpp
index c67cc423..2add2184 100644
--- a/source/armarx/navigation/components/ExampleClient/ExampleClient.cpp
+++ b/source/armarx/navigation/components/ExampleClient/ExampleClient.cpp
@@ -20,166 +20,178 @@
* GNU General Public License
*/
+#include <chrono>
+#include <thread>
+
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
#include <armarx/navigation/components/ExampleClient/ExampleClient.h>
#include <armarx/navigation/global_planning/AStar.h>
#include <armarx/navigation/trajectory_control/TrajectoryFollowingController.h>
-// STD/STL
-#include <chrono>
-#include <thread>
-exns::ExampleClient::ExampleClient()
+namespace armarx::navigation::examples
{
- // pass
-}
-exns::ExampleClient::~ExampleClient()
-{
- // pass
-}
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(ExampleClient);
-void
-exns::ExampleClient::onInitComponent()
-{
- // pass
-}
-void
-exns::ExampleClient::onConnectComponent()
-{
- task = new armarx::RunningTask<ExampleClient>(this, &ExampleClient::exampleNavigation);
- task->start();
-}
+ ExampleClient::ExampleClient()
+ {
+ // pass
+ }
-void
-exns::ExampleClient::onDisconnectComponent()
-{
- const bool join = true;
- task->stop(join);
-}
+ ExampleClient::~ExampleClient()
+ {
+ // pass
+ }
-void
-exns::ExampleClient::onExitComponent()
-{
- // pass
-}
+ void
+ ExampleClient::onInitComponent()
+ {
+ // pass
+ }
-std::string
-exns::ExampleClient::getDefaultName() const
-{
- return "ExampleClient";
-}
+ void
+ ExampleClient::onConnectComponent()
+ {
+ task = new armarx::RunningTask<ExampleClient>(this, &ExampleClient::exampleNavigation);
+ task->start();
+ }
-void
-exns::ExampleClient::exampleNavigation()
-{
- using namespace std::chrono_literals;
+ void
+ ExampleClient::onDisconnectComponent()
+ {
+ const bool join = true;
+ task->stop(join);
+ }
- // Import relevant namespaces.
- using namespace armarx::navigation;
+ void
+ ExampleClient::onExitComponent()
+ {
+ // pass
+ }
- ARMARX_INFO << "Configuring navigator";
+ std::string
+ ExampleClient::getDefaultName() const
+ {
+ return "ExampleClient";
+ }
- // Create an example configuration valid for the following move* calls.
- configureNavigator(
- client::NavigationStackConfig()
- .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
- .globalPlanner(glob_plan::AStarParams())
- .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
+ void
+ ExampleClient::exampleNavigation()
+ {
+ using namespace std::chrono_literals;
- // Example of registering a lambda as callback.
- navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
+ // Import relevant namespaces.
+ using namespace armarx::navigation;
- // Example of registering a method as callback.
- navigator.onGoalReached([this] { goalReached(); });
+ ARMARX_INFO << "Configuring navigator";
- std::this_thread::sleep_for(1s);
+ // Create an example configuration valid for the following move* calls.
+ configureNavigator(
+ client::NavigationStackConfig()
+ .general({} /*{.maxVel = VelocityLimits{.linear = 400 , .angular = 0.1}}*/)
+ .globalPlanner(glob_plan::AStarParams())
+ .trajectoryController(traj_ctrl::TrajectoryFollowingControllerParams()));
- ARMARX_INFO << "Moving to goal pose";
- // Start moving to goal position using above config.
- core::Pose goal = core::Pose::Identity();
- goal.translation() << 2000, 1000, 0;
+ // Example of registering a lambda as callback.
+ navigator.onGoalReached([&]() { ARMARX_IMPORTANT << "Goal reached! (lambda-style)"; });
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ // Example of registering a method as callback.
+ navigator.onGoalReached([this] { goalReached(); });
- std::this_thread::sleep_for(15s);
+ std::this_thread::sleep_for(1s);
- // Wait until goal is reached
- armarx::navigation::client::StopEvent se = navigator.waitForStop();
- if (se)
- {
- ARMARX_INFO << "Goal 1 reached.";
- }
- else
- {
- if (se.isSafetyStopTriggeredEvent())
+ ARMARX_INFO << "Moving to goal pose";
+ // Start moving to goal position using above config.
+ core::Pose goal = core::Pose::Identity();
+ goal.translation() << 2000, 1000, 0;
+
+ navigator.moveTo(goal, core::NavigationFrame::Absolute);
+
+ std::this_thread::sleep_for(15s);
+
+ // Wait until goal is reached
+ armarx::navigation::client::StopEvent se = navigator.waitForStop();
+ if (se)
{
- ARMARX_ERROR << "Safety stop was triggered!";
+ ARMARX_INFO << "Goal 1 reached.";
}
- else if (se.isUserAbortTriggeredEvent())
+ else
{
- ARMARX_ERROR << "Aborted by user!";
+ if (se.isSafetyStopTriggeredEvent())
+ {
+ ARMARX_ERROR << "Safety stop was triggered!";
+ }
+ else if (se.isUserAbortTriggeredEvent())
+ {
+ ARMARX_ERROR << "Aborted by user!";
+ }
+ else if (se.isInternalErrorEvent())
+ {
+ ARMARX_ERROR << "Unknown internal error occured! "
+ << se.toInternalErrorEvent().message;
+ }
}
- else if (se.isInternalErrorEvent())
+
+ goal.translation() << -1500, 1000, 0;
+ navigator.moveTo(goal, core::NavigationFrame::Absolute);
+
+ std::this_thread::sleep_for(15s);
+
+ // Wait until goal is reached
+ se = navigator.waitForStop();
+ if (se)
{
- ARMARX_ERROR << "Unknown internal error occured! " << se.toInternalErrorEvent().message;
+ ARMARX_INFO << "Goal 2 reached.";
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not reach goal 2!";
}
- }
-
- goal.translation() << -1500, 1000, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
- std::this_thread::sleep_for(15s);
+ goal.translation() << 4500, 4500, 0;
+ navigator.moveTo(goal, core::NavigationFrame::Absolute);
- // Wait until goal is reached
- se = navigator.waitForStop();
- if (se)
- {
- ARMARX_INFO << "Goal 2 reached.";
- }
- else
- {
- ARMARX_ERROR << "Could not reach goal 2!";
- }
+ // Wait until goal is reached
+ se = navigator.waitForStop();
+ if (se)
+ {
+ ARMARX_INFO << "Goal 3 reached.";
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not reach goal 3!";
+ }
- goal.translation() << 4500, 4500, 0;
- navigator.moveTo(goal, core::NavigationFrame::Absolute);
+ std::this_thread::sleep_for(15s);
- // Wait until goal is reached
- se = navigator.waitForStop();
- if (se)
- {
- ARMARX_INFO << "Goal 3 reached.";
- }
- else
- {
- ARMARX_ERROR << "Could not reach goal 3!";
- }
+ // TODO example with waypoints
- std::this_thread::sleep_for(15s);
+ ARMARX_INFO << "Moving into certain direction.";
+ // Start moving towards a direction
+ navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
- // TODO example with waypoints
+ std::this_thread::sleep_for(3s);
- ARMARX_INFO << "Moving into certain direction.";
- // Start moving towards a direction
- navigator.moveTowards(core::Direction(100, 100, 0), core::NavigationFrame::Relative);
+ ARMARX_INFO << "Pausing movement.";
+ navigator.pause();
+ }
- std::this_thread::sleep_for(3s);
+ void
+ ExampleClient::goalReached()
+ {
+ ARMARX_IMPORTANT << "Goal reached! (method-style)";
+ }
- ARMARX_INFO << "Pausing movement.";
- navigator.pause();
-}
+ armarx::PropertyDefinitionsPtr
+ ExampleClient::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+ return def;
+ }
-void
-exns::ExampleClient::goalReached()
-{
- ARMARX_IMPORTANT << "Goal reached! (method-style)";
-}
-armarx::PropertyDefinitionsPtr
-exns::ExampleClient::createPropertyDefinitions()
-{
- armarx::PropertyDefinitionsPtr def =
- new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
- return def;
-}
+} // namespace armarx::navigation::examples
diff --git a/source/armarx/navigation/components/ExampleClient/ExampleClient.h b/source/armarx/navigation/components/ExampleClient/ExampleClient.h
index d4b46046..a8879fb2 100644
--- a/source/armarx/navigation/components/ExampleClient/ExampleClient.h
+++ b/source/armarx/navigation/components/ExampleClient/ExampleClient.h
@@ -22,13 +22,12 @@
#pragma once
-// ArmarX
#include <ArmarXCore/util/tasks.h>
-// Navigation
#include <armarx/navigation/client.h>
-namespace exns
+
+namespace armarx::navigation::examples
{
/**
@@ -77,4 +76,5 @@ namespace exns
armarx::RunningTask<ExampleClient>::pointer_type task;
};
-} // namespace exns
+
+} // namespace armarx::navigation::examples
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 36936fee..92694a45 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -20,29 +20,27 @@
* GNU General Public License
*/
-#include <VirtualRobot/VirtualRobot.h>
-
#include "GraphImportExport.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 <armarx/navigation/graph/Graph.h>
-#include <armarx/navigation/graph/Visu.h>
+#include <iomanip>
+#include <VirtualRobot/VirtualRobot.h>
-#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
-#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/armem/core/Commit.h>
#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <iomanip>
+#include <MemoryX/core/MemoryXCoreObjectFactories.h>
+#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
+#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <armarx/navigation/location/constants.h>
namespace armarx::nav
@@ -55,9 +53,11 @@ namespace armarx::nav
}
- armarx::PropertyDefinitionsPtr GraphImportExport::createPropertyDefinitions()
+ armarx::PropertyDefinitionsPtr
+ GraphImportExport::createPropertyDefinitions()
{
- armarx::PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
def->component(proxies.priorKnowledge);
def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver");
@@ -66,7 +66,8 @@ namespace armarx::nav
}
- void GraphImportExport::onInitComponent()
+ void
+ GraphImportExport::onInitComponent()
{
// Topics and properties defined above are automagically registered.
@@ -75,7 +76,8 @@ namespace armarx::nav
}
- void GraphImportExport::onConnectComponent()
+ void
+ GraphImportExport::onConnectComponent()
{
// Get proxies.
if (proxies.priorKnowledge->hasGraphSegment())
@@ -92,23 +94,27 @@ namespace armarx::nav
}
- void GraphImportExport::onDisconnectComponent()
+ void
+ GraphImportExport::onDisconnectComponent()
{
}
- void GraphImportExport::onExitComponent()
+ void
+ GraphImportExport::onExitComponent()
{
}
- std::string GraphImportExport::getDefaultName() const
+ std::string
+ GraphImportExport::getDefaultName() const
{
return "GraphImportExport";
}
- void GraphImportExport::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
+ void
+ GraphImportExport::createRemoteGuiTab(const std::vector<std::string>& sceneNames)
{
using namespace armarx::RemoteGui::Client;
@@ -132,7 +138,9 @@ namespace armarx::nav
GridLayout grid;
int row = 0;
{
- grid.add(Label("Scene:"), {row, 0}).add(tab.sceneComboBox, {row, 1}).add(tab.sceneRefreshButton, {row, 2});
+ 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});
@@ -144,12 +152,14 @@ namespace armarx::nav
grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
++row;
- grid.add(tab.locationsMemoryxToArmemButton, {row, 0}).add(tab.locationsArmemToMemoryxButton, {row, 1})
- .add(tab.locationsClearArMemButton, {row, 2});
+ 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});
+ grid.add(tab.graphMemoryxToArmemButton, {row, 0})
+ .add(tab.graphArmemToMemoryxButton, {row, 1})
+ .add(tab.graphClearArMemButton, {row, 2});
++row;
}
@@ -158,7 +168,8 @@ namespace armarx::nav
}
- void GraphImportExport::RemoteGui_update()
+ void
+ GraphImportExport::RemoteGui_update()
{
if (tab.sceneRefreshButton.wasClicked())
{
@@ -193,14 +204,16 @@ namespace armarx::nav
}
- void GraphImportExport::refreshScenes()
+ void
+ GraphImportExport::refreshScenes()
{
const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes();
createRemoteGuiTab(sceneNames);
}
- void GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
+ void
+ GraphImportExport::locationsMemoryxToArmem(const std::string& sceneName)
{
const armem::Time time = armem::Time::now();
armem::Commit commit;
@@ -219,28 +232,26 @@ namespace armarx::nav
armarx::FramedPosePtr pose = armarx::FramedPosePtr::dynamicCast(node->getPose());
ARMARX_CHECK_NOT_NULL(pose);
- FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
+ 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()
- ;
+ 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::loc::arondto::Location data;
+ navigation::location::arondto::Location data;
data.globalRobotPose = globalNodePose->toEigen();
armem::EntityUpdate& update = commit.add();
update.entityID = getLocationProviderSegmentID().withEntityName(name);
update.timeCreated = time;
- update.instancesData = { data.toAron() };
+ update.instancesData = {data.toAron()};
}
}
@@ -249,7 +260,8 @@ namespace armarx::nav
armem::CommitResult result = proxies.locationWriter.commit(commit);
if (result.allSuccess())
{
- ARMARX_IMPORTANT << "Successfully exported " << result.results.size() << " locations from MemoryX to ArMem.";
+ ARMARX_IMPORTANT << "Successfully exported " << result.results.size()
+ << " locations from MemoryX to ArMem.";
}
else
{
@@ -263,21 +275,23 @@ namespace armarx::nav
}
- void GraphImportExport::locationsArmemToMemoryx(const std::string& sceneName)
+ void
+ GraphImportExport::locationsArmemToMemoryx(const std::string& sceneName)
{
ARMARX_IMPORTANT << "locationsArmemToMemoryx() is WIP!";
- (void) sceneName;
+ (void)sceneName;
}
- void GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
+ void
+ GraphImportExport::graphMemoryxToArmem(const std::string& sceneName)
{
memoryx::GraphNodeBaseList graphNodes = proxies.graphSegment->getNodesByScene(sceneName);
- nav::graph::Graph graph = toArmemGraph(graphNodes);
+ navigation::graph::Graph graph = toArmemGraph(graphNodes);
if (tab.visuEnabled.getValue())
{
- nav::graph::GraphVisu visu;
+ navigation::graph::GraphVisu visu;
viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
visu.draw(layer, graph);
@@ -286,7 +300,7 @@ namespace armarx::nav
}
// Build ARON Graph
- nav::graph::arondto::Graph aron;
+ navigation::graph::arondto::Graph aron;
toAron(aron, graph);
// Build commit
@@ -294,14 +308,15 @@ namespace armarx::nav
armem::EntityUpdate update;
update.entityID = getGraphProviderSegmentID().withEntityName(sceneName);
update.timeCreated = time;
- update.instancesData = { aron.toAron() };
+ update.instancesData = {aron.toAron()};
if (not tab.dryRun.getValue())
{
armem::EntityUpdateResult result = proxies.graphWriter.commit(update);
if (result.success)
{
- ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName << "' from MemoryX to ArMem.";
+ ARMARX_IMPORTANT << "Successfully exported graph '" << sceneName
+ << "' from MemoryX to ArMem.";
}
else
{
@@ -315,14 +330,17 @@ namespace armarx::nav
}
- void GraphImportExport::graphArmemToMemoryx(const std::string& sceneName)
+ void
+ GraphImportExport::graphArmemToMemoryx(const std::string& sceneName)
{
ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
- (void) sceneName;
+ (void)sceneName;
}
- void GraphImportExport::clearArMemProviderSegment(armem::client::Writer& writer, const armem::MemoryID& providerSegmentID)
+ void
+ GraphImportExport::clearArMemProviderSegment(armem::client::Writer& writer,
+ const armem::MemoryID& providerSegmentID)
{
const bool clearWhenExists = true;
auto result = writer.addSegment(providerSegmentID, clearWhenExists);
@@ -337,25 +355,30 @@ namespace armarx::nav
}
- armem::MemoryID GraphImportExport::getLocationProviderSegmentID()
+ armem::MemoryID
+ GraphImportExport::getLocationProviderSegmentID()
{
- return properties.locationCoreSegmentID.withProviderSegmentName(tab.providerSegmentLine.getValue());
+ return properties.locationCoreSegmentID.withProviderSegmentName(
+ tab.providerSegmentLine.getValue());
}
- armem::MemoryID GraphImportExport::getGraphProviderSegmentID()
+ armem::MemoryID
+ GraphImportExport::getGraphProviderSegmentID()
{
- return properties.graphCoreSegmentID.withProviderSegmentName(tab.providerSegmentLine.getValue());
+ return properties.graphCoreSegmentID.withProviderSegmentName(
+ tab.providerSegmentLine.getValue());
}
- nav::graph::Graph GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
+ navigation::graph::Graph
+ GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
{
- nav::graph::Graph graph;
- std::map<std::string, nav::graph::Graph::Vertex> vertexMap;
+ navigation::graph::Graph graph;
+ std::map<std::string, navigation::graph::Graph::Vertex> vertexMap;
// Add nodes
- semrel::ShapeID nextVertexID { 0 };
+ semrel::ShapeID nextVertexID{0};
for (memoryx::GraphNodeBasePtr node : graphNodes)
{
ARMARX_CHECK_NOT_NULL(node);
@@ -364,14 +387,17 @@ namespace armarx::nav
// This is the readable name entered in the GUI.
const std::string name = node->getName();
- FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(proxies.graphNodePoseResolver->resolveToGlobalPose(node));
+ FramedPosePtr globalNodePose = FramedPosePtr::dynamicCast(
+ proxies.graphNodePoseResolver->resolveToGlobalPose(node));
ARMARX_CHECK_NOT_NULL(globalNodePose);
ARMARX_VERBOSE << "\n- Adding node: \t" << name;
- nav::graph::Graph::Vertex& vertex = vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
+ navigation::graph::Graph::Vertex& vertex =
+ vertexMap.emplace(name, graph.addVertex(nextVertexID)).first->second;
vertex.attrib().aron.vertexID = static_cast<long>(nextVertexID);
- toAron(vertex.attrib().aron.locationID, getLocationProviderSegmentID().withEntityName(name));
+ toAron(vertex.attrib().aron.locationID,
+ getLocationProviderSegmentID().withEntityName(name));
vertex.attrib().setPose(globalNodePose->toEigen());
nextVertexID++;
@@ -384,18 +410,22 @@ namespace armarx::nav
const auto& sourceVertex = vertexMap.at(node->getName());
for (int i = 0; i < node->getOutdegree(); i++)
{
- auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
+ auto adjacent =
+ memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
ARMARX_CHECK_NOT_NULL(adjacent);
const auto& targetVertex = vertexMap.at(adjacent->getName());
- ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> " << adjacent->getName();
- nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
- edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
- edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> "
+ << adjacent->getName();
+ navigation::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
+ edge.attrib().aron.sourceVertexID =
+ static_cast<long>(sourceVertex.attrib().aron.vertexID);
+ edge.attrib().aron.targetVertexID =
+ static_cast<long>(targetVertex.attrib().aron.vertexID);
}
}
return graph;
}
-}
+} // namespace armarx::nav
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index 949ad673..1f2a9758 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -22,21 +22,20 @@
#pragma once
-#include <armarx/navigation/graph/forward_declarations.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <MemoryX/interface/memorytypes/MemoryEntities.h>
-#include <MemoryX/interface/memorytypes/MemorySegments.h>
-#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
-#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystemComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
-#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-
-#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <ArmarXCore/core/Component.h>
+#include <MemoryX/interface/components/GraphNodePoseResolverInterface.h>
+#include <MemoryX/interface/components/PriorKnowledgeInterface.h>
+#include <MemoryX/interface/memorytypes/MemoryEntities.h>
+#include <MemoryX/interface/memorytypes/MemorySegments.h>
+#include <armarx/navigation/graph/forward_declarations.h>
namespace armarx::nav
@@ -54,14 +53,13 @@ namespace armarx::nav
* Detailed description of class GraphImportExport.
*/
class GraphImportExport :
- virtual public armarx::Component
- , virtual public armarx::DebugObserverComponentPluginUser
- , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- , virtual public armarx::ArVizComponentPluginUser
- , virtual public armarx::armem::client::MemoryNameSystemComponentPluginUser
+ virtual public armarx::Component,
+ virtual public armarx::DebugObserverComponentPluginUser,
+ virtual public armarx::LightweightRemoteGuiComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
+ virtual public armarx::armem::client::MemoryNameSystemComponentPluginUser
{
public:
-
GraphImportExport();
@@ -70,7 +68,6 @@ namespace armarx::nav
protected:
-
/// @see PropertyUser::createPropertyDefinitions()
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
@@ -98,7 +95,6 @@ namespace armarx::nav
private:
-
void refreshScenes();
void locationsMemoryxToArmem(const std::string& sceneName);
@@ -106,16 +102,16 @@ namespace armarx::nav
void graphMemoryxToArmem(const std::string& sceneName);
void graphArmemToMemoryx(const std::string& sceneName);
- void clearArMemProviderSegment(armem::client::Writer& writer, const armem::MemoryID& providerSegmentID);
+ void clearArMemProviderSegment(armem::client::Writer& writer,
+ const armem::MemoryID& providerSegmentID);
armem::MemoryID getLocationProviderSegmentID();
armem::MemoryID getGraphProviderSegmentID();
- nav::graph::Graph toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes);
+ navigation::graph::Graph toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes);
private:
-
struct Proxies
{
memoryx::PriorKnowledgeInterfacePrx priorKnowledge;
@@ -157,6 +153,5 @@ namespace armarx::nav
armarx::RemoteGui::Client::CheckBox visuEnabled;
};
RemoteGuiTab tab;
-
};
-}
+} // namespace armarx::nav
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 8427c268..2d2bd9cc 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -27,8 +27,8 @@ armarx_add_component(NavigationMemory
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## NavigationMemoryInterfaces # If you defined a component ice interface above.
- Navigation::Graph
- Navigation::Location
+ armarx_navigation::graph
+ armarx_navigation::location
SOURCES
NavigationMemory.cpp
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index cf3dd696..3858e9db 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -23,10 +23,11 @@
#include "NavigationMemory.h"
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include "Visu.h"
-#include <Navigation/libraries/core/aron/Trajectory.aron.generated.h>
-#include <Navigation/libraries/core/aron/Twist.aron.generated.h>
+#include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
+#include <armarx/navigation/core/aron/Twist.aron.generated.h>
#include <armarx/navigation/graph/Graph.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
@@ -34,9 +35,12 @@
#include <armarx/navigation/location/constants.h>
-namespace armarx::nav
+namespace armarx::navigation
{
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(NavigationMemory);
+
+
armarx::PropertyDefinitionsPtr
NavigationMemory::createPropertyDefinitions()
{
@@ -81,22 +85,22 @@ namespace armarx::nav
workingMemory.addCoreSegment("Parameterization");
workingMemory.addCoreSegment("Results_GlobalPlanner",
- nav::core::arondto::Trajectory::toAronType());
+ navigation::core::arondto::Trajectory::toAronType());
workingMemory.addCoreSegment("Results_LocalPlanner",
- nav::core::arondto::Trajectory::toAronType());
+ navigation::core::arondto::Trajectory::toAronType());
workingMemory.addCoreSegment("Results_TrajectoryController",
- nav::core::arondto::Twist::toAronType());
+ navigation::core::arondto::Twist::toAronType());
workingMemory.addCoreSegment("Results_SafetyController",
- nav::core::arondto::Twist::toAronType());
+ navigation::core::arondto::Twist::toAronType());
workingMemory.addCoreSegment("Events"); //, armem::example::ExampleData::toAronType());
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::toAronType());
- workingMemory.addCoreSegment(nav::loc::coreSegmentID.coreSegmentName,
- nav::loc::arondto::Location::toAronType());
- workingMemory.addCoreSegment(nav::graph::coreSegmentID.coreSegmentName,
- nav::graph::arondto::Graph::toAronType());
+ workingMemory.addCoreSegment(navigation::location::coreSegmentID.coreSegmentName,
+ navigation::location::arondto::Location::toAronType());
+ workingMemory.addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
+ navigation::graph::arondto::Graph::toAronType());
}
@@ -206,8 +210,8 @@ namespace armarx::nav
NavigationMemory::visuRun()
{
memory::Visu visu{arviz,
- workingMemory.getCoreSegment(nav::loc::coreSegmentID),
- workingMemory.getCoreSegment(nav::graph::coreSegmentID)};
+ workingMemory.getCoreSegment(navigation::location::coreSegmentID),
+ workingMemory.getCoreSegment(navigation::graph::coreSegmentID)};
Properties::LocationGraph p;
{
@@ -238,4 +242,4 @@ namespace armarx::nav
}
-} // namespace armarx::nav
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index 8c9d763b..8c971578 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -22,6 +22,10 @@
#pragma once
+#include <mutex>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -29,13 +33,9 @@
#include <RobotAPI/libraries/armem/server/ComponentPlugin.h>
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-#include <mutex>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-namespace armarx::nav
+namespace armarx::navigation
{
/**
@@ -130,4 +130,4 @@ namespace armarx::nav
Tasks tasks;
};
-} // namespace armarx::nav
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
index 78a75005..244db064 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -30,7 +30,7 @@
#include <armarx/navigation/location/aron/Location.aron.generated.h>
-namespace armarx::nav::memory
+namespace armarx::navigation::memory
{
Visu::Visu(viz::Client arviz,
@@ -57,7 +57,7 @@ namespace armarx::nav::memory
viz::Layer& layer = layers.emplace_back(arviz.layer(locSegment.id().str()));
if (enabled)
{
- std::map<armem::MemoryID, loc::arondto::Location> locations;
+ std::map<armem::MemoryID, location::arondto::Location> locations;
locSegment.doLocked(
[&]()
{
@@ -96,7 +96,7 @@ namespace armarx::nav::memory
{
if (const wm::EntityInstance* instance = entity.findLatestInstance())
{
- nav::graph::arondto::Graph aron;
+ navigation::graph::arondto::Graph aron;
aron.fromAron(instance->data());
fromAron(aron, graph);
}
@@ -117,4 +117,4 @@ namespace armarx::nav::memory
}
}
-} // namespace armarx::nav::memory
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
index 851ea493..f28fa6d8 100644
--- a/source/armarx/navigation/components/NavigationMemory/Visu.h
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -29,11 +29,12 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
class GraphVisu;
}
-namespace armarx::nav::memory
+
+namespace armarx::navigation::memory
{
class Visu
@@ -55,8 +56,8 @@ namespace armarx::nav::memory
const armem::server::wm::CoreSegment& locSegment;
const armem::server::wm::CoreSegment& graphSegment;
- std::unique_ptr<graph::GraphVisu> visu;
+ std::unique_ptr<navigation::graph::GraphVisu> visu;
};
-} // namespace armarx::nav::memory
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index bd86572b..26eaba1f 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -41,6 +41,7 @@
#include <ArmarXCore/core/logging/LogSender.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
#include "ArmarXGui/libraries/RemoteGui/Client/Widgets.h"
@@ -56,314 +57,330 @@
#include <armarx/navigation/server/introspection/MemoryIntrospector.h>
#include <armarx/navigation/util/util.h>
-std::vector<armarx::navigation::core::Pose>
-convert(const std::vector<Eigen::Matrix4f>& wps)
+
+namespace armarx::navigation::components
{
- using namespace armarx::navigation;
- std::vector<core::Pose> p;
- p.reserve(wps.size());
- std::transform(
- wps.begin(), wps.end(), std::back_inserter(p), [](const auto& p) { return core::Pose(p); });
- return p;
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(Navigator);
}
-armarx::navigation::components::Navigator::Navigator() :
- parameterizationReader(memoryNameSystem),
- parameterizationWriter(memoryNameSystem),
- eventsWriter(memoryNameSystem),
- resultsWriter(memoryNameSystem),
- parameterizationService(¶meterizationReader, ¶meterizationWriter),
- publisher(&resultsWriter, &eventsWriter)
+namespace armarx::navigation
{
- scene.timeServer = &timeServer;
-}
+ std::vector<core::Pose>
+ convert(const std::vector<Eigen::Matrix4f>& wps)
+ {
+ using namespace armarx::navigation;
+ std::vector<core::Pose> p;
+ p.reserve(wps.size());
+ std::transform(wps.begin(),
+ wps.end(),
+ std::back_inserter(p),
+ [](const auto& p) { return core::Pose(p); });
+ return p;
+ }
-armarx::navigation::components::Navigator::~Navigator() = default;
+} // namespace armarx::navigation
-void
-armarx::navigation::components::Navigator::onInitComponent()
+namespace armarx::navigation::components
{
-}
-void
-armarx::navigation::components::Navigator::onConnectComponent()
-{
- static bool initialized{false};
-
- // redirect to onReconnect to avoid any setup issues
- if (initialized)
+ components::Navigator::Navigator() :
+ parameterizationReader(memoryNameSystem),
+ parameterizationWriter(memoryNameSystem),
+ eventsWriter(memoryNameSystem),
+ resultsWriter(memoryNameSystem),
+ parameterizationService(¶meterizationReader, ¶meterizationWriter),
+ publisher(&resultsWriter, &eventsWriter)
{
- ARMARX_INFO << "Reconnecting ...";
- onReconnectComponent();
- return;
+ scene.timeServer = &timeServer;
}
- // TODO check if reconnecting
- scene.robot = getRobot();
- scene.staticScene = staticScene();
+ components::Navigator::~Navigator() = default;
- executor = server::PlatformUnitExecutor(platformUnit);
+ void
+ components::Navigator::onInitComponent()
+ {
+ }
- introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
+ void
+ components::Navigator::onConnectComponent()
+ {
+ static bool initialized{false};
- // TODO dynamic scene
- // TODO memory
- // TODO param (10)
- resultsWriter.connect();
- eventsWriter.connect();
- // parameterizationReader.connect();
- parameterizationWriter.connect();
+ // redirect to onReconnect to avoid any setup issues
+ if (initialized)
+ {
+ ARMARX_INFO << "Reconnecting ...";
+ onReconnectComponent();
+ return;
+ }
+ // TODO check if reconnecting
- robotStateUpdateTask = new PeriodicTask<Navigator>(
- this, &Navigator::updateRobot, 10, false, "RobotStateUpdateTask");
- robotStateUpdateTask->start();
+ scene.robot = getRobot();
+ scene.staticScene = staticScene();
- navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
- navRemoteGui->enable();
+ executor = server::PlatformUnitExecutor(platformUnit);
+ introspector = server::ArvizIntrospector(getArvizClient(), scene.robot);
- initialized = true;
-}
+ // TODO dynamic scene
+ // TODO memory
+ // TODO param (10)
+ resultsWriter.connect();
+ eventsWriter.connect();
+ // parameterizationReader.connect();
+ parameterizationWriter.connect();
-void
-armarx::navigation::components::Navigator::onReconnectComponent()
-{
- robotStateUpdateTask->start();
- // TODO not in all cases meaningful
- //resume();
- resultsWriter.connect();
- eventsWriter.connect();
- // parameterizationReader.connect();
- parameterizationWriter.connect();
+ robotStateUpdateTask = new PeriodicTask<Navigator>(
+ this, &Navigator::updateRobot, 10, false, "RobotStateUpdateTask");
+ robotStateUpdateTask->start();
- navRemoteGui->enable();
-}
+ navRemoteGui = std::make_unique<NavigatorRemoteGui>(remoteGui, *this);
+ navRemoteGui->enable();
-void
-armarx::navigation::components::Navigator::onDisconnectComponent()
-{
- robotStateUpdateTask->stop();
- stopAll();
+ initialized = true;
+ }
- navRemoteGui->disable();
-}
+ void
+ components::Navigator::onReconnectComponent()
+ {
+ robotStateUpdateTask->start();
-void
-armarx::navigation::components::Navigator::onExitComponent()
-{
-}
+ // TODO not in all cases meaningful
+ //resume();
+ resultsWriter.connect();
+ eventsWriter.connect();
+ // parameterizationReader.connect();
+ parameterizationWriter.connect();
-void
-armarx::navigation::components::Navigator::updateContext()
-{
- scene.robot = getRobot();
- scene.staticScene = staticScene();
- // TODO dynamic scene
-}
+ navRemoteGui->enable();
+ }
-std::string
-armarx::navigation::components::Navigator::getDefaultName() const
-{
- return "Navigator";
-}
+ void
+ components::Navigator::onDisconnectComponent()
+ {
+ robotStateUpdateTask->stop();
-void
-armarx::navigation::components::Navigator::createConfig(const aron::data::AronDictPtr& stackConfig,
- const std::string& callerId,
- const Ice::Current&)
-{
- // TODO: Not thread-safe.
- ARMARX_TRACE;
- ARMARX_INFO << "Creating config for caller '" << callerId << "'";
-
- parameterizationService.store(stackConfig, callerId, timeServer.now());
-
- server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
-
- memoryIntrospectors.emplace_back(
- std::make_unique<server::MemoryIntrospector>(resultsWriter, callerId));
-
- navigators.emplace(
- std::piecewise_construct,
- std::forward_as_tuple(callerId),
- std::forward_as_tuple(
- server::Navigator::Config{.stack = std::move(stack),
- .scene = &scene,
- .general = server::Navigator::Config::General{}},
- server::Navigator::InjectedServices{.executor = &executor.value(),
- .publisher = &publisher,
- .introspector = &(introspector.value())}));
-}
+ stopAll();
-void
-armarx::navigation::components::Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current&)
-{
+ navRemoteGui->disable();
+ }
- ARMARX_INFO << "moveTo() requested by caller '" << callerId << "'";
+ void
+ components::Navigator::onExitComponent()
+ {
+ }
- try
+ void
+ components::Navigator::updateContext()
{
- navigators.at(callerId).moveTo(convert(waypoints),
- core::NavigationFrameNames.from_name(navigationMode));
+ scene.robot = getRobot();
+ scene.staticScene = staticScene();
+ // TODO dynamic scene
}
- catch (...)
+
+ std::string
+ components::Navigator::getDefaultName() const
{
- // TODO: Error handling.
- ARMARX_WARNING << "Failed to execute moveTo()."
- << "Movement will be paused.";
- stopAll(); // TODO pause movement must not throw
- throw;
+ return "Navigator";
}
-}
-void
-armarx::navigation::components::Navigator::moveTowards(const Eigen::Vector3f& direction,
- const std::string& navigationMode,
- const std::string& callerId,
- const Ice::Current&)
-{
- // TODO: Error handling.
+ void
+ components::Navigator::createConfig(const aron::data::AronDictPtr& stackConfig,
+ const std::string& callerId,
+ const Ice::Current&)
+ {
+ // TODO: Not thread-safe.
+ ARMARX_TRACE;
+ ARMARX_INFO << "Creating config for caller '" << callerId << "'";
+
+ parameterizationService.store(stackConfig, callerId, timeServer.now());
+
+ server::NavigationStack stack = fac::NavigationStackFactory::create(stackConfig, scene);
+
+ memoryIntrospectors.emplace_back(
+ std::make_unique<server::MemoryIntrospector>(resultsWriter, callerId));
+
+ navigators.emplace(
+ std::piecewise_construct,
+ std::forward_as_tuple(callerId),
+ std::forward_as_tuple(
+ server::Navigator::Config{.stack = std::move(stack),
+ .scene = &scene,
+ .general = server::Navigator::Config::General{}},
+ server::Navigator::InjectedServices{.executor = &executor.value(),
+ .publisher = &publisher,
+ .introspector = &(introspector.value())}));
+ }
- ARMARX_INFO << "MoveTowards requested by caller '" << callerId << "'";
+ void
+ components::Navigator::moveTo(const std::vector<Eigen::Matrix4f>& waypoints,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current&)
+ {
- navigators.at(callerId).moveTowards(direction,
- core::NavigationFrameNames.from_name(navigationMode));
-}
+ ARMARX_INFO << "moveTo() requested by caller '" << callerId << "'";
+
+ try
+ {
+ navigators.at(callerId).moveTo(convert(waypoints),
+ core::NavigationFrameNames.from_name(navigationMode));
+ }
+ catch (...)
+ {
+ // TODO: Error handling.
+ ARMARX_WARNING << "Failed to execute moveTo()."
+ << "Movement will be paused.";
+ stopAll(); // TODO pause movement must not throw
+ throw;
+ }
+ }
-void
-armarx::navigation::components::Navigator::pause(const std::string& configId, const Ice::Current&)
-{
- navigators.at(configId).pause();
-}
+ void
+ components::Navigator::moveTowards(const Eigen::Vector3f& direction,
+ const std::string& navigationMode,
+ const std::string& callerId,
+ const Ice::Current&)
+ {
+ // TODO: Error handling.
-void
-armarx::navigation::components::Navigator::resume(const std::string& configId, const Ice::Current&)
-{
- navigators.at(configId).resume();
-}
+ ARMARX_INFO << "MoveTowards requested by caller '" << callerId << "'";
-void
-armarx::navigation::components::Navigator::stop(const std::string& configId, const Ice::Current&)
-{
- navigators.at(configId).stop();
-}
+ navigators.at(callerId).moveTowards(direction,
+ core::NavigationFrameNames.from_name(navigationMode));
+ }
-void
-armarx::navigation::components::Navigator::stopAll(const Ice::Current&)
-{
- for (auto& [_, navigator] : navigators)
+ void
+ components::Navigator::pause(const std::string& configId, const Ice::Current&)
{
- navigator.stop();
+ navigators.at(configId).pause();
}
-}
-bool
-armarx::navigation::components::Navigator::isPaused(const std::string& configId,
- const Ice::Current&)
-{
- return navigators.at(configId).isPaused();
-}
+ void
+ components::Navigator::resume(const std::string& configId, const Ice::Current&)
+ {
+ navigators.at(configId).resume();
+ }
-bool
-armarx::navigation::components::Navigator::isStopped(const std::string& configId,
- const Ice::Current&)
-{
- return navigators.at(configId).isStopped();
-}
+ void
+ components::Navigator::stop(const std::string& configId, const Ice::Current&)
+ {
+ navigators.at(configId).stop();
+ }
-armarx::PropertyDefinitionsPtr
-armarx::navigation::components::Navigator::createPropertyDefinitions()
-{
- ARMARX_TRACE;
+ void
+ components::Navigator::stopAll(const Ice::Current&)
+ {
+ for (auto& [_, navigator] : navigators)
+ {
+ navigator.stop();
+ }
+ }
+
+ bool
+ components::Navigator::isPaused(const std::string& configId, const Ice::Current&)
+ {
+ return navigators.at(configId).isPaused();
+ }
- PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
+ bool
+ components::Navigator::isStopped(const std::string& configId, const Ice::Current&)
+ {
+ return navigators.at(configId).isStopped();
+ }
- // Publish to a topic (passing the TopicListenerPrx).
- // def->topic(myTopicListener);
+ armarx::PropertyDefinitionsPtr
+ components::Navigator::createPropertyDefinitions()
+ {
+ ARMARX_TRACE;
- // Subscribe to a topic (passing the topic name).
- // def->topic<PlatformUnitListener>("MyTopic");
+ PropertyDefinitionsPtr def = new ComponentPropertyDefinitions(getConfigIdentifier());
- // Use (and depend on) another component (passing the ComponentInterfacePrx).
- def->component(platformUnit);
+ // Publish to a topic (passing the TopicListenerPrx).
+ // def->topic(myTopicListener);
- def->component(remoteGui, "RemoteGuiProvider");
- // Add a required property.
- // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+ // Subscribe to a topic (passing the topic name).
+ // def->topic<PlatformUnitListener>("MyTopic");
- // Add an optionalproperty.
- // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+ // Use (and depend on) another component (passing the ComponentInterfacePrx).
+ def->component(platformUnit);
- resultsWriter.registerPropertyDefinitions(def);
- eventsWriter.registerPropertyDefinitions(def);
- // parameterizationReader.registerPropertyDefinitions(def);
- parameterizationWriter.registerPropertyDefinitions(def);
+ def->component(remoteGui, "RemoteGuiProvider");
+ // Add a required property.
+ // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
- return def;
-}
+ // Add an optionalproperty.
+ // def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
-armarx::navigation::core::StaticScene
-armarx::navigation::components::Navigator::staticScene()
-{
- ARMARX_TRACE;
+ resultsWriter.registerPropertyDefinitions(def);
+ eventsWriter.registerPropertyDefinitions(def);
+ // parameterizationReader.registerPropertyDefinitions(def);
+ parameterizationWriter.registerPropertyDefinitions(def);
- const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
- core::StaticScene scene{.objects = util::asSceneObjects(objectPoses)};
+ return def;
+ }
- ARMARX_INFO << "The scene consists of " << scene.objects->getSize() << " objects";
+ core::StaticScene
+ components::Navigator::staticScene()
+ {
+ ARMARX_TRACE;
- return scene;
-}
+ const objpose::ObjectPoseSeq objectPoses = ObjectPoseClientPluginUser::getObjectPoses();
+ core::StaticScene scene{.objects = util::asSceneObjects(objectPoses)};
-VirtualRobot::RobotPtr
-armarx::navigation::components::Navigator::getRobot()
-{
- ARMARX_TRACE;
+ ARMARX_INFO << "The scene consists of " << scene.objects->getSize() << " objects";
- auto robot = RemoteRobot::createLocalCloneFromFile(
- getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
- // auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
- ARMARX_CHECK_NOT_NULL(robot);
+ return scene;
+ }
- // ARMARX_INFO << "Resetting robot";
- // robot = VirtualRobot::RobotIO::loadRobot(robot->getFilename());
+ VirtualRobot::RobotPtr
+ components::Navigator::getRobot()
+ {
+ ARMARX_TRACE;
- RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
+ auto robot = RemoteRobot::createLocalCloneFromFile(
+ getRobotStateComponent(), VirtualRobot::RobotIO::RobotDescription::eFull);
+ // auto robot = RemoteRobot::createLocalClone(getRobotStateComponent());
+ ARMARX_CHECK_NOT_NULL(robot);
- return robot;
-}
+ // ARMARX_INFO << "Resetting robot";
+ // robot = VirtualRobot::RobotIO::loadRobot(robot->getFilename());
-void
-armarx::navigation::components::Navigator::updateRobot()
-{
- synchronizeLocalClone(scene.robot);
-}
+ RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
-armarx::navigation::server::Navigator*
-armarx::navigation::components::Navigator::activeNavigator()
-{
- // We define the active navigator to be the one whose movement is enabled.
- const auto isActive = [](auto& nameNavPair) -> bool
- {
- server::Navigator& navigator = nameNavPair.second;
- return not navigator.isPaused();
- };
+ return robot;
+ }
- auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
+ void
+ components::Navigator::updateRobot()
+ {
+ synchronizeLocalClone(scene.robot);
+ }
- // no navigator active?
- if (it == navigators.end())
+ server::Navigator*
+ components::Navigator::activeNavigator()
{
- return nullptr;
+ // We define the active navigator to be the one whose movement is enabled.
+ const auto isActive = [](auto& nameNavPair) -> bool
+ {
+ server::Navigator& navigator = nameNavPair.second;
+ return not navigator.isPaused();
+ };
+
+ auto it = std::find_if(navigators.begin(), navigators.end(), isActive);
+
+ // no navigator active?
+ if (it == navigators.end())
+ {
+ return nullptr;
+ }
+
+ return &it->second;
}
- return &it->second;
-}
+} // namespace armarx::navigation::components
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index a53eb52b..261213e9 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -1,11 +1,12 @@
-set(LIB_NAME ${PROJECT_NAME}Graph)
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
+armarx_add_aron_library(graph_aron
+ ARON_FILES
+ aron/Graph.xml
+)
-armarx_add_library(
- LIBS
+armarx_add_library(graph
+ DEPENDENCIES
# ArmarXCore
ArmarXCoreInterfaces
ArmarXCore
@@ -13,41 +14,19 @@ armarx_add_library(
# RobotAPI
aron
armem
-
+ # SemanticObjectRelations
+ SemanticObjectRelations
+ # this package
+ armarx_navigation::graph_aron
+ DEPENDENCIES_LEGACY
+ VTK
SOURCES
constants.cpp
Graph.cpp
Visu.cpp
-
HEADERS
constants.h
forward_declarations.h
Graph.h
Visu.h
)
-
-
-add_library(
- ${PROJECT_NAME}::Graph
- ALIAS
- "${LIB_NAME}"
-)
-
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/Graph.xml
-)
-
-
-find_package(VTK QUIET)
-armarx_build_if(VTK_FOUND "VTK not available")
-
-find_package(SemanticObjectRelations QUIET)
-armarx_build_if(SemanticObjectRelations_FOUND "SemanticObjectRelations not available")
-if(SemanticObjectRelations_FOUND)
- target_link_libraries(${LIB_NAME} PUBLIC SemanticObjectRelations)
- # target_include_directories(${LIB_NAME} PUBLIC SemanticObjectRelations)
-endif()
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
index 31f6d056..db69eabb 100644
--- a/source/armarx/navigation/graph/Graph.cpp
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -21,51 +21,55 @@
#include "Graph.h"
-#include <armarx/navigation/location/aron/Location.aron.generated.h>
-
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
- std::string VertexAttribs::getName() const
+ std::string
+ VertexAttribs::getName() const
{
return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
}
- bool VertexAttribs::hasPose() const
+ bool
+ VertexAttribs::hasPose() const
{
return _pose.has_value();
}
- Eigen::Matrix4f VertexAttribs::getPose() const
+ Eigen::Matrix4f
+ VertexAttribs::getPose() const
{
ARMARX_CHECK(_pose.has_value());
return _pose.value();
}
- void VertexAttribs::setPose(const Eigen::Matrix4f& pose)
+ void
+ VertexAttribs::setPose(const Eigen::Matrix4f& pose)
{
this->_pose = pose;
}
-}
+} // namespace armarx::navigation::graph
-namespace armarx::nav
+namespace armarx::navigation
{
-void graph::toAron(arondto::Graph& dto, const Graph& bo)
-{
- dto = {};
- for (auto vertex : bo.vertices())
+ void
+ graph::toAron(arondto::Graph& dto, const Graph& bo)
{
- auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
- v.vertexID = static_cast<long>(vertex.objectID());
+ dto = {};
+ for (auto vertex : bo.vertices())
+ {
+ auto& v = dto.vertices.emplace_back(vertex.attrib().aron);
+ v.vertexID = static_cast<long>(vertex.objectID());
}
ARMARX_CHECK_EQUAL(dto.vertices.size(), bo.numVertices());
@@ -79,7 +83,8 @@ void graph::toAron(arondto::Graph& dto, const Graph& bo)
}
- void graph::fromAron(const arondto::Graph& dto, Graph& bo)
+ void
+ graph::fromAron(const arondto::Graph& dto, Graph& bo)
{
bo = {};
for (const arondto::Vertex& vertex : dto.vertices)
@@ -91,19 +96,22 @@ void graph::toAron(arondto::Graph& dto, const Graph& bo)
for (const arondto::Edge& edge : dto.edges)
{
- auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID), semrel::ShapeID(edge.targetVertexID));
+ auto e = bo.addEdge(semrel::ShapeID(edge.sourceVertexID),
+ semrel::ShapeID(edge.targetVertexID));
e.attrib().aron = edge;
}
ARMARX_CHECK_EQUAL(bo.numEdges(), dto.edges.size());
}
- void graph::resolveLocation(Graph::Vertex& vertex, const aron::datanavigator::DictNavigatorPtr& locationData)
+ void
+ graph::resolveLocation(Graph::Vertex& vertex,
+ const aron::datanavigator::DictNavigatorPtr& locationData)
{
- nav::loc::arondto::Location dto;
+ navigation::location::arondto::Location dto;
dto.fromAron(locationData);
vertex.attrib().setPose(dto.globalRobotPose);
}
-}
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/graph/Graph.h b/source/armarx/navigation/graph/Graph.h
index be40293d..388d4eae 100644
--- a/source/armarx/navigation/graph/Graph.h
+++ b/source/armarx/navigation/graph/Graph.h
@@ -21,21 +21,20 @@
#pragma once
-#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
-
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
struct VertexAttribs : public semrel::ShapeVertex
{
- armarx::nav::graph::arondto::Vertex aron;
+ armarx::navigation::graph::arondto::Vertex aron;
std::string getName() const;
@@ -44,14 +43,12 @@ namespace armarx::nav::graph
void setPose(const Eigen::Matrix4f& pose);
private:
-
std::optional<Eigen::Matrix4f> _pose;
-
};
struct EdgeAttribs
{
- armarx::nav::graph::arondto::Edge aron;
+ armarx::navigation::graph::arondto::Edge aron;
};
struct GraphAttribs
@@ -65,14 +62,15 @@ namespace armarx::nav::graph
void fromAron(const arondto::Graph& dto, Graph& bo);
-
// Location resolution
- void resolveLocation(Graph::Vertex& vertex, const aron::datanavigator::DictNavigatorPtr& locationData);
+ void resolveLocation(Graph::Vertex& vertex,
+ const aron::datanavigator::DictNavigatorPtr& locationData);
template <class MemoryContainerT>
- bool resolveLocation(Graph::Vertex& vertex, const MemoryContainerT& locationContainer)
+ bool
+ resolveLocation(Graph::Vertex& vertex, const MemoryContainerT& locationContainer)
{
armem::MemoryID locationID;
fromAron(vertex.attrib().aron.locationID, locationID);
@@ -90,7 +88,8 @@ namespace armarx::nav::graph
template <class MemoryContainerT>
- void resolveLocations(Graph& graph, const MemoryContainerT& locationContainer)
+ void
+ resolveLocations(Graph& graph, const MemoryContainerT& locationContainer)
{
for (graph::Graph::Vertex vertex : graph.vertices())
{
@@ -98,4 +97,4 @@ namespace armarx::nav::graph
}
}
-}
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
index 6eb4c17b..38b8a081 100644
--- a/source/armarx/navigation/graph/Visu.cpp
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -19,59 +19,65 @@
* GNU General Public License
*/
-#include <VirtualRobot/VirtualRobot.h>
-
#include "Visu.h"
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/math/pose.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
- viz::Pose VertexVisu::Pose::draw(const VertexAttribs& attribs) const
+ viz::Pose
+ VertexVisu::Pose::draw(const VertexAttribs& attribs) const
{
return draw(attribs.getName(), attribs.getPose());
}
- viz::Pose VertexVisu::Pose::draw(const std::string& name, const Eigen::Matrix4f& pose) const
+ viz::Pose
+ VertexVisu::Pose::draw(const std::string& name, const Eigen::Matrix4f& pose) const
{
return viz::Pose(name).pose(pose).scale(scale);
}
- viz::Arrow VertexVisu::ForwardArrow::draw(const VertexAttribs& attribs) const
+ viz::Arrow
+ VertexVisu::ForwardArrow::draw(const VertexAttribs& attribs) const
{
return draw(attribs.getName(), attribs.getPose());
}
- viz::Arrow VertexVisu::ForwardArrow::draw(const std::string& name, const Eigen::Matrix4f& pose) const
+ viz::Arrow
+ VertexVisu::ForwardArrow::draw(const std::string& name, const Eigen::Matrix4f& pose) const
{
return viz::Arrow(name + " forward")
- .fromTo(simox::math::position(pose),
- simox::math::transform_position(pose, length * Eigen::Vector3f::UnitY()))
- .color(color)
- .width(width);
+ .fromTo(simox::math::position(pose),
+ simox::math::transform_position(pose, length * Eigen::Vector3f::UnitY()))
+ .color(color)
+ .width(width);
}
- void VertexVisu::draw(viz::Layer& layer, Graph::ConstVertex vertex) const
+ void
+ VertexVisu::draw(viz::Layer& layer, Graph::ConstVertex vertex) const
{
draw(layer, vertex.attrib());
}
- void VertexVisu::draw(viz::Layer& layer, const VertexAttribs& attribs) const
+ void
+ VertexVisu::draw(viz::Layer& layer, const VertexAttribs& attribs) const
{
draw(layer, attribs.getName(), attribs.getPose());
}
- void VertexVisu::draw(viz::Layer& layer, const std::string& name, const Eigen::Matrix4f& pose) const
+ void
+ VertexVisu::draw(viz::Layer& layer, const std::string& name, const Eigen::Matrix4f& pose) const
{
if (this->pose.has_value())
{
@@ -84,27 +90,29 @@ namespace armarx::nav::graph
}
- viz::Arrow EdgeVisu::Arrow::draw(Graph::ConstEdge edge) const
+ viz::Arrow
+ EdgeVisu::Arrow::draw(Graph::ConstEdge edge) const
{
return draw(edge.attrib(), edge.source().attrib(), edge.target().attrib());
}
- viz::Arrow EdgeVisu::Arrow::draw(
- const EdgeAttribs& edge,
- const VertexAttribs& source,
- const VertexAttribs& target) const
+ viz::Arrow
+ EdgeVisu::Arrow::draw(const EdgeAttribs& edge,
+ const VertexAttribs& source,
+ const VertexAttribs& target) const
{
- (void) edge;
+ (void)edge;
return viz::Arrow(source.getName() + " -> " + target.getName())
- .fromTo(simox::math::position(source.getPose()),
- simox::math::position(target.getPose()))
- .width(width)
- .color(color);
+ .fromTo(simox::math::position(source.getPose()),
+ simox::math::position(target.getPose()))
+ .width(width)
+ .color(color);
}
- void EdgeVisu::draw(viz::Layer& layer, Graph::ConstEdge edge) const
+ void
+ EdgeVisu::draw(viz::Layer& layer, Graph::ConstEdge edge) const
{
if (arrow.has_value())
{
@@ -113,7 +121,8 @@ namespace armarx::nav::graph
}
- void GraphVisu::draw(viz::Layer& layer, const Graph& graph) const
+ void
+ GraphVisu::draw(viz::Layer& layer, const Graph& graph) const
{
if (vertex.has_value())
{
@@ -130,4 +139,4 @@ namespace armarx::nav::graph
}
}
}
-}
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
index 90976f01..8d98e616 100644
--- a/source/armarx/navigation/graph/Visu.h
+++ b/source/armarx/navigation/graph/Visu.h
@@ -21,11 +21,11 @@
#pragma once
-#include <armarx/navigation/graph/Graph.h>
+#include <optional>
#include <SimoxUtility/color/Color.h>
-#include <optional>
+#include <armarx/navigation/graph/Graph.h>
namespace armarx::viz
@@ -33,8 +33,8 @@ namespace armarx::viz
class Arrow;
class Layer;
class Pose;
-}
-namespace armarx::nav::graph
+} // namespace armarx::viz
+namespace armarx::navigation::graph
{
struct VertexVisu
@@ -46,7 +46,7 @@ namespace armarx::nav::graph
viz::Pose draw(const VertexAttribs& attribs) const;
viz::Pose draw(const std::string& name, const Eigen::Matrix4f& pose) const;
};
- std::optional<Pose> pose = Pose {};
+ std::optional<Pose> pose = Pose{};
struct ForwardArrow
{
@@ -57,7 +57,7 @@ namespace armarx::nav::graph
viz::Arrow draw(const VertexAttribs& attribs) const;
viz::Arrow draw(const std::string& name, const Eigen::Matrix4f& pose) const;
};
- std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow{};
void draw(viz::Layer& layer, Graph::ConstVertex vertex) const;
@@ -74,9 +74,11 @@ namespace armarx::nav::graph
simox::Color color = simox::Color::azure(196);
viz::Arrow draw(Graph::ConstEdge edge) const;
- viz::Arrow draw(const EdgeAttribs& edge, const VertexAttribs& source, const VertexAttribs& target) const;
+ viz::Arrow draw(const EdgeAttribs& edge,
+ const VertexAttribs& source,
+ const VertexAttribs& target) const;
};
- std::optional<Arrow> arrow = Arrow {};
+ std::optional<Arrow> arrow = Arrow{};
void draw(viz::Layer& layer, Graph::ConstEdge edge) const;
@@ -85,11 +87,11 @@ namespace armarx::nav::graph
struct GraphVisu
{
- std::optional<VertexVisu> vertex = VertexVisu {};
- std::optional<EdgeVisu> edge = EdgeVisu {};
+ std::optional<VertexVisu> vertex = VertexVisu{};
+ std::optional<EdgeVisu> edge = EdgeVisu{};
void draw(viz::Layer& layer, const Graph& graph) const;
};
-}
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
index fe09dcc0..beda2163 100644
--- a/source/armarx/navigation/graph/aron/Graph.xml
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -9,7 +9,7 @@
<GenerateTypes>
- <Object name='armarx::nav::graph::arondto::Vertex'>
+ <Object name='armarx::navigation::graph::arondto::Vertex'>
<ObjectChild key='vertexID'>
<Long />
@@ -27,7 +27,7 @@
</Object>
- <Object name='armarx::nav::graph::arondto::Edge'>
+ <Object name='armarx::navigation::graph::arondto::Edge'>
<ObjectChild key='sourceVertexID'>
<Long />
@@ -40,17 +40,17 @@
</Object>
- <Object name='armarx::nav::graph::arondto::Graph'>
+ <Object name='armarx::navigation::graph::arondto::Graph'>
<ObjectChild key='vertices'>
<List>
- <armarx::nav::graph::arondto::Vertex />
+ <armarx::navigation::graph::arondto::Vertex />
</List>
</ObjectChild>
<ObjectChild key='edges'>
<List>
- <armarx::nav::graph::arondto::Edge />
+ <armarx::navigation::graph::arondto::Edge />
</List>
</ObjectChild>
diff --git a/source/armarx/navigation/graph/constants.cpp b/source/armarx/navigation/graph/constants.cpp
index f862be9c..a87fc00d 100644
--- a/source/armarx/navigation/graph/constants.cpp
+++ b/source/armarx/navigation/graph/constants.cpp
@@ -3,9 +3,9 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-namespace armarx::nav
+namespace armarx::navigation
{
- const armem::MemoryID graph::coreSegmentID { "Navigation", "Graph" };
+ const armem::MemoryID graph::coreSegmentID{"Navigation", "Graph"};
}
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
index ccb1cc7e..f88b3cde 100644
--- a/source/armarx/navigation/graph/constants.h
+++ b/source/armarx/navigation/graph/constants.h
@@ -24,9 +24,10 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
+ // FIXME inline ...
extern const armem::MemoryID coreSegmentID;
-}
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/forward_declarations.h b/source/armarx/navigation/graph/forward_declarations.h
index fad17f2b..2f30f783 100644
--- a/source/armarx/navigation/graph/forward_declarations.h
+++ b/source/armarx/navigation/graph/forward_declarations.h
@@ -27,7 +27,7 @@ namespace semrel
template <class VertexAttribT, class EdgeAttribT, class GraphAttribT>
class RelationGraph;
}
-namespace armarx::nav::graph
+namespace armarx::navigation::graph
{
struct VertexAttribs;
@@ -36,4 +36,4 @@ namespace armarx::nav::graph
using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
-}
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
index 9b927dcf..c60d7f2e 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
@@ -1,13 +1,13 @@
#include "GraphScene.h"
+#include <Eigen/Core>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <SemanticObjectRelations/Shapes/Shape.h>
-#include <Eigen/Core>
-
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
QGraphicsEllipseItem*
@@ -18,13 +18,12 @@ namespace armarx::nav::locgrapheditor
// To capture by copy
semrel::ShapeID vertexID = vertex.objectID();
- GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
- {
+ GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem{
[this, vertexID]() { emit vertexSelected(vertexID); },
static_cast<qreal>(pose(0, 3)),
- static_cast<qreal>(- pose(1, 3)),
- 0, 0
- };
+ static_cast<qreal>(-pose(1, 3)),
+ 0,
+ 0};
addItem(item);
// setToolTip on graphicsItem does not work
@@ -45,25 +44,26 @@ namespace armarx::nav::locgrapheditor
Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
- GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
- {
+ GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem{
[this, sourceID, targetID]() { emit edgeSelected(sourceID, targetID); },
- sourcePose(0, 3), - sourcePose(1, 3),
- targetPose(0, 3), - targetPose(1, 3)
- };
+ sourcePose(0, 3),
+ -sourcePose(1, 3),
+ targetPose(0, 3),
+ -targetPose(1, 3)};
addItem(item);
// setToolTip on item does not work
std::stringstream toolTip;
- toolTip << "Edge '" << edge.source().attrib().getName()
- << "' -> '" << edge.target().attrib().getName();
+ toolTip << "Edge '" << edge.source().attrib().getName() << "' -> '"
+ << edge.target().attrib().getName();
item->setToolTip(QString::fromStdString(toolTip.str()));
return item;
}
- void GraphScene::updateVertex(GuiGraph::Vertex& vertex)
+ void
+ GraphScene::updateVertex(GuiGraph::Vertex& vertex)
{
QGraphicsEllipseItem* item = vertex.attrib().graphicsItem;
ARMARX_CHECK_NOT_NULL(item);
@@ -73,16 +73,17 @@ namespace armarx::nav::locgrapheditor
QColor color = vertex.attrib().highlighted ? colorSelected : colorDefault;
double lineWidth = vertex.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
- item->setPen(QPen {color});
- item->setBrush(QBrush {color});
- item->setRect( pose(0, 3) - lineWidth * verticesScaleFactor / 2,
- - pose(1, 3) - lineWidth * verticesScaleFactor / 2,
+ item->setPen(QPen{color});
+ item->setBrush(QBrush{color});
+ item->setRect(pose(0, 3) - lineWidth * verticesScaleFactor / 2,
+ -pose(1, 3) - lineWidth * verticesScaleFactor / 2,
lineWidth * verticesScaleFactor,
lineWidth * verticesScaleFactor);
}
- void GraphScene::updateEdge(GuiGraph::Edge& edge)
+ void
+ GraphScene::updateEdge(GuiGraph::Edge& edge)
{
QGraphicsLineItem* item = edge.attrib().graphicsItem;
ARMARX_CHECK_NOT_NULL(item);
@@ -93,16 +94,16 @@ namespace armarx::nav::locgrapheditor
QColor color = edge.attrib().highlighted ? colorSelected : colorDefault;
double lineWidth = edge.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
- QPen pen {color};
+ QPen pen{color};
pen.setWidthF(lineWidth * lineScaleFactor);
item->setPen(pen);
- item->setLine(sourcePose(0, 3), - sourcePose(1, 3),
- targetPose(0, 3), - targetPose(1, 3));
+ item->setLine(sourcePose(0, 3), -sourcePose(1, 3), targetPose(0, 3), -targetPose(1, 3));
}
- void GraphScene::removeVertex(QGraphicsEllipseItem*& item)
+ void
+ GraphScene::removeVertex(QGraphicsEllipseItem*& item)
{
removeItem(item);
delete item;
@@ -110,7 +111,8 @@ namespace armarx::nav::locgrapheditor
}
- void GraphScene::removeEdge(QGraphicsLineItem*& item)
+ void
+ GraphScene::removeEdge(QGraphicsLineItem*& item)
{
removeItem(item);
delete item;
@@ -119,12 +121,13 @@ namespace armarx::nav::locgrapheditor
GraphVisualizerGraphicsEllipseItem::GraphVisualizerGraphicsEllipseItem(
- std::function<void(void)> onDoubleClicked,
- qreal x, qreal y,
- qreal width, qreal height,
- QGraphicsItem* parent) :
- QGraphicsEllipseItem {x, y, width, height, parent},
- onDoubleClicked{onDoubleClicked}
+ std::function<void(void)> onDoubleClicked,
+ qreal x,
+ qreal y,
+ qreal width,
+ qreal height,
+ QGraphicsItem* parent) :
+ QGraphicsEllipseItem{x, y, width, height, parent}, onDoubleClicked{onDoubleClicked}
{
}
@@ -134,19 +137,21 @@ namespace armarx::nav::locgrapheditor
}
- void GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ void
+ GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
{
onDoubleClicked();
}
-
GraphVisualizerGraphicsLineItem::GraphVisualizerGraphicsLineItem(
- std::function<void(void)> onDoubleClicked,
- qreal x1, qreal y1, qreal x2, qreal y2,
- QGraphicsItem* parent) :
- QGraphicsLineItem {x1, y1, x2, y2, parent},
- onDoubleClicked{onDoubleClicked}
+ std::function<void(void)> onDoubleClicked,
+ qreal x1,
+ qreal y1,
+ qreal x2,
+ qreal y2,
+ QGraphicsItem* parent) :
+ QGraphicsLineItem{x1, y1, x2, y2, parent}, onDoubleClicked{onDoubleClicked}
{
}
@@ -156,10 +161,10 @@ namespace armarx::nav::locgrapheditor
}
- void GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ void
+ GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
{
onDoubleClicked();
}
-}
-
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
index 099f4158..df113b0f 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -1,21 +1,19 @@
#pragma once
-#include "GuiGraph.h"
-
-#include <armarx/navigation/graph/Graph.h>
-
-#include <QGraphicsScene>
#include <QGraphicsEllipseItem>
#include <QGraphicsLineItem>
-
+#include <QGraphicsScene>
#include <functional>
+#include "GuiGraph.h"
+#include <armarx/navigation/graph/Graph.h>
+
namespace semrel
{
struct ShapeID;
}
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class GraphScene : public QGraphicsScene
@@ -23,7 +21,6 @@ namespace armarx::nav::locgrapheditor
Q_OBJECT
public:
-
using QGraphicsScene::QGraphicsScene;
QGraphicsEllipseItem* addVertex(const graph::Graph::Vertex& vertex);
@@ -46,7 +43,6 @@ namespace armarx::nav::locgrapheditor
public:
-
double lineWidthDefault = 5;
double lineWidthSelected = 10;
@@ -56,11 +52,9 @@ namespace armarx::nav::locgrapheditor
QColor colorDefault = QColor::fromRgb(128, 128, 255);
QColor colorSelected = QColor::fromRgb(255, 128, 0);
-
};
-
/**
* @brief Required to override the double click event.
* This is required to toggle the select state.
@@ -68,26 +62,24 @@ namespace armarx::nav::locgrapheditor
class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
{
public:
-
- GraphVisualizerGraphicsEllipseItem(
- std::function<void(void)> onDoubleClicked,
- qreal x, qreal y, qreal width, qreal height,
- QGraphicsItem* parent = nullptr);
+ GraphVisualizerGraphicsEllipseItem(std::function<void(void)> onDoubleClicked,
+ qreal x,
+ qreal y,
+ qreal width,
+ qreal height,
+ QGraphicsItem* parent = nullptr);
~GraphVisualizerGraphicsEllipseItem() override;
protected:
-
void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
std::function<void(void)> onDoubleClicked;
-
};
-
/**
* @brief Required to override the double click event.
* This is required to toggle the select state.
@@ -95,26 +87,22 @@ namespace armarx::nav::locgrapheditor
class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
{
public:
-
- GraphVisualizerGraphicsLineItem(
- std::function<void(void)> onDoubleClicked,
- qreal x1, qreal y1,
- qreal x2, qreal y2,
- QGraphicsItem* parent = nullptr);
+ GraphVisualizerGraphicsLineItem(std::function<void(void)> onDoubleClicked,
+ qreal x1,
+ qreal y1,
+ qreal x2,
+ qreal y2,
+ QGraphicsItem* parent = nullptr);
~GraphVisualizerGraphicsLineItem() override;
protected:
-
-
void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
std::function<void(void)> onDoubleClicked;
-
};
-}
-
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
index 7cd38cc0..034b0609 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -22,14 +22,15 @@
#include "GuiGraph.h"
-#include <SimoxUtility/math/convert/rad_to_deg.h>
#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
- bool GuiGraph::hasChanged() const
+ bool
+ GuiGraph::hasChanged() const
{
if (attrib().edgesChanged)
{
@@ -92,51 +93,54 @@ namespace armarx::nav::locgrapheditor
return map;
}
-}
+} // namespace armarx::navigation::locgrapheditor
namespace armarx::nav
{
- float locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
+ float
+ locgrapheditor::getYawAngleDegree(const Eigen::Matrix4f& pose)
{
return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(2));
}
- double locgrapheditor::getYawAngleDegree(const Eigen::Matrix4d& pose)
+ double
+ locgrapheditor::getYawAngleDegree(const Eigen::Matrix4d& pose)
{
return simox::math::rad_to_deg(simox::math::mat4f_to_rpy(pose)(2));
}
- auto locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
+ auto
+ locgrapheditor::toGuiGraph(const graph::Graph& nav) -> GuiGraph
{
GuiGraph gui;
for (auto v : nav.vertices())
{
- gui.addVertex(v.objectID(), { v.attrib() });
+ gui.addVertex(v.objectID(), {v.attrib()});
}
for (auto e : nav.edges())
{
- gui.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ gui.addEdge(e.sourceObjectID(), e.targetObjectID(), {e.attrib()});
}
return gui;
}
- nav::graph::Graph locgrapheditor::fromGuiGraph(const GuiGraph& gui)
+ navigation::graph::Graph
+ locgrapheditor::fromGuiGraph(const GuiGraph& gui)
{
graph::Graph nav;
for (auto v : gui.vertices())
{
- nav.addVertex(v.objectID(), { v.attrib() });
+ nav.addVertex(v.objectID(), {v.attrib()});
}
for (auto e : gui.edges())
{
- nav.addEdge(e.sourceObjectID(), e.targetObjectID(), { e.attrib() });
+ nav.addEdge(e.sourceObjectID(), e.targetObjectID(), {e.attrib()});
}
return nav;
}
-}
-
+} // namespace armarx::nav
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
index 561f4f58..9355d962 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -21,27 +21,26 @@
#pragma once
-#include <armarx/navigation/graph/Graph.h>
-
-#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
-
#include <map>
#include <optional>
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+#include <armarx/navigation/graph/Graph.h>
+
class QGraphicsEllipseItem;
class QGraphicsLineItem;
class QTableWidgetItem;
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
/**
* @brief The NodeData struct holds data required for the node.
* The name is stored in the key used in the map vertices.
*/
- struct VertexData : public nav::graph::VertexAttribs
+ struct VertexData : public navigation::graph::VertexAttribs
{
/// The ellipse in the scene.
QGraphicsEllipseItem* graphicsItem = nullptr;
@@ -61,7 +60,7 @@ namespace armarx::nav::locgrapheditor
* @brief The EdgeData struct holds data required for the edge.
* The name is stored in the key used in the map edges.
*/
- struct EdgeData : public nav::graph::EdgeAttribs
+ struct EdgeData : public navigation::graph::EdgeAttribs
{
/// The line in the scene.
QGraphicsLineItem* graphicsItem = nullptr;
@@ -74,7 +73,7 @@ namespace armarx::nav::locgrapheditor
};
- struct GraphData : public nav::graph::GraphAttribs
+ struct GraphData : public navigation::graph::GraphAttribs
{
/// Whether the graph structure was changed since loading or committing.
bool edgesChanged = false;
@@ -84,7 +83,6 @@ namespace armarx::nav::locgrapheditor
class GuiGraph : public semrel::RelationGraph<VertexData, EdgeData, GraphData>
{
public:
-
using RelationGraph::RelationGraph;
@@ -101,8 +99,7 @@ namespace armarx::nav::locgrapheditor
graph::Graph fromGuiGraph(const GuiGraph& graph);
-
float getYawAngleDegree(const Eigen::Matrix4f& pose);
double getYawAngleDegree(const Eigen::Matrix4d& pose);
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
index c5d86e4d..0a050d56 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -20,10 +20,18 @@
* GNU General Public License
*/
+#include "LocationGraphEditorWidgetController.h"
+
#include <VirtualRobot/VirtualRobot.h>
-#include "LocationGraphEditorWidgetController.h"
-#include "widgets/utils.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
#include "widgets/EdgeTableWidget.h"
#include "widgets/NewEntityIdDialog.h"
#include "widgets/RobotVisuWidget.h"
@@ -31,21 +39,12 @@
#include "widgets/VertexTableWidget.h"
#include "widgets/graph_scene/Scene.h"
#include "widgets/graph_scene/Widget.h"
-
+#include "widgets/utils.h"
#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
-
-#include <armarx/navigation/location/constants.h>
-#include <armarx/navigation/location/aron/Location.aron.generated.h>
-#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
-
-#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
+#include <armarx/navigation/graph/constants.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <armarx/navigation/location/constants.h>
// Qt headers
#include <QDialog>
@@ -66,21 +65,23 @@
static const QString SETTING_LAST_SCENE = "lastScene";
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
- QString LocationGraphEditorWidgetController::GetWidgetName()
+ QString
+ LocationGraphEditorWidgetController::GetWidgetName()
{
return "Navigation.LocationGraphEditor";
}
- QIcon LocationGraphEditorWidgetController::GetWidgetIcon()
+ QIcon
+ LocationGraphEditorWidgetController::GetWidgetIcon()
{
- return QIcon {"://icons/location_graph_editor.svg"};
+ return QIcon{"://icons/location_graph_editor.svg"};
}
LocationGraphEditorWidgetController::LocationGraphEditorWidgetController() :
- settings {"KIT", "LocationGraphEditorWidgetController"}
+ settings{"KIT", "LocationGraphEditorWidgetController"}
{
widget.setupUi(getWidget());
@@ -89,7 +90,7 @@ namespace armarx::nav::locgrapheditor
widget.loadGraphButton->setEnabled(false);
view.vertexData = new VertexDataWidget();
- view.vertexData->setEnabled(false); // Enable on first selection of vertex.
+ view.vertexData->setEnabled(false); // Enable on first selection of vertex.
widget.locationDataGroupBox->layout()->addWidget(view.vertexData);
if (QBoxLayout* layout = dynamic_cast<QBoxLayout*>(widget.locationDataGroupBox->layout()))
{
@@ -112,63 +113,59 @@ namespace armarx::nav::locgrapheditor
connect(this, &This::connected, this, &This::queryGraphs);
connect(this, &This::connected, this, &This::graphChanged);
- connect(widget.createGraphButton, &QPushButton::pressed,
- this, &This::createGraphDialog);
+ connect(widget.createGraphButton, &QPushButton::pressed, this, &This::createGraphDialog);
- connect(widget.queryGraphsButton, &QPushButton::pressed,
- this, &This::queryGraphs);
+ connect(widget.queryGraphsButton, &QPushButton::pressed, this, &This::queryGraphs);
- connect(this, &This::locationMemoryChanged,
- this, &This::memoryChanged);
- connect(this, &This::graphMemoryChanged,
- this, &This::memoryChanged);
+ connect(this, &This::locationMemoryChanged, this, &This::memoryChanged);
+ connect(this, &This::graphMemoryChanged, this, &This::memoryChanged);
- connect(this, &This::graphMemoryChanged,
- this, &This::updateGraphList);
+ connect(this, &This::graphMemoryChanged, this, &This::updateGraphList);
- connect(widget.loadGraphButton, &QPushButton::pressed,
- this, &This::loadGraph);
- connect(widget.commitGraphButton, &QPushButton::pressed,
- this, &This::commit);
+ connect(widget.loadGraphButton, &QPushButton::pressed, this, &This::loadGraph);
+ connect(widget.commitGraphButton, &QPushButton::pressed, this, &This::commit);
// View updates
- connect(this, &This::graphChanged,
- this, &This::updateGraphView);
- connect(view.vertexData, &VertexDataWidget::vertexDataChanged,
- this, &This::updateGraphView);
- connect(view.robotVisu, &RobotVisuWidget::connected,
- this, &This::updateGraphView);
- connect(view.robotVisu, &RobotVisuWidget::connected,
- this, [this]()
- {
- view.vertexData->setRobotConnection(&view.robotVisu->connection());
- });
- connect(view.robotVisu, &RobotVisuWidget::settingsChanged,
- this, &This::updateGraphView);
+ connect(this, &This::graphChanged, this, &This::updateGraphView);
+ connect(
+ view.vertexData, &VertexDataWidget::vertexDataChanged, this, &This::updateGraphView);
+ connect(view.robotVisu, &RobotVisuWidget::connected, this, &This::updateGraphView);
+ connect(view.robotVisu,
+ &RobotVisuWidget::connected,
+ this,
+ [this]() { view.vertexData->setRobotConnection(&view.robotVisu->connection()); });
+ connect(view.robotVisu, &RobotVisuWidget::settingsChanged, this, &This::updateGraphView);
// Selection
- connect(view.vertexTable, &VertexTableWidget::currentItemChanged,
+ connect(view.vertexTable,
+ &VertexTableWidget::currentItemChanged,
[this](QTableWidgetItem* current, QTableWidgetItem* previous)
- {
- (void) previous;
- this->selectVertex(current);
- });
+ {
+ (void)previous;
+ this->selectVertex(current);
+ });
- connect(view.vertexTable, &VertexTableWidget::itemSelectionChanged,
- this, &This::updateVertexHighlighting);
- connect(view.edgeTable, &EdgeTableWidget::itemSelectionChanged,
- this, &This::updateEdgeHighlighting);
+ connect(view.vertexTable,
+ &VertexTableWidget::itemSelectionChanged,
+ this,
+ &This::updateVertexHighlighting);
+ connect(view.edgeTable,
+ &EdgeTableWidget::itemSelectionChanged,
+ this,
+ &This::updateEdgeHighlighting);
// Graph modification
- connect(view.vertexTable, &VertexTableWidget::newVertexRequested,
- this, &This::createVertexDialog);
- connect(view.vertexTable, &VertexTableWidget::newEdgesRequested,
- this, &This::addEdges);
- connect(view.vertexTable, &VertexTableWidget::edgeRemovalRequested,
- this, &This::removeEdgesOfVertex);
-
- connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested,
- this, &This::removeEdges);
+ connect(view.vertexTable,
+ &VertexTableWidget::newVertexRequested,
+ this,
+ &This::createVertexDialog);
+ connect(view.vertexTable, &VertexTableWidget::newEdgesRequested, this, &This::addEdges);
+ connect(view.vertexTable,
+ &VertexTableWidget::edgeRemovalRequested,
+ this,
+ &This::removeEdgesOfVertex);
+
+ connect(view.edgeTable, &EdgeTableWidget::edgeRemovalRequested, this, &This::removeEdges);
}
@@ -178,59 +175,75 @@ namespace armarx::nav::locgrapheditor
}
- QPointer<QDialog> LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
+ QPointer<QDialog>
+ LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
{
if (not configDialog)
{
configDialog = new SimpleConfigDialog(parent);
- configDialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>("MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
+ configDialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>(
+ "MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
}
return qobject_cast<SimpleConfigDialog*>(configDialog);
}
- void LocationGraphEditorWidgetController::configured()
+ void
+ LocationGraphEditorWidgetController::configured()
{
remote.memoryNameSystemName = configDialog->getProxyName("MemoryNameSystem");
}
- void LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
+ void
+ LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
{
- remote.memoryNameSystemName = settings->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName)).toString().toStdString();
+ remote.memoryNameSystemName =
+ settings
+ ->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName))
+ .toString()
+ .toStdString();
}
- void LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
+ void
+ LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
{
- settings->setValue("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName));
+ settings->setValue("memoryNameSystemName",
+ QString::fromStdString(remote.memoryNameSystemName));
}
- void LocationGraphEditorWidgetController::loadAutomaticSettings()
+ void
+ LocationGraphEditorWidgetController::loadAutomaticSettings()
{
- lastSelectedSceneName = settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
+ lastSelectedSceneName =
+ settings.value(SETTING_LAST_SCENE, lastSelectedSceneName).toString();
}
- void LocationGraphEditorWidgetController::saveAutomaticSettings()
+ void
+ LocationGraphEditorWidgetController::saveAutomaticSettings()
{
settings.setValue(SETTING_LAST_SCENE, lastSelectedSceneName);
}
- void LocationGraphEditorWidgetController::onInitComponent()
+ void
+ LocationGraphEditorWidgetController::onInitComponent()
{
usingProxy(remote.memoryNameSystemName);
}
- void LocationGraphEditorWidgetController::onConnectComponent()
+ void
+ LocationGraphEditorWidgetController::onConnectComponent()
{
remote.connect(*this);
{
std::stringstream ss;
- ss << "Navigation Graphs (Entities from Core Segment " << nav::graph::coreSegmentID << ")";
+ ss << "Navigation Graphs (Entities from Core Segment "
+ << navigation::graph::coreSegmentID << ")";
widget.graphGroupBox->setTitle(QString::fromStdString(ss.str()));
widget.graphGroupBox->setEnabled(true);
}
@@ -239,27 +252,30 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::Remote::connect(Component& parent)
+ void
+ LocationGraphEditorWidgetController::Remote::connect(Component& parent)
{
- auto mnsProxy = parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
+ auto mnsProxy =
+ parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
memoryNameSystem = armem::client::MemoryNameSystem(mnsProxy, &parent);
- locationReader = memoryNameSystem.useReader(nav::loc::coreSegmentID);
- locationWriter = memoryNameSystem.useWriter(nav::loc::coreSegmentID);
+ locationReader = memoryNameSystem.useReader(navigation::location::coreSegmentID);
+ locationWriter = memoryNameSystem.useWriter(navigation::location::coreSegmentID);
- graphReader = memoryNameSystem.useReader(nav::graph::coreSegmentID);
- graphWriter = memoryNameSystem.useWriter(nav::graph::coreSegmentID);
+ graphReader = memoryNameSystem.useReader(navigation::graph::coreSegmentID);
+ graphWriter = memoryNameSystem.useWriter(navigation::graph::coreSegmentID);
arviz = std::make_unique<viz::Client>(viz::Client::createForGuiPlugin(parent));
}
- void LocationGraphEditorWidgetController::queryMemory()
+ void
+ LocationGraphEditorWidgetController::queryMemory()
{
armem::client::QueryResult locResult = queryLocations();
armem::client::QueryResult graphResult = queryGraphs();
- if (not (locResult.success and graphResult.success))
+ if (not(locResult.success and graphResult.success))
{
QStringList status;
std::stringstream ss;
@@ -276,9 +292,11 @@ namespace armarx::nav::locgrapheditor
}
- armem::client::QueryResult LocationGraphEditorWidgetController::queryLocations()
+ armem::client::QueryResult
+ LocationGraphEditorWidgetController::queryLocations()
{
- armem::client::QueryResult result = remote.locationReader.getLatestSnapshotsIn(nav::loc::coreSegmentID);
+ armem::client::QueryResult result =
+ remote.locationReader.getLatestSnapshotsIn(navigation::location::coreSegmentID);
if (result.success)
{
model.locationsMemory = std::move(result.memory);
@@ -288,9 +306,11 @@ namespace armarx::nav::locgrapheditor
}
- armem::client::QueryResult LocationGraphEditorWidgetController::queryGraphs()
+ armem::client::QueryResult
+ LocationGraphEditorWidgetController::queryGraphs()
{
- armem::client::QueryResult result = remote.graphReader.getLatestSnapshotsIn(nav::graph::coreSegmentID);
+ armem::client::QueryResult result =
+ remote.graphReader.getLatestSnapshotsIn(navigation::graph::coreSegmentID);
if (result.success)
{
model.graphMemory = std::move(result.memory);
@@ -300,28 +320,29 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::updateGraphList()
+ void
+ LocationGraphEditorWidgetController::updateGraphList()
{
QString previousText = widget.graphsComboBox->currentText();
widget.graphsComboBox->clear();
int i = 0;
- int previousIndex = -1; // To keep selection.
- model.graphMemory.forEachEntity([&](const armem::wm::Entity& entity)
- {
- bool hasChanged = (entity.id() == model.graphEntityID
- ? model.graph.hasChanged()
- : false);
- QString text = getGraphDisplayName(entity.id(), hasChanged);
- QString id = QString::fromStdString(entity.id().str());
-
- widget.graphsComboBox->addItem(text, id);
- if (previousIndex < 0 and text == previousText)
+ int previousIndex = -1; // To keep selection.
+ model.graphMemory.forEachEntity(
+ [&](const armem::wm::Entity& entity)
{
- previousIndex = i;
- }
- ++i;
- });
+ bool hasChanged =
+ (entity.id() == model.graphEntityID ? model.graph.hasChanged() : false);
+ QString text = getGraphDisplayName(entity.id(), hasChanged);
+ QString id = QString::fromStdString(entity.id().str());
+
+ widget.graphsComboBox->addItem(text, id);
+ if (previousIndex < 0 and text == previousText)
+ {
+ previousIndex = i;
+ }
+ ++i;
+ });
if (previousIndex >= 0)
{
widget.graphsComboBox->setCurrentIndex(previousIndex);
@@ -330,7 +351,8 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::loadGraph()
+ void
+ LocationGraphEditorWidgetController::loadGraph()
{
if (model.graph.hasChanged())
{
@@ -341,7 +363,7 @@ namespace armarx::nav::locgrapheditor
}
const armem::MemoryID entityID = armem::MemoryID::fromString(
- widget.graphsComboBox->currentData().toString().toStdString());
+ widget.graphsComboBox->currentData().toString().toStdString());
// Refresh local memory.
queryMemory();
@@ -349,7 +371,8 @@ namespace armarx::nav::locgrapheditor
const armem::wm::EntityInstance* instance = model.graphMemory.findLatestInstance(entityID);
if (instance)
{
- widget.statusLabel->setText(QString::fromStdString("Loaded snapshot " + instance->id().getEntitySnapshotID().str()));
+ widget.statusLabel->setText(QString::fromStdString(
+ "Loaded snapshot " + instance->id().getEntitySnapshotID().str()));
}
else
{
@@ -358,9 +381,9 @@ namespace armarx::nav::locgrapheditor
widget.statusLabel->setText(QString::fromStdString(ss.str()));
}
- nav::graph::Graph nav;
+ navigation::graph::Graph nav;
{
- nav::graph::arondto::Graph dto;
+ navigation::graph::arondto::Graph dto;
ARMARX_CHECK_NOT_NULL(instance->data());
dto.fromAron(instance->data());
fromAron(dto, nav);
@@ -369,7 +392,7 @@ namespace armarx::nav::locgrapheditor
{
resolveLocations(nav, model.locationsMemory);
std::vector<semrel::ShapeID> remove;
- for (nav::graph::Graph::Vertex vertex : nav.vertices())
+ for (navigation::graph::Graph::Vertex vertex : nav.vertices())
{
if (not vertex.attrib().hasPose())
{
@@ -393,8 +416,8 @@ namespace armarx::nav::locgrapheditor
}
-
- bool LocationGraphEditorWidgetController::loadGraphDialog()
+ bool
+ LocationGraphEditorWidgetController::loadGraphDialog()
{
ARMARX_CHECK(model.graph.hasChanged());
@@ -410,7 +433,8 @@ namespace armarx::nav::locgrapheditor
{
if (vertex.attrib().changed)
{
- detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) + " has changed.");
+ detailLines.append("Location " + QString::fromStdString(vertex.attrib().getName()) +
+ " has changed.");
}
}
msgBox.setDetailedText(detailLines.join("\n"));
@@ -419,12 +443,12 @@ namespace armarx::nav::locgrapheditor
switch (msgBox.exec())
{
- case QMessageBox::Discard:
- return true; // Ok go.
- case QMessageBox::Cancel:
- return false; // Abort loading.
- default:
- return false; // Something went wrong.
+ case QMessageBox::Discard:
+ return true; // Ok go.
+ case QMessageBox::Cancel:
+ return false; // Abort loading.
+ default:
+ return false; // Something went wrong.
}
}
@@ -442,7 +466,7 @@ namespace armarx::nav::locgrapheditor
ss << "Committed " << locResults.results.size() << " location snapshot";
if (locResults.results.size() != 1)
{
- ss << "s"; // plural
+ ss << "s"; // plural
}
if (graphResult.success)
{
@@ -466,12 +490,14 @@ namespace armarx::nav::locgrapheditor
if (graphResult.success)
{
ss << "Committed 1 graph snapshot " << graphResult.snapshotID;
- ss << " and " << numSuccess << " locations, but failed to commit "
- << numFailed << " locations: \n" << locResults.allErrorMessages();
+ ss << " and " << numSuccess << " locations, but failed to commit " << numFailed
+ << " locations: \n"
+ << locResults.allErrorMessages();
}
else
{
- ss << "Failed to commit graph and " << numFailed << " of " << numLocs << " locations: \n";
+ ss << "Failed to commit graph and " << numFailed << " of " << numLocs
+ << " locations: \n";
ss << graphResult.errorMessage << "\n";
ss << locResults.allErrorMessages();
}
@@ -498,7 +524,7 @@ namespace armarx::nav::locgrapheditor
fromAron(vertex.attrib().aron.locationID, update.entityID);
update.timeCreated = armem::Time::now();
- nav::loc::arondto::Location dto;
+ navigation::location::arondto::Location dto;
dto.globalRobotPose = vertex.attrib().getPose();
update.instancesData = {dto.toAron()};
}
@@ -526,9 +552,9 @@ namespace armarx::nav::locgrapheditor
armem::EntityUpdateResult
LocationGraphEditorWidgetController::commitGraph()
{
- nav::graph::arondto::Graph dto;
+ navigation::graph::arondto::Graph dto;
{
- nav::graph::Graph nav = fromGuiGraph(model.graph);
+ navigation::graph::Graph nav = fromGuiGraph(model.graph);
toAron(dto, nav);
}
@@ -548,7 +574,8 @@ namespace armarx::nav::locgrapheditor
template <class GraphT>
- semrel::ShapeID findNextFreeVertexID(const GraphT& graph, semrel::ShapeID vertexID)
+ semrel::ShapeID
+ findNextFreeVertexID(const GraphT& graph, semrel::ShapeID vertexID)
{
while (graph.hasVertex(vertexID))
{
@@ -557,9 +584,9 @@ namespace armarx::nav::locgrapheditor
return vertexID;
}
- void LocationGraphEditorWidgetController::setGraph(
- const armem::MemoryID& entityID,
- const graph::Graph& nav)
+ void
+ LocationGraphEditorWidgetController::setGraph(const armem::MemoryID& entityID,
+ const graph::Graph& nav)
{
model.graphEntityID = entityID;
@@ -574,22 +601,24 @@ namespace armarx::nav::locgrapheditor
for (auto vertex : nav.vertices())
{
ARMARX_CHECK(vertex.attrib().hasPose());
- addVertex(vertex.objectID(), { vertex.attrib() });
+ addVertex(vertex.objectID(), {vertex.attrib()});
- coveredLocationIDs.insert(aron::fromAron<armem::MemoryID>(vertex.attrib().aron.locationID));
+ coveredLocationIDs.insert(
+ aron::fromAron<armem::MemoryID>(vertex.attrib().aron.locationID));
}
// Add locations which have not been part of graph.
// ToDo: This should be an explicit step in the GUI.
{
- semrel::ShapeID vertexID { 0 };
- model.locationsMemory.forEachInstance([&](const armem::wm::EntityInstance& instance)
- {
- if (coveredLocationIDs.count(instance.id().getEntityID()) == 0)
+ semrel::ShapeID vertexID{0};
+ model.locationsMemory.forEachInstance(
+ [&](const armem::wm::EntityInstance& instance)
{
- vertexID = findNextFreeVertexID(model.graph, vertexID);
- addVertex(vertexID, instance);
- }
- });
+ if (coveredLocationIDs.count(instance.id().getEntityID()) == 0)
+ {
+ vertexID = findNextFreeVertexID(model.graph, vertexID);
+ addVertex(vertexID, instance);
+ }
+ });
}
@@ -597,7 +626,7 @@ namespace armarx::nav::locgrapheditor
{
addEdge(model.graph.vertex(edge.sourceObjectID()),
model.graph.vertex(edge.targetObjectID()),
- { edge.attrib() });
+ {edge.attrib()});
}
}
@@ -606,8 +635,8 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::setEmptyGraph(
- const armem::MemoryID& entityID)
+ void
+ LocationGraphEditorWidgetController::setEmptyGraph(const armem::MemoryID& entityID)
{
model.graphEntityID = entityID;
@@ -615,13 +644,14 @@ namespace armarx::nav::locgrapheditor
QSignalBlocker blocker(this);
clearGraph();
- semrel::ShapeID id { 0 };
+ semrel::ShapeID id{0};
queryLocations();
- model.locationsMemory.forEachInstance([this, &id](const armem::wm::EntityInstance& instance)
- {
- addVertex(id, instance);
- ++id;
- });
+ model.locationsMemory.forEachInstance(
+ [this, &id](const armem::wm::EntityInstance& instance)
+ {
+ addVertex(id, instance);
+ ++id;
+ });
}
// Mark graph as changed.
@@ -631,9 +661,9 @@ namespace armarx::nav::locgrapheditor
}
- GuiGraph::Vertex LocationGraphEditorWidgetController::addVertex(
- semrel::ShapeID vertexID,
- const VertexData& defaultAttribs)
+ GuiGraph::Vertex
+ LocationGraphEditorWidgetController::addVertex(semrel::ShapeID vertexID,
+ const VertexData& defaultAttribs)
{
ARMARX_CHECK(not model.graph.hasVertex(vertexID));
@@ -649,10 +679,10 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Vertex
LocationGraphEditorWidgetController::addVertex(
- semrel::ShapeID vertexID,
- const armem::wm::EntityInstance& locationInstance)
+ semrel::ShapeID vertexID,
+ const armem::wm::EntityInstance& locationInstance)
{
- nav::loc::arondto::Location location;
+ navigation::location::arondto::Location location;
location.fromAron(locationInstance.data());
VertexData attrib;
@@ -663,15 +693,13 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Edge
- LocationGraphEditorWidgetController::addEdge(
- GuiGraph::ConstVertex source,
- GuiGraph::ConstVertex target,
- const EdgeData& defaultAttribs)
+ LocationGraphEditorWidgetController::addEdge(GuiGraph::ConstVertex source,
+ GuiGraph::ConstVertex target,
+ const EdgeData& defaultAttribs)
{
ARMARX_CHECK(not model.graph.hasEdge(source.objectID(), target.objectID()))
- << "Edge must not exist before being added: '"
- << source.attrib().getName() << "' -> '"
- << target.attrib().getName() << "'";
+ << "Edge must not exist before being added: '" << source.attrib().getName() << "' -> '"
+ << target.attrib().getName() << "'";
GuiGraph::Edge edge = model.graph.addEdge(source, target, defaultAttribs);
edge.attrib().tableWidgetItem = view.edgeTable->addEdge(edge);
@@ -683,7 +711,8 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::updateGraphView()
+ void
+ LocationGraphEditorWidgetController::updateGraphView()
{
for (auto vertex : model.graph.vertices())
{
@@ -694,22 +723,25 @@ namespace armarx::nav::locgrapheditor
updateEdgeView(edge);
}
- int index = widget.graphsComboBox->findData(QString::fromStdString(model.graphEntityID.str()));
+ int index =
+ widget.graphsComboBox->findData(QString::fromStdString(model.graphEntityID.str()));
if (index >= 0)
{
- widget.graphsComboBox->setItemText(index, getGraphDisplayName(model.graphEntityID, model.graph.hasChanged()));
+ widget.graphsComboBox->setItemText(
+ index, getGraphDisplayName(model.graphEntityID, model.graph.hasChanged()));
}
updateArViz();
}
- void LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
+ void
+ LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex vertex)
{
view.graph->scene()->updateVertex(vertex);
view.vertexTable->updateVertex(vertex);
- if (/* DISABLES CODE */ (false)) // Disable this for the moment.
+ if (/* DISABLES CODE */ (false)) // Disable this for the moment.
{
// Highlight all edges between highlighted vertices
std::set<semrel::ShapeID> highlightedVertices;
@@ -723,9 +755,8 @@ namespace armarx::nav::locgrapheditor
for (auto edge : model.graph.edges())
{
- bool verticesHighlighted =
- highlightedVertices.count(edge.sourceObjectID())
- and highlightedVertices.count(edge.targetObjectID());
+ bool verticesHighlighted = highlightedVertices.count(edge.sourceObjectID()) and
+ highlightedVertices.count(edge.targetObjectID());
// Already highlighted but vertices not highlighted => to false, update
// Not highlighted but vertices highlighted => to true, update
@@ -746,7 +777,8 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::updateArViz()
+ void
+ LocationGraphEditorWidgetController::updateArViz()
{
if (remote.arviz)
{
@@ -763,8 +795,8 @@ namespace armarx::nav::locgrapheditor
viz::Layer& robot = layers.emplace_back(remote.arviz->layer("Robot"));
if (view.vertexData->vertex().has_value() and view.robotVisu->isEnabled())
{
- robot.add(view.robotVisu->connection().vizRobot("robot")
- .pose(view.vertexData->vertex()->attrib().getPose()));
+ robot.add(view.robotVisu->connection().vizRobot("robot").pose(
+ view.vertexData->vertex()->attrib().getPose()));
}
}
remote.arviz->commit(layers);
@@ -773,10 +805,9 @@ namespace armarx::nav::locgrapheditor
template <class T>
- static
- void updateElementHighlighting(
- QList<QTableWidgetItem*> selectedItems,
- std::map<QTableWidgetItem*, T>&& itemToElementMap)
+ static void
+ updateElementHighlighting(QList<QTableWidgetItem*> selectedItems,
+ std::map<QTableWidgetItem*, T>&& itemToElementMap)
{
for (QTableWidgetItem* selected : selectedItems)
{
@@ -793,21 +824,26 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::updateVertexHighlighting()
+ void
+ LocationGraphEditorWidgetController::updateVertexHighlighting()
{
- updateElementHighlighting(view.vertexTable->selectedVertexItems(), model.graph.getTableItemToVertexMap());
+ updateElementHighlighting(view.vertexTable->selectedVertexItems(),
+ model.graph.getTableItemToVertexMap());
emit graphChanged();
}
- void LocationGraphEditorWidgetController::updateEdgeHighlighting()
+ void
+ LocationGraphEditorWidgetController::updateEdgeHighlighting()
{
- updateElementHighlighting(view.edgeTable->selectedEdgeItems(), model.graph.getTableItemToEdgeMap());
+ updateElementHighlighting(view.edgeTable->selectedEdgeItems(),
+ model.graph.getTableItemToEdgeMap());
emit graphChanged();
}
- void LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
+ void
+ LocationGraphEditorWidgetController::selectVertex(QTableWidgetItem* vertexItem)
{
if (vertexItem == nullptr)
{
@@ -820,13 +856,15 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::selectVertex(GuiGraph::Vertex vertex)
+ void
+ LocationGraphEditorWidgetController::selectVertex(GuiGraph::Vertex vertex)
{
view.vertexData->setVertex(vertex);
}
- void LocationGraphEditorWidgetController::clearGraph()
+ void
+ LocationGraphEditorWidgetController::clearGraph()
{
{
QSignalBlocker blocker(this);
@@ -842,10 +880,11 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::clearEdges()
+ void
+ LocationGraphEditorWidgetController::clearEdges()
{
// Remove in reverse order to be more array-friendly.
- std::vector<GuiGraph::Edge> edges { model.graph.edges().begin(), model.graph.edges().end() };
+ std::vector<GuiGraph::Edge> edges{model.graph.edges().begin(), model.graph.edges().end()};
{
QSignalBlocker blocker(this);
for (auto it = edges.rbegin(); it != edges.rend(); ++it)
@@ -862,13 +901,15 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::clearVertices()
+ void
+ LocationGraphEditorWidgetController::clearVertices()
{
ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0)
- << "The graph may not have any edges when clearing the vertices.";
+ << "The graph may not have any edges when clearing the vertices.";
// Remove in reverse order to be more array-friendly.
- std::vector<GuiGraph::Vertex> vertices { model.graph.vertices().begin(), model.graph.vertices().end() };
+ std::vector<GuiGraph::Vertex> vertices{model.graph.vertices().begin(),
+ model.graph.vertices().end()};
{
QSignalBlocker blocker(this);
for (auto it = vertices.rbegin(); it != vertices.rend(); ++it)
@@ -885,10 +926,11 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::removeEdge(GuiGraph::Edge& edge)
+ void
+ LocationGraphEditorWidgetController::removeEdge(GuiGraph::Edge& edge)
{
ARMARX_CHECK(model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
- << "Cannot remove edge that does not exist.";
+ << "Cannot remove edge that does not exist.";
// Remove view elements
{
@@ -901,18 +943,21 @@ namespace armarx::nav::locgrapheditor
model.graph.removeEdge(edge);
ARMARX_CHECK(not model.graph.hasEdge(edge.sourceDescriptor(), edge.targetDescriptor()))
- << edge.sourceDescriptor() << " -> " << edge.targetDescriptor();
+ << edge.sourceDescriptor() << " -> " << edge.targetDescriptor();
emit graphChanged();
}
- void LocationGraphEditorWidgetController::removeVertex(GuiGraph::Vertex& vertex)
+ void
+ LocationGraphEditorWidgetController::removeVertex(GuiGraph::Vertex& vertex)
{
ARMARX_CHECK_EQUAL(vertex.inDegree(), 0)
- << "A vertex must not have any edges before being removed. " << vertex.attrib().getName();
+ << "A vertex must not have any edges before being removed. "
+ << vertex.attrib().getName();
ARMARX_CHECK_EQUAL(vertex.outDegree(), 0)
- << "A vertex must not have any edges before being removed. " << vertex.attrib().getName();
+ << "A vertex must not have any edges before being removed. "
+ << vertex.attrib().getName();
// Remove view elements
{
@@ -932,21 +977,22 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::createVertexDialog()
+ void
+ LocationGraphEditorWidgetController::createVertexDialog()
{
- NewEntityIdDialog dialog(nav::loc::coreSegmentID, nullptr);
+ NewEntityIdDialog dialog(navigation::location::coreSegmentID, nullptr);
switch (dialog.exec())
{
- case QDialog::Accepted:
- // Ok go.
- break;
- case QDialog::Rejected:
- // Abort creation.
- return;
+ case QDialog::Accepted:
+ // Ok go.
+ break;
+ case QDialog::Rejected:
+ // Abort creation.
+ return;
}
// Find free vertex ID
- const semrel::ShapeID vertexID = findNextFreeVertexID(model.graph, semrel::ShapeID{ 0 });
+ const semrel::ShapeID vertexID = findNextFreeVertexID(model.graph, semrel::ShapeID{0});
// Initiaize attributes
VertexData attribs;
@@ -958,8 +1004,9 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::addEdges(
- QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
+ void
+ LocationGraphEditorWidgetController::addEdges(
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
{
const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
{
@@ -981,8 +1028,8 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::removeEdges(
- QList<QTableWidgetItem*> edgeItems)
+ void
+ LocationGraphEditorWidgetController::removeEdges(QList<QTableWidgetItem*> edgeItems)
{
const auto itemToEdgeMap = model.graph.getTableItemToEdgeMap();
{
@@ -1000,9 +1047,9 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::removeEdgesOfVertex(
- QList<QTableWidgetItem*> vertexItems,
- utils::EdgeDirection direction)
+ void
+ LocationGraphEditorWidgetController::removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction)
{
auto removeEdges = [this](auto&& range)
{
@@ -1020,18 +1067,18 @@ namespace armarx::nav::locgrapheditor
GuiGraph::Vertex vertex = itemToVertexMap.at(vertexItem);
switch (direction)
{
- case utils::EdgeDirection::To:
- removeEdges(vertex.inEdges());
- break;
-
- case utils::EdgeDirection::From:
- removeEdges(vertex.outEdges());
- break;
-
- case utils::EdgeDirection::Bidirectional:
- removeEdges(vertex.inEdges());
- removeEdges(vertex.outEdges());
- break;
+ case utils::EdgeDirection::To:
+ removeEdges(vertex.inEdges());
+ break;
+
+ case utils::EdgeDirection::From:
+ removeEdges(vertex.outEdges());
+ break;
+
+ case utils::EdgeDirection::Bidirectional:
+ removeEdges(vertex.inEdges());
+ removeEdges(vertex.outEdges());
+ break;
}
}
}
@@ -1041,15 +1088,16 @@ namespace armarx::nav::locgrapheditor
}
- void LocationGraphEditorWidgetController::createGraphDialog()
+ void
+ LocationGraphEditorWidgetController::createGraphDialog()
{
- NewEntityIdDialog dialog(nav::graph::coreSegmentID, nullptr);
+ NewEntityIdDialog dialog(navigation::graph::coreSegmentID, nullptr);
switch (dialog.exec())
{
- case QDialog::Accepted:
- break;
- case QDialog::Rejected:
- return;
+ case QDialog::Accepted:
+ break;
+ case QDialog::Rejected:
+ return;
}
const armem::MemoryID entityID = dialog.entityID();
@@ -1064,10 +1112,12 @@ namespace armarx::nav::locgrapheditor
}
- QString LocationGraphEditorWidgetController::getGraphDisplayName(
- const armem::MemoryID& entityID, bool changed) const
+ QString
+ LocationGraphEditorWidgetController::getGraphDisplayName(const armem::MemoryID& entityID,
+ bool changed) const
{
- QString name = QString::fromStdString(entityID.providerSegmentName + "/" + entityID.entityName);
+ QString name =
+ QString::fromStdString(entityID.providerSegmentName + "/" + entityID.entityName);
if (changed)
{
name += "*";
@@ -1075,4 +1125,4 @@ namespace armarx::nav::locgrapheditor
return name;
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
index 3746eea6..30ee5941 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -21,29 +21,27 @@
*/
#pragma once
-#include "GuiGraph.h"
-#include "Visu.h"
-#include "widgets/graph_scene.h"
+#include <memory>
+#include <string>
-#include <armarx/navigation/graph/Graph.h>
-#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
-#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <RobotAPI/libraries/armem/client/forward_declarations.h>
#include <RobotAPI/libraries/armem/client/MemoryNameSystem.h>
#include <RobotAPI/libraries/armem/client/Reader.h>
#include <RobotAPI/libraries/armem/client/Writer.h>
+#include <RobotAPI/libraries/armem/client/forward_declarations.h>
+#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
-#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
-#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.h>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/system/ImportExportComponent.h>
-
-#include <memory>
-#include <string>
+#include "GuiGraph.h"
+#include "Visu.h"
+#include "widgets/graph_scene.h"
+#include <Navigation/gui-plugins/LocationGraphEditor/ui_LocationGraphEditorWidget.h>
+#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
class QDialog;
@@ -53,11 +51,11 @@ namespace armarx::viz
{
class Client;
}
-namespace armarx::nav::locgrapheditor::utils
+namespace armarx::navigation::locgrapheditor::utils
{
enum class EdgeDirection;
}
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class EdgeTableWidget;
class RobotVisuWidget;
@@ -92,9 +90,8 @@ namespace armarx::nav::locgrapheditor
*
* Detailed description
*/
- class ARMARXCOMPONENT_IMPORT_EXPORT
- LocationGraphEditorWidgetController :
- public armarx::ArmarXComponentWidgetControllerTemplate < LocationGraphEditorWidgetController >
+ class ARMARXCOMPONENT_IMPORT_EXPORT LocationGraphEditorWidgetController :
+ public armarx::ArmarXComponentWidgetControllerTemplate<LocationGraphEditorWidgetController>
{
Q_OBJECT
@@ -102,7 +99,6 @@ namespace armarx::nav::locgrapheditor
public:
-
static QString GetWidgetName();
static QIcon GetWidgetIcon();
@@ -177,23 +173,23 @@ namespace armarx::nav::locgrapheditor
void addEdges(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems);
void removeEdges(QList<QTableWidgetItem*> edgeItems);
- void removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
+ void removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction);
void createGraphDialog();
private:
-
void setGraph(const armem::MemoryID& entityID, const graph::Graph& nav);
void setEmptyGraph(const armem::MemoryID& entityID);
- GuiGraph::Vertex
- addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs);
- GuiGraph::Vertex
- addVertex(semrel::ShapeID vertexID, const armem::wm::EntityInstance& locationInstance);
+ GuiGraph::Vertex addVertex(semrel::ShapeID vertexID, const VertexData& defaultAttribs);
+ GuiGraph::Vertex addVertex(semrel::ShapeID vertexID,
+ const armem::wm::EntityInstance& locationInstance);
- GuiGraph::Edge
- addEdge(GuiGraph::ConstVertex source, GuiGraph::ConstVertex target, const EdgeData& defaultAttribs);
+ GuiGraph::Edge addEdge(GuiGraph::ConstVertex source,
+ GuiGraph::ConstVertex target,
+ const EdgeData& defaultAttribs);
void removeVertex(GuiGraph::Vertex& vertex);
void removeEdge(GuiGraph::Edge& edge);
@@ -207,7 +203,6 @@ namespace armarx::nav::locgrapheditor
private:
-
/// Widget Form
Ui::LocationGraphEditorWidget widget;
QPointer<SimpleConfigDialog> configDialog;
@@ -258,15 +253,13 @@ namespace armarx::nav::locgrapheditor
private:
-
QSettings settings;
QString lastSelectedSceneName;
};
-}
+} // namespace armarx::navigation::locgrapheditor
namespace armarx
{
- using armarx::nav::locgrapheditor::LocationGraphEditorWidgetController;
+ using armarx::navigation::locgrapheditor::LocationGraphEditorWidgetController;
}
-
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
index be1323e9..bbfb5d6c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -19,25 +19,25 @@
* GNU General Public License
*/
-#include <VirtualRobot/VirtualRobot.h>
-
#include "Visu.h"
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
#include <SimoxUtility/color/Color.h>
#include <SimoxUtility/math/pose.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
const simox::Color visu::defaultColorHighlighted = simox::Color::orange();
- viz::Pose VertexVisu::Pose::draw(const VertexData& attribs) const
+ viz::Pose
+ VertexVisu::Pose::draw(const VertexData& attribs) const
{
- viz::Pose pose = nav::graph::VertexVisu::Pose::draw(attribs);
+ viz::Pose pose = navigation::graph::VertexVisu::Pose::draw(attribs);
if (attribs.highlighted)
{
pose.scale(scale * scaleFactorHighlighted);
@@ -46,9 +46,10 @@ namespace armarx::nav::locgrapheditor
}
- viz::Arrow VertexVisu::ForwardArrow::draw(const VertexData& attribs) const
+ viz::Arrow
+ VertexVisu::ForwardArrow::draw(const VertexData& attribs) const
{
- viz::Arrow arrow = nav::graph::VertexVisu::ForwardArrow::draw(attribs);
+ viz::Arrow arrow = navigation::graph::VertexVisu::ForwardArrow::draw(attribs);
if (attribs.highlighted)
{
arrow.color(colorHighlighted);
@@ -58,13 +59,15 @@ namespace armarx::nav::locgrapheditor
}
- void VertexVisu::draw(viz::Layer& layer, GuiGraph::ConstVertex vertex) const
+ void
+ VertexVisu::draw(viz::Layer& layer, GuiGraph::ConstVertex vertex) const
{
draw(layer, vertex.attrib());
}
- void VertexVisu::draw(viz::Layer& layer, const VertexData& attribs) const
+ void
+ VertexVisu::draw(viz::Layer& layer, const VertexData& attribs) const
{
if (pose.has_value())
{
@@ -77,18 +80,19 @@ namespace armarx::nav::locgrapheditor
}
- viz::Arrow EdgeVisu::Arrow::draw(GuiGraph::ConstEdge edge) const
+ viz::Arrow
+ EdgeVisu::Arrow::draw(GuiGraph::ConstEdge edge) const
{
return draw(edge.attrib(), edge.source().attrib(), edge.target().attrib());
}
- viz::Arrow EdgeVisu::Arrow::draw(
- const EdgeData& edge,
- const VertexData& source,
- const VertexData& target) const
+ viz::Arrow
+ EdgeVisu::Arrow::draw(const EdgeData& edge,
+ const VertexData& source,
+ const VertexData& target) const
{
- viz::Arrow arrow = nav::graph::EdgeVisu::Arrow::draw(edge, source, target);
+ viz::Arrow arrow = navigation::graph::EdgeVisu::Arrow::draw(edge, source, target);
if (edge.highlighted)
{
arrow.color(colorHighlighted);
@@ -98,7 +102,8 @@ namespace armarx::nav::locgrapheditor
}
- void EdgeVisu::draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const
+ void
+ EdgeVisu::draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const
{
if (arrow.has_value())
{
@@ -107,7 +112,8 @@ namespace armarx::nav::locgrapheditor
}
- void GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph) const
+ void
+ GraphVisu::draw(viz::Layer& layer, const GuiGraph& graph) const
{
if (vertex.has_value())
{
@@ -125,4 +131,4 @@ namespace armarx::nav::locgrapheditor
}
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
index 0266cd2f..77193be2 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -21,39 +21,38 @@
#pragma once
-#include "GuiGraph.h"
-
-#include <armarx/navigation/graph/Visu.h>
+#include <optional>
#include <SimoxUtility/color/Color.h>
-#include <optional>
+#include "GuiGraph.h"
+#include <armarx/navigation/graph/Visu.h>
-namespace armarx::nav::locgrapheditor::visu
+namespace armarx::navigation::locgrapheditor::visu
{
extern const simox::Color defaultColorHighlighted;
}
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
struct VertexVisu
{
- struct Pose : public nav::graph::VertexVisu::Pose
+ struct Pose : public navigation::graph::VertexVisu::Pose
{
float scaleFactorHighlighted = 1.5;
viz::Pose draw(const VertexData& attribs) const;
};
- std::optional<Pose> pose = Pose {};
+ std::optional<Pose> pose = Pose{};
- struct ForwardArrow : public nav::graph::VertexVisu::ForwardArrow
+ struct ForwardArrow : public navigation::graph::VertexVisu::ForwardArrow
{
simox::Color colorHighlighted = visu::defaultColorHighlighted;
viz::Arrow draw(const VertexData& attribs) const;
};
- std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow{};
void draw(viz::Layer& layer, GuiGraph::ConstVertex vertex) const;
@@ -63,14 +62,15 @@ namespace armarx::nav::locgrapheditor
struct EdgeVisu
{
- struct Arrow : public nav::graph::EdgeVisu::Arrow
+ struct Arrow : public navigation::graph::EdgeVisu::Arrow
{
simox::Color colorHighlighted = visu::defaultColorHighlighted;
viz::Arrow draw(GuiGraph::ConstEdge edge) const;
- viz::Arrow draw(const EdgeData& edge, const VertexData& source, const VertexData& target) const;
+ viz::Arrow
+ draw(const EdgeData& edge, const VertexData& source, const VertexData& target) const;
};
- std::optional<Arrow> arrow = Arrow {};
+ std::optional<Arrow> arrow = Arrow{};
void draw(viz::Layer& layer, GuiGraph::ConstEdge edge) const;
@@ -79,8 +79,8 @@ namespace armarx::nav::locgrapheditor
struct GraphVisu
{
- std::optional<VertexVisu> vertex = VertexVisu {};
- std::optional<EdgeVisu> edge = EdgeVisu {};
+ std::optional<VertexVisu> vertex = VertexVisu{};
+ std::optional<EdgeVisu> edge = EdgeVisu{};
void draw(viz::Layer& layer, const GuiGraph& graph) const;
@@ -88,7 +88,8 @@ namespace armarx::nav::locgrapheditor
template <class VisuT, class RangeT>
- void applyVisu(viz::Layer& layer, const std::optional<VisuT>& visu, RangeT&& range)
+ void
+ applyVisu(viz::Layer& layer, const std::optional<VisuT>& visu, RangeT&& range)
{
if (visu.has_value())
{
@@ -99,4 +100,4 @@ namespace armarx::nav::locgrapheditor
}
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
index 9ced9d91..3752fc45 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.cpp
@@ -22,7 +22,7 @@
#include "ConnectDialog.h"
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
}
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
index 14157a33..2090fea9 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
@@ -21,34 +21,29 @@
#pragma once
-#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+#include <QDialog>
+#include <QDialogButtonBox>
#include <ArmarXCore/core/ManagedIceObject.h>
-#include <QDialog>
-#include <QDialogButtonBox>
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
template <class ProxyT>
class ConnectDialog : public QDialog
{
public:
-
- ConnectDialog(
- QString searchMask,
- ManagedIceObject& component,
- QWidget* parent = nullptr) :
- QDialog(parent),
- component(component)
+ ConnectDialog(QString searchMask, ManagedIceObject& component, QWidget* parent = nullptr) :
+ QDialog(parent), component(component)
{
finder = new armarx::IceProxyFinder<ProxyT>();
finder->setIceManager(component.getIceManager());
finder->setSearchMask(searchMask);
- QDialogButtonBox* buttonBox = new QDialogButtonBox(
- QDialogButtonBox::Ok | QDialogButtonBox::Cancel, this);
+ QDialogButtonBox* buttonBox =
+ new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, this);
setLayout(new QVBoxLayout);
layout()->addWidget(finder);
@@ -63,7 +58,8 @@ namespace armarx::nav::locgrapheditor
}
- ProxyT getProxy()
+ ProxyT
+ getProxy()
{
QString name = finder->getSelectedProxyName();
if (name.size() > 0)
@@ -78,11 +74,9 @@ namespace armarx::nav::locgrapheditor
public:
-
ManagedIceObject& component;
armarx::IceProxyFinder<ProxyT>* finder = nullptr;
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
index 93d87392..13c436fe 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -20,14 +20,15 @@
*/
#include "EdgeTableWidget.h"
-#include "utils.h"
#include <QAction>
#include <QHeaderView>
#include <QMenu>
+#include "utils.h"
+
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
EdgeTableWidget::EdgeTableWidget()
@@ -49,15 +50,13 @@ namespace armarx::nav::locgrapheditor
setStyleSheet(styleSheet);
setContextMenuPolicy(Qt::CustomContextMenu);
- connect(this, &This::customContextMenuRequested,
- this, &This::makeContextMenu);
+ connect(this, &This::customContextMenuRequested, this, &This::makeContextMenu);
}
QTableWidgetItem*
- EdgeTableWidget::addEdge(
- const graph::VertexAttribs& sourceAttrib,
- const graph::VertexAttribs& targetAttrib)
+ EdgeTableWidget::addEdge(const graph::VertexAttribs& sourceAttrib,
+ const graph::VertexAttribs& targetAttrib)
{
QTableWidgetItem* result = nullptr;
@@ -66,8 +65,8 @@ namespace armarx::nav::locgrapheditor
int row = rowCount();
setRowCount(row + 1);
- setItem(row, 0, new QTableWidgetItem {QString::fromStdString(sourceAttrib.getName())});
- setItem(row, 1, new QTableWidgetItem {QString::fromStdString(targetAttrib.getName())});
+ setItem(row, 0, new QTableWidgetItem{QString::fromStdString(sourceAttrib.getName())});
+ setItem(row, 1, new QTableWidgetItem{QString::fromStdString(targetAttrib.getName())});
result = item(row, 0);
}
@@ -136,8 +135,8 @@ namespace armarx::nav::locgrapheditor
QString desc;
if (items.size() == 1)
{
- desc = "edge '" + items[0]->text() + "' " + utils::arrowRight
- + " '" + item(row(items[0]), 1)->text() + "'";
+ desc = "edge '" + items[0]->text() + "' " + utils::arrowRight + " '" +
+ item(row(items[0]), 1)->text() + "'";
}
else
{
@@ -146,13 +145,11 @@ namespace armarx::nav::locgrapheditor
menu.addSection("Selected " + desc);
connect(menu.addAction("Remove " + desc),
- &QAction::triggered, [this, &items]()
- {
- emit edgeRemovalRequested(items);
- });
+ &QAction::triggered,
+ [this, &items]() { emit edgeRemovalRequested(items); });
}
menu.exec(mapToGlobal(pos));
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
index 2e745a1e..9e1aae15 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -21,15 +21,15 @@
#pragma once
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
-
#include <QColor>
#include <QList>
#include <QTableWidget>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
+
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class EdgeTableWidget : public QTableWidget
@@ -39,19 +39,18 @@ namespace armarx::nav::locgrapheditor
public:
-
EdgeTableWidget();
template <class EdgeT>
- QTableWidgetItem* addEdge(const EdgeT& edge)
+ QTableWidgetItem*
+ addEdge(const EdgeT& edge)
{
return addEdge(edge.source().attrib(), edge.target().attrib());
}
- QTableWidgetItem*
- addEdge(const graph::VertexAttribs& sourceAttrib,
- const graph::VertexAttribs& targetAttrib);
+ QTableWidgetItem* addEdge(const graph::VertexAttribs& sourceAttrib,
+ const graph::VertexAttribs& targetAttrib);
void updateEdge(GuiGraph::Edge edge);
@@ -75,11 +74,8 @@ namespace armarx::nav::locgrapheditor
public:
-
QColor bgColorDefault = default_colors::tableBackgroundDefault;
QColor bgColorSelected = default_colors::tableBackgroundSelected;
-
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
index 381556b4..7fdac3c4 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
@@ -21,32 +21,31 @@
#include "NewEntityIdDialog.h"
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
#include <QDialogButtonBox>
#include <QGridLayout>
-#include <QLineEdit>
#include <QLabel>
+#include <QLineEdit>
#include <QPushButton>
#include <QVBoxLayout>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
NewEntityIdDialog::NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent) :
- QDialog(parent),
- coreSegmentID(std::make_unique<armem::MemoryID>(coreSegmentID))
+ QDialog(parent), coreSegmentID(std::make_unique<armem::MemoryID>(coreSegmentID))
{
_providerSegmentName = new QLineEdit(this);
_entityName = new QLineEdit(this);
- _buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok
- | QDialogButtonBox::Cancel, this);
+ _buttonBox = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, this);
- QLabel* instruction = new QLabel("Enter provider segment name and entity name for new entity:");
+ QLabel* instruction =
+ new QLabel("Enter provider segment name and entity name for new entity:");
QFont font = instruction->font();
font.setBold(true);
instruction->setFont(font);
@@ -84,7 +83,8 @@ namespace armarx::nav::locgrapheditor
auto enableOkIfReady = [this]()
{
_buttonBox->button(QDialogButtonBox::Ok)
- ->setEnabled(_providerSegmentName->text().size() > 0 and _entityName->text().size() > 0);
+ ->setEnabled(_providerSegmentName->text().size() > 0 and
+ _entityName->text().size() > 0);
};
connect(_providerSegmentName, &QLineEdit::textChanged, this, enableOkIfReady);
connect(_entityName, &QLineEdit::textChanged, this, enableOkIfReady);
@@ -98,19 +98,22 @@ namespace armarx::nav::locgrapheditor
}
- QString NewEntityIdDialog::providerSegmentName() const
+ QString
+ NewEntityIdDialog::providerSegmentName() const
{
return _providerSegmentName->text();
}
- QString NewEntityIdDialog::entityName() const
+ QString
+ NewEntityIdDialog::entityName() const
{
return _entityName->text();
}
- armem::MemoryID NewEntityIdDialog::entityIDWithoutCoreSegmentID() const
+ armem::MemoryID
+ NewEntityIdDialog::entityIDWithoutCoreSegmentID() const
{
armem::MemoryID id;
id.providerSegmentName = providerSegmentName().toStdString();
@@ -118,7 +121,8 @@ namespace armarx::nav::locgrapheditor
return id;
}
- armem::MemoryID NewEntityIdDialog::entityID() const
+ armem::MemoryID
+ NewEntityIdDialog::entityID() const
{
armem::MemoryID entityID = entityIDWithoutCoreSegmentID();
ARMARX_CHECK_NOT_NULL(coreSegmentID);
@@ -126,4 +130,4 @@ namespace armarx::nav::locgrapheditor
return entityID;
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
index 18659cda..2a0aad3d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
@@ -21,23 +21,21 @@
#pragma once
-#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-
#include <QDialog>
-
#include <memory>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
class QLineEdit;
class QDialogButtonBox;
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class NewEntityIdDialog : public QDialog
{
public:
-
NewEntityIdDialog(const armem::MemoryID& coreSegmentID, QWidget* parent = nullptr);
virtual ~NewEntityIdDialog() override;
@@ -50,7 +48,6 @@ namespace armarx::nav::locgrapheditor
private:
-
QLineEdit* _providerSegmentName = nullptr;
QLineEdit* _entityName = nullptr;
@@ -58,7 +55,6 @@ namespace armarx::nav::locgrapheditor
std::unique_ptr<armem::MemoryID> coreSegmentID;
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
index e6699a15..afe0a4a1 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
@@ -20,17 +20,6 @@
*/
#include "RobotVisuWidget.h"
-#include "ConnectDialog.h"
-
-#include <RobotAPI/interface/core/RobotState.h>
-#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-#include <RobotAPI/components/ArViz/Client/Elements.h>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <ArmarXCore/core/ManagedIceObject.h>
-
-#include <VirtualRobot/Robot.h>
#include <QCheckBox>
#include <QDialog>
@@ -38,12 +27,22 @@
#include <QPushButton>
#include <QVBoxLayout>
+#include <VirtualRobot/Robot.h>
-namespace armarx::nav::locgrapheditor
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/interface/core/RobotState.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
+
+#include "ConnectDialog.h"
+
+
+namespace armarx::navigation::locgrapheditor
{
- robotvisu::SettingsWidget::SettingsWidget(QWidget* parent) :
- QWidget(parent)
+ robotvisu::SettingsWidget::SettingsWidget(QWidget* parent) : QWidget(parent)
{
_enabled = new QCheckBox("Enable Visu", this);
_enabled->setChecked(true);
@@ -65,28 +64,30 @@ namespace armarx::nav::locgrapheditor
}
- bool robotvisu::SettingsWidget::isEnabled() const
+ bool
+ robotvisu::SettingsWidget::isEnabled() const
{
return _enabled->isChecked();
}
- bool robotvisu::SettingsWidget::useCollisionModel() const
+ bool
+ robotvisu::SettingsWidget::useCollisionModel() const
{
return _collisionModel->isChecked();
}
- robotvisu::Connection::Connection(
- RobotStateComponentInterfacePrx robotStateComponent,
- robotvisu::SettingsWidget* settings) :
- _robotStateComponent(std::make_unique<RobotStateComponentInterfacePrx>(robotStateComponent)),
+ robotvisu::Connection::Connection(RobotStateComponentInterfacePrx robotStateComponent,
+ robotvisu::SettingsWidget* settings) :
+ _robotStateComponent(
+ std::make_unique<RobotStateComponentInterfacePrx>(robotStateComponent)),
_settings(settings)
{
ARMARX_CHECK_NOT_NULL(robotStateComponent);
_filename = robotStateComponent->getRobotFilename();
_robot = RemoteRobot::createLocalClone(
- robotStateComponent, "", {}, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ robotStateComponent, "", {}, VirtualRobot::RobotIO::RobotDescription::eStructure);
ARMARX_CHECK_NOT_NULL(_robot);
}
@@ -96,7 +97,8 @@ namespace armarx::nav::locgrapheditor
}
- Eigen::Matrix4f robotvisu::Connection::getGlobalPose(bool synchronize)
+ Eigen::Matrix4f
+ robotvisu::Connection::getGlobalPose(bool synchronize)
{
_synchronize(synchronize);
return _robot->getGlobalPose();
@@ -111,15 +113,14 @@ namespace armarx::nav::locgrapheditor
}
- viz::Robot robotvisu::Connection::vizRobot(
- const std::string& id,
- bool synchronize)
+ viz::Robot
+ robotvisu::Connection::vizRobot(const std::string& id, bool synchronize)
{
_synchronize(synchronize);
viz::Robot robot = viz::Robot(id)
- .file("", _filename)
- .pose(getGlobalPose(false))
- .joints(getJointValues(false));
+ .file("", _filename)
+ .pose(getGlobalPose(false))
+ .joints(getJointValues(false));
if (_settings and _settings->useCollisionModel())
{
@@ -129,20 +130,23 @@ namespace armarx::nav::locgrapheditor
}
- RobotStateComponentInterfacePrx robotvisu::Connection::getRobotStateComponent() const
+ RobotStateComponentInterfacePrx
+ robotvisu::Connection::getRobotStateComponent() const
{
ARMARX_CHECK_NOT_NULL(_robotStateComponent);
return *_robotStateComponent;
}
- std::string robotvisu::Connection::getConnectedName() const
+ std::string
+ robotvisu::Connection::getConnectedName() const
{
return getRobotStateComponent()->ice_getIdentity().name;
}
- void robotvisu::Connection::_synchronize(bool synchronize)
+ void
+ robotvisu::Connection::_synchronize(bool synchronize)
{
if (synchronize)
{
@@ -151,10 +155,8 @@ namespace armarx::nav::locgrapheditor
}
-
RobotVisuWidget::RobotVisuWidget(ManagedIceObject& component, QWidget* parent) :
- QWidget(parent),
- _component(component)
+ QWidget(parent), _component(component)
{
_statusLabel = new QLabel("Not connected", this);
_connectButton = new QPushButton("Connect ...", this);
@@ -171,51 +173,54 @@ namespace armarx::nav::locgrapheditor
layout->addWidget(_settings);
- connect(_connectButton, &QPushButton::pressed,
- this, &This::connectToRobot);
+ connect(_connectButton, &QPushButton::pressed, this, &This::connectToRobot);
- connect(_settings, &robotvisu::SettingsWidget::changed,
- this, &This::settingsChanged);
+ connect(_settings, &robotvisu::SettingsWidget::changed, this, &This::settingsChanged);
- connect(this, &This::connected, this, [this]()
- {
- QString name = QString::fromStdString(connection().getConnectedName());
- _statusLabel->setText("Connected to '" + name + "'");
- });
- connect(this, &This::connected, this, [this]()
- {
- _settings->setEnabled(true);
- });
+ connect(this,
+ &This::connected,
+ this,
+ [this]()
+ {
+ QString name = QString::fromStdString(connection().getConnectedName());
+ _statusLabel->setText("Connected to '" + name + "'");
+ });
+ connect(this, &This::connected, this, [this]() { _settings->setEnabled(true); });
}
- bool RobotVisuWidget::isEnabled() const
+ bool
+ RobotVisuWidget::isEnabled() const
{
return isConnected() and _settings->isEnabled();
}
- bool RobotVisuWidget::isConnected() const
+ bool
+ RobotVisuWidget::isConnected() const
{
return _connection.has_value();
}
- robotvisu::Connection& RobotVisuWidget::connection()
+ robotvisu::Connection&
+ RobotVisuWidget::connection()
{
return _connection.value();
}
- void RobotVisuWidget::connectToRobot()
+ void
+ RobotVisuWidget::connectToRobot()
{
- ConnectDialog<RobotStateComponentInterfacePrx> dialog("*RobotStateComponent", _component, this);
+ ConnectDialog<RobotStateComponentInterfacePrx> dialog(
+ "*RobotStateComponent", _component, this);
switch (dialog.exec())
{
- case QDialog::Accepted:
- break;
- case QDialog::Rejected:
- return;
+ case QDialog::Accepted:
+ break;
+ case QDialog::Rejected:
+ return;
}
auto robotStateComponent = dialog.getProxy();
@@ -226,4 +231,4 @@ namespace armarx::nav::locgrapheditor
}
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
index 7a926922..1a7083dc 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
@@ -21,15 +21,14 @@
#pragma once
-#include <Eigen/Core>
-
#include <QWidget>
-
#include <map>
#include <memory>
#include <optional>
#include <string>
+#include <Eigen/Core>
+
class QCheckBox;
class QLabel;
@@ -42,7 +41,8 @@ namespace VirtualRobot
}
namespace IceInternal
{
- template<typename T> class ProxyHandle;
+ template <typename T>
+ class ProxyHandle;
}
namespace IceProxy::armarx
{
@@ -51,15 +51,16 @@ namespace IceProxy::armarx
namespace armarx
{
class ManagedIceObject;
- using RobotStateComponentInterfacePrx = ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface>;
-}
+ using RobotStateComponentInterfacePrx =
+ ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface>;
+} // namespace armarx
namespace armarx::viz
{
class Robot;
}
-namespace armarx::nav::locgrapheditor::robotvisu
+namespace armarx::navigation::locgrapheditor::robotvisu
{
class SettingsWidget : public QWidget
@@ -68,7 +69,6 @@ namespace armarx::nav::locgrapheditor::robotvisu
using This = SettingsWidget;
public:
-
SettingsWidget(QWidget* parent = nullptr);
@@ -82,18 +82,14 @@ namespace armarx::nav::locgrapheditor::robotvisu
private:
-
QCheckBox* _enabled = nullptr;
QCheckBox* _collisionModel = nullptr;
-
};
-
class Connection
{
public:
-
Connection(RobotStateComponentInterfacePrx robotStateComponent,
robotvisu::SettingsWidget* settings);
~Connection();
@@ -110,7 +106,6 @@ namespace armarx::nav::locgrapheditor::robotvisu
private:
-
void _synchronize(bool synchronize);
std::unique_ptr<RobotStateComponentInterfacePrx> _robotStateComponent;
@@ -118,12 +113,11 @@ namespace armarx::nav::locgrapheditor::robotvisu
std::string _filename;
VirtualRobot::RobotPtr _robot;
-
};
-}
+} // namespace armarx::navigation::locgrapheditor::robotvisu
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class RobotVisuWidget : public QWidget
@@ -132,7 +126,6 @@ namespace armarx::nav::locgrapheditor
using This = RobotVisuWidget;
public:
-
RobotVisuWidget(ManagedIceObject& component, QWidget* parent = nullptr);
@@ -155,14 +148,12 @@ namespace armarx::nav::locgrapheditor
private:
-
ManagedIceObject& _component;
std::optional<robotvisu::Connection> _connection;
QLabel* _statusLabel = nullptr;
QPushButton* _connectButton = nullptr;
robotvisu::SettingsWidget* _settings = nullptr;
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
index d228ec6c..f1a25de4 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -21,11 +21,6 @@
*/
#include "VertexDataWidget.h"
-#include "RobotVisuWidget.h"
-
-#include <RobotAPI/libraries/armem/core/MemoryID.h>
-#include <RobotAPI/libraries/armem/core/aron_conversions.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
#include <QDoubleSpinBox>
#include <QFormLayout>
@@ -42,8 +37,14 @@
#include <SimoxUtility/math/convert/rpy_to_mat4f.h>
#include <SimoxUtility/math/pose/pose.h>
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+
+#include "RobotVisuWidget.h"
+
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
VertexDataWidget::VertexDataWidget()
@@ -70,7 +71,8 @@ namespace armarx::nav::locgrapheditor
_useCurrentRobotPose = new QPushButton("Use Current Robot Pose");
_useCurrentRobotPose->setEnabled(false);
- _useCurrentRobotPose->setToolTip("Use the robot's current global pose (requires connection to robot state).");
+ _useCurrentRobotPose->setToolTip(
+ "Use the robot's current global pose (requires connection to robot state).");
for (QDoubleSpinBox* pos : _positionSpinBoxes())
@@ -112,33 +114,34 @@ namespace armarx::nav::locgrapheditor
for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
{
- connect(angleUnit, &QRadioButton::toggled,
- this, &This::_updateAngleUnit);
+ connect(angleUnit, &QRadioButton::toggled, this, &This::_updateAngleUnit);
}
- connect(frame, &QLineEdit::editingFinished,
- this, &This::_updateVertexAttribs);
+ connect(frame, &QLineEdit::editingFinished, this, &This::_updateVertexAttribs);
for (QDoubleSpinBox* spinBox : _allSpinBoxes())
{
- connect(spinBox, QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
- this, &This::_updateVertexAttribs);
+ connect(spinBox,
+ QOverload<qreal>::of(&QDoubleSpinBox::valueChanged),
+ this,
+ &This::_updateVertexAttribs);
}
- connect(_useCurrentRobotPose, &QPushButton::pressed,
- this, &This::_setFromCurrentRobotPose);
+ connect(_useCurrentRobotPose, &QPushButton::pressed, this, &This::_setFromCurrentRobotPose);
angleUnitDeg->click();
}
- std::optional<GuiGraph::Vertex> VertexDataWidget::vertex()
+ std::optional<GuiGraph::Vertex>
+ VertexDataWidget::vertex()
{
return _vertex;
}
- void VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
+ void
+ VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
{
QSignalBlocker blocker(this);
@@ -148,20 +151,23 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::clearVertex()
+ void
+ VertexDataWidget::clearVertex()
{
_vertex = std::nullopt;
setEnabled(false);
}
- Eigen::Vector3d VertexDataWidget::xyz() const
+ Eigen::Vector3d
+ VertexDataWidget::xyz() const
{
- return { x->value(), y->value(), z->value() };
+ return {x->value(), y->value(), z->value()};
}
- Eigen::Vector3d VertexDataWidget::rpyDeg() const
+ Eigen::Vector3d
+ VertexDataWidget::rpyDeg() const
{
Eigen::Vector3d raw = _rpyRaw();
if (angleUnitRad->isChecked())
@@ -175,7 +181,8 @@ namespace armarx::nav::locgrapheditor
}
- Eigen::Vector3d VertexDataWidget::rpyRad() const
+ Eigen::Vector3d
+ VertexDataWidget::rpyRad() const
{
Eigen::Vector3d raw = _rpyRaw();
if (angleUnitDeg->isChecked())
@@ -189,25 +196,29 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::setRobotConnection(robotvisu::Connection* connection)
+ void
+ VertexDataWidget::setRobotConnection(robotvisu::Connection* connection)
{
this->_robotConnection = connection;
_useCurrentRobotPose->setEnabled(_robotConnection != nullptr);
}
- void VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
+ void
+ VertexDataWidget::_setFromVertex(const GuiGraph::Vertex& vertex)
{
const VertexData& attrib = vertex.attrib();
name->setText(QString::fromStdString(attrib.getName()));
- locationID->setText(QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
+ locationID->setText(
+ QString::fromStdString(aron::fromAron<armem::MemoryID>(attrib.aron.locationID).str()));
frame->setText("<WIP>");
_setPose(attrib.getPose().cast<qreal>());
}
- void VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
+ void
+ VertexDataWidget::_getToVertex(GuiGraph::Vertex& vertex)
{
VertexData& attrib = vertex.attrib();
@@ -220,7 +231,8 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_updateAngleUnit()
+ void
+ VertexDataWidget::_updateAngleUnit()
{
std::function<double(double)> convertValue;
QString suffix = " \u00b0";
@@ -231,10 +243,7 @@ namespace armarx::nav::locgrapheditor
if (angleUnitRad->isChecked())
{
- convertValue = [](double deg)
- {
- return simox::math::deg_to_rad(deg);
- };
+ convertValue = [](double deg) { return simox::math::deg_to_rad(deg); };
suffix = " rad";
min = simox::math::deg_to_rad(min);
max = simox::math::deg_to_rad(max);
@@ -244,10 +253,7 @@ namespace armarx::nav::locgrapheditor
else
{
ARMARX_CHECK(angleUnitDeg->isChecked());
- convertValue = [](double rad)
- {
- return simox::math::rad_to_deg(rad);
- };
+ convertValue = [](double rad) { return simox::math::rad_to_deg(rad); };
}
std::vector<QSignalBlocker> blockers;
@@ -270,7 +276,8 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_updateVertexAttribs()
+ void
+ VertexDataWidget::_updateVertexAttribs()
{
if (not signalsBlocked() and _vertex.has_value())
{
@@ -281,32 +288,37 @@ namespace armarx::nav::locgrapheditor
}
- std::vector<QDoubleSpinBox*> VertexDataWidget::_positionSpinBoxes()
+ std::vector<QDoubleSpinBox*>
+ VertexDataWidget::_positionSpinBoxes()
{
- return { x, y, z };
+ return {x, y, z};
}
- std::vector<QDoubleSpinBox*> VertexDataWidget::_angleSpinBoxes()
+ std::vector<QDoubleSpinBox*>
+ VertexDataWidget::_angleSpinBoxes()
{
- return { roll, pitch, yaw };
+ return {roll, pitch, yaw};
}
- std::vector<QDoubleSpinBox*> VertexDataWidget::_allSpinBoxes()
+ std::vector<QDoubleSpinBox*>
+ VertexDataWidget::_allSpinBoxes()
{
- return { x, y, z, roll, pitch, yaw };
+ return {x, y, z, roll, pitch, yaw};
}
- void VertexDataWidget::_setPose(const Eigen::Matrix4d& pose)
+ void
+ VertexDataWidget::_setPose(const Eigen::Matrix4d& pose)
{
_setXyz(simox::math::position(pose));
_setRpyRad(simox::math::mat4f_to_rpy(pose));
}
- void VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
+ void
+ VertexDataWidget::_setXyz(const Eigen::Vector3d& xyz)
{
x->setValue(xyz.x());
y->setValue(xyz.y());
@@ -314,13 +326,15 @@ namespace armarx::nav::locgrapheditor
}
- Eigen::Vector3d VertexDataWidget::_rpyRaw() const
+ Eigen::Vector3d
+ VertexDataWidget::_rpyRaw() const
{
- return { roll->value(), pitch->value(), yaw->value() };
+ return {roll->value(), pitch->value(), yaw->value()};
}
- void VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
+ void
+ VertexDataWidget::_setRpyRaw(const Eigen::Vector3d& rpy) const
{
roll->setValue(rpy(0));
pitch->setValue(rpy(1));
@@ -328,7 +342,8 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
+ void
+ VertexDataWidget::_setRpyDeg(const Eigen::Vector3d& rpyDeg) const
{
if (angleUnitDeg->isChecked())
{
@@ -341,7 +356,8 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
+ void
+ VertexDataWidget::_setRpyRad(const Eigen::Vector3d& rpyRad) const
{
if (angleUnitDeg->isChecked())
{
@@ -354,7 +370,8 @@ namespace armarx::nav::locgrapheditor
}
- void VertexDataWidget::_setFromCurrentRobotPose()
+ void
+ VertexDataWidget::_setFromCurrentRobotPose()
{
ARMARX_CHECK_NOT_NULL(_robotConnection);
{
@@ -364,4 +381,4 @@ namespace armarx::nav::locgrapheditor
_updateVertexAttribs();
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
index 21ff9daf..6b5c2587 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -21,11 +21,11 @@
#pragma once
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <QWidget>
#include <Eigen/Core>
-#include <QWidget>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
class QLineEdit;
@@ -34,12 +34,11 @@ class QPushButton;
class QRadioButton;
-
-namespace armarx::nav::locgrapheditor::robotvisu
+namespace armarx::navigation::locgrapheditor::robotvisu
{
class Connection;
}
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class VertexDataWidget : public QWidget
@@ -49,7 +48,6 @@ namespace armarx::nav::locgrapheditor
public:
-
VertexDataWidget();
@@ -77,7 +75,6 @@ namespace armarx::nav::locgrapheditor
private:
-
void _setFromVertex(const GuiGraph::Vertex& vertex);
void _getToVertex(GuiGraph::Vertex& vertex);
@@ -97,7 +94,6 @@ namespace armarx::nav::locgrapheditor
private:
-
std::optional<GuiGraph::Vertex> _vertex;
@@ -119,7 +115,6 @@ namespace armarx::nav::locgrapheditor
robotvisu::Connection* _robotConnection = nullptr;
QPushButton* _useCurrentRobotPose = nullptr;
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
index b1a5ceed..ecaaca3c 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -20,17 +20,18 @@
*/
#include "VertexTableWidget.h"
-#include "utils.h"
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/logging/Logging.h>
#include <QAction>
#include <QHeaderView>
#include <QMenu>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "utils.h"
+
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
VertexTableWidget::VertexTableWidget()
@@ -51,8 +52,7 @@ namespace armarx::nav::locgrapheditor
setStyleSheet(styleSheet);
setContextMenuPolicy(Qt::CustomContextMenu);
- connect(this, &This::customContextMenuRequested,
- this, &This::makeContextMenu);
+ connect(this, &This::customContextMenuRequested, this, &This::makeContextMenu);
}
@@ -70,7 +70,7 @@ namespace armarx::nav::locgrapheditor
for (int col = 0; col < 4; ++col)
{
// Just fill with vanilla items, they will get values in the update.
- QTableWidgetItem* item = new QTableWidgetItem {};
+ QTableWidgetItem* item = new QTableWidgetItem{};
setItem(row, col, item);
if (col >= 1)
@@ -177,39 +177,39 @@ namespace armarx::nav::locgrapheditor
menu.addSection("Selected pair of locations");
// Generate actions for connecting these two nodes.
- std::sort(items.begin(), items.end(), [](QTableWidgetItem* first, QTableWidgetItem* second)
- {
- return _nameOf(first) < _nameOf(second);
- });
+ std::sort(items.begin(),
+ items.end(),
+ [](QTableWidgetItem* first, QTableWidgetItem* second)
+ { return _nameOf(first) < _nameOf(second); });
QTableWidgetItem* first = items[0];
QTableWidgetItem* second = items[1];
- connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowRight + " '" + _nameOf(second) + "'"),
- &QAction::triggered, [this, first, second]()
- {
- emit newEdgesRequested({{first, second}});
- });
- connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowLeft + " '" + _nameOf(second) + "'"),
- &QAction::triggered, [this, first, second]()
- {
- emit newEdgesRequested({{second, first}});
- });
- connect(menu.addAction("Add edges '" + _nameOf(first) + "' " + utils::arrowBoth + " '" + _nameOf(second) + "'"),
- &QAction::triggered, [this, first, second]()
- {
- emit newEdgesRequested({{first, second}, {second, first}});
- });
+ connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowRight + " '" +
+ _nameOf(second) + "'"),
+ &QAction::triggered,
+ [this, first, second]() {
+ emit newEdgesRequested({{first, second}});
+ });
+ connect(menu.addAction("Add edge '" + _nameOf(first) + "' " + utils::arrowLeft + " '" +
+ _nameOf(second) + "'"),
+ &QAction::triggered,
+ [this, first, second]() {
+ emit newEdgesRequested({{second, first}});
+ });
+ connect(menu.addAction("Add edges '" + _nameOf(first) + "' " + utils::arrowBoth + " '" +
+ _nameOf(second) + "'"),
+ &QAction::triggered,
+ [this, first, second]() {
+ emit newEdgesRequested({{first, second}, {second, first}});
+ });
}
// Partners via context menu
if (items.size() > 0)
{
- QString edges = items.size() == 1
- ? "edge"
- : "edges";
- QString desc = items.size() == 1
- ? "'" + _nameOf(items[0]) + "'"
- : QString::number(items.size()) + " locations";
+ QString edges = items.size() == 1 ? "edge" : "edges";
+ QString desc = items.size() == 1 ? "'" + _nameOf(items[0]) + "'"
+ : QString::number(items.size()) + " locations";
if (items.size() == 1)
{
@@ -223,7 +223,8 @@ namespace armarx::nav::locgrapheditor
using Item = QTableWidgetItem;
using ListOfEdges = QList<QPair<Item*, Item*>>;
- using AppendFunc = std::function<void(ListOfEdges& edges, Item* selected, Item* action)>;
+ using AppendFunc =
+ std::function<void(ListOfEdges & edges, Item * selected, Item * action)>;
auto addBulkAddEdgeActions = [this, &items](QMenu* submenu, AppendFunc appendFunc)
{
@@ -235,70 +236,73 @@ namespace armarx::nav::locgrapheditor
for (int row = 0; row < rowCount(); ++row)
{
QTableWidgetItem* action = this->item(row, 0);
- if (items.count(action) == 0) // Do no generate self-edges
+ if (items.count(action) == 0) // Do no generate self-edges
{
QAction* a = submenu->addAction(_nameOf(action));
- connect(a, &QAction::triggered, this,
+ connect(a,
+ &QAction::triggered,
+ this,
[this, items, action, appendFunc]()
- {
- QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges;
- for (auto* selected : items)
- {
- appendFunc(edges, selected, action);
- }
- emit newEdgesRequested(edges);
- });
+ {
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges;
+ for (auto* selected : items)
+ {
+ appendFunc(edges, selected, action);
+ }
+ emit newEdgesRequested(edges);
+ });
}
}
};
- addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
- [](ListOfEdges& edges, Item* selected, Item* action)
- {
- edges.append({selected, action});
- });
- addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
- [](ListOfEdges& edges, Item* selected, Item* action)
- {
- edges.append({action, selected});
- });
- addBulkAddEdgeActions(menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
- [](ListOfEdges& edges, Item* selected, Item* action)
- {
- edges.append({selected, action});
- edges.append({action, selected});
- });
+ addBulkAddEdgeActions(
+ menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowRight + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action) {
+ edges.append({selected, action});
+ });
+ addBulkAddEdgeActions(
+ menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowLeft + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action) {
+ edges.append({action, selected});
+ });
+ addBulkAddEdgeActions(
+ menu.addMenu("Add " + edges + " " + desc + " " + utils::arrowBoth + " ..."),
+ [](ListOfEdges& edges, Item* selected, Item* action)
+ {
+ edges.append({selected, action});
+ edges.append({action, selected});
+ });
- auto connectBulkRemoveEdgeAction = [this, &items](
- QAction* action, utils::EdgeDirection edgeDirection)
+ auto connectBulkRemoveEdgeAction =
+ [this, &items](QAction* action, utils::EdgeDirection edgeDirection)
{
- connect(action, &QAction::triggered, this, [this, items, edgeDirection]()
- {
- emit edgeRemovalRequested(items, edgeDirection);
- });
+ connect(action,
+ &QAction::triggered,
+ this,
+ [this, items, edgeDirection]()
+ { emit edgeRemovalRequested(items, edgeDirection); });
};
connectBulkRemoveEdgeAction(
- menu.addAction("Remove all edges " + utils::arrowRight + " " + desc),
- utils::EdgeDirection::To);
+ menu.addAction("Remove all edges " + utils::arrowRight + " " + desc),
+ utils::EdgeDirection::To);
connectBulkRemoveEdgeAction(
- menu.addAction("Remove all edges " + utils::arrowLeft + " " + desc),
- utils::EdgeDirection::From);
+ menu.addAction("Remove all edges " + utils::arrowLeft + " " + desc),
+ utils::EdgeDirection::From);
connectBulkRemoveEdgeAction(
- menu.addAction("Remove all edges " + utils::arrowBoth + " " + desc),
- utils::EdgeDirection::Bidirectional);
+ menu.addAction("Remove all edges " + utils::arrowBoth + " " + desc),
+ utils::EdgeDirection::Bidirectional);
}
menu.addSection("Manage Locations");
- connect(menu.addAction("Create new location ..."), &QAction::triggered, this,
- [this]()
- {
- emit newVertexRequested();
- });
+ connect(menu.addAction("Create new location ..."),
+ &QAction::triggered,
+ this,
+ [this]() { emit newVertexRequested(); });
menu.exec(mapToGlobal(pos));
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
index 9224c249..0ce3c98d 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -21,20 +21,20 @@
#pragma once
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
-
#include <QColor>
#include <QList>
#include <QPair>
#include <QTableWidget>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h>
+
-namespace armarx::nav::locgrapheditor::utils
+namespace armarx::navigation::locgrapheditor::utils
{
enum class EdgeDirection;
}
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
class VertexTableWidget : public QTableWidget
{
@@ -43,7 +43,6 @@ namespace armarx::nav::locgrapheditor
public:
-
VertexTableWidget();
@@ -62,7 +61,8 @@ namespace armarx::nav::locgrapheditor
void newVertexRequested();
void newEdgesRequested(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges);
- void edgeRemovalRequested(QList<QTableWidgetItem*> vertexItems, utils::EdgeDirection direction);
+ void edgeRemovalRequested(QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction);
public slots:
@@ -71,16 +71,12 @@ namespace armarx::nav::locgrapheditor
private:
-
static QString _nameOf(QTableWidgetItem* item);
public:
-
QColor bgColorDefault = default_colors::tableBackgroundDefault;
QColor bgColorSelected = default_colors::tableBackgroundSelected;
-
-
};
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
index fd026201..130485d7 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.cpp
@@ -24,10 +24,10 @@
#include <QColor>
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
const QColor default_colors::tableBackgroundDefault = QColor::fromRgb(255, 255, 255);
const QColor default_colors::tableBackgroundSelected = QColor::fromRgb(255, 210, 160);
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
index 0a355fa6..e8067a33 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/default_colors.h
@@ -24,10 +24,10 @@
class QColor;
-namespace armarx::nav::locgrapheditor::default_colors
+namespace armarx::navigation::locgrapheditor::default_colors
{
extern const QColor tableBackgroundDefault;
extern const QColor tableBackgroundSelected;
-}
+} // namespace armarx::navigation::locgrapheditor::default_colors
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
index 5edd0120..5baadfde 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
@@ -1,17 +1,16 @@
#pragma once
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
class Scene;
class Widget;
-}
+} // namespace armarx::navigation::locgrapheditor::graph_scene
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
using GraphScene = graph_scene::Scene;
using GraphSceneWidget = graph_scene::Widget;
-}
-
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
index d6dcf9b7..ee81158b 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
@@ -7,17 +7,17 @@
#include <QHBoxLayout>
#include <QLabel>
#include <QPushButton>
-
#include <cmath>
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
ControlWidget::ControlWidget()
{
- _angle.turnClockwise = new QPushButton("\u21bb"); // https://unicode-table.com/de/21BB/
- _angle.turnCounterClockwise = new QPushButton("\u21ba"); // https://unicode-table.com/de/21BA/
+ _angle.turnClockwise = new QPushButton("\u21bb"); // https://unicode-table.com/de/21BB/
+ _angle.turnCounterClockwise =
+ new QPushButton("\u21ba"); // https://unicode-table.com/de/21BA/
std::vector<QPushButton*> turnButtons;
for (QPushButton* button : turnButtons)
{
@@ -61,46 +61,53 @@ namespace armarx::nav::locgrapheditor::graph_scene
// Connect
- connect(_angle.value, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
- this, &This::angleChanged);
-
- connect(_angle.turnClockwise, &QPushButton::pressed, [this]()
- {
- qreal newangle = std::fmod(angle() + _angle.rotateStepSize, 360.0);
- setAngle(newangle);
- });
- connect(_angle.turnCounterClockwise, &QPushButton::pressed, [this]()
- {
- setAngle(std::fmod(angle() - _angle.rotateStepSize, 360.0));
- });
-
- connect(_zoom.value, QOverload<double>::of(&QDoubleSpinBox::valueChanged),
- this, &This::angleChanged);
- connect(_zoom.audoAdjust, &QPushButton::pressed,
- this, &This::zoomAutoAdjustRequested);
+ connect(_angle.value,
+ QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+ this,
+ &This::angleChanged);
+
+ connect(_angle.turnClockwise,
+ &QPushButton::pressed,
+ [this]()
+ {
+ qreal newangle = std::fmod(angle() + _angle.rotateStepSize, 360.0);
+ setAngle(newangle);
+ });
+ connect(_angle.turnCounterClockwise,
+ &QPushButton::pressed,
+ [this]() { setAngle(std::fmod(angle() - _angle.rotateStepSize, 360.0)); });
+
+ connect(_zoom.value,
+ QOverload<double>::of(&QDoubleSpinBox::valueChanged),
+ this,
+ &This::angleChanged);
+ connect(_zoom.audoAdjust, &QPushButton::pressed, this, &This::zoomAutoAdjustRequested);
}
- qreal ControlWidget::angle()
+ qreal
+ ControlWidget::angle()
{
return _angle.value->value();
}
- void ControlWidget::setAngle(qreal angle)
+ void
+ ControlWidget::setAngle(qreal angle)
{
_angle.value->setValue(angle);
}
- qreal ControlWidget::zoom()
+ qreal
+ ControlWidget::zoom()
{
return _zoom.value->value();
}
- void ControlWidget::setZoom(qreal zoom)
+ void
+ ControlWidget::setZoom(qreal zoom)
{
_zoom.value->setValue(zoom);
}
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
index 64d4270e..7a2af024 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
@@ -7,7 +7,7 @@ class QHBoxLayout;
class QPushButton;
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
class ControlWidget : public QWidget
@@ -17,7 +17,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
public:
-
ControlWidget();
qreal angle();
@@ -35,7 +34,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
public:
-
struct Angle
{
QPushButton* turnClockwise = nullptr;
@@ -52,8 +50,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
QPushButton* audoAdjust = nullptr;
};
Zoom _zoom;
-
};
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
index 330a15dc..dcd52068 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -1,13 +1,13 @@
#include "Scene.h"
+#include <Eigen/Core>
+
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <SemanticObjectRelations/Shapes/Shape.h>
-#include <Eigen/Core>
-
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
QGraphicsEllipseItem*
@@ -15,13 +15,8 @@ namespace armarx::nav::locgrapheditor::graph_scene
{
const Eigen::Matrix4d pose = attrib.getPose().cast<qreal>();
- GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem
- {
- [this, vertexID]() { emit vertexSelected(vertexID); },
- pose(0, 3),
- - pose(1, 3),
- 2, 2
- };
+ GraphVisualizerGraphicsEllipseItem* item = new GraphVisualizerGraphicsEllipseItem{
+ [this, vertexID]() { emit vertexSelected(vertexID); }, pose(0, 3), -pose(1, 3), 2, 2};
addItem(item);
// setToolTip on graphicsItem does not work
@@ -34,30 +29,33 @@ namespace armarx::nav::locgrapheditor::graph_scene
QGraphicsLineItem*
- Scene::addEdge(semrel::ShapeID sourceID, const graph::VertexAttribs& sourceAttrib,
- semrel::ShapeID targetID, const graph::VertexAttribs& targetAttrib)
+ Scene::addEdge(semrel::ShapeID sourceID,
+ const graph::VertexAttribs& sourceAttrib,
+ semrel::ShapeID targetID,
+ const graph::VertexAttribs& targetAttrib)
{
Eigen::Matrix4d sourcePose = sourceAttrib.getPose().cast<qreal>();
Eigen::Matrix4d targetPose = targetAttrib.getPose().cast<qreal>();
- GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem
- {
+ GraphVisualizerGraphicsLineItem* item = new GraphVisualizerGraphicsLineItem{
[this, sourceID, targetID]() { emit edgeSelected(sourceID, targetID); },
- sourcePose(0, 3), - sourcePose(1, 3),
- targetPose(0, 3), - targetPose(1, 3)
- };
+ sourcePose(0, 3),
+ -sourcePose(1, 3),
+ targetPose(0, 3),
+ -targetPose(1, 3)};
addItem(item);
// setToolTip on item does not work
std::stringstream toolTip;
- toolTip << "Edge '" << sourceAttrib.getName() << "' -> '" << targetAttrib.getName();
+ toolTip << "Edge '" << sourceAttrib.getName() << "' -> '" << targetAttrib.getName();
item->setToolTip(QString::fromStdString(toolTip.str()));
return item;
}
- void Scene::updateVertex(GuiGraph::Vertex& vertex)
+ void
+ Scene::updateVertex(GuiGraph::Vertex& vertex)
{
QGraphicsEllipseItem* item = vertex.attrib().graphicsItem;
ARMARX_CHECK_NOT_NULL(item);
@@ -67,20 +65,21 @@ namespace armarx::nav::locgrapheditor::graph_scene
QColor color = vertex.attrib().highlighted ? colorSelected : colorDefault;
double lineWidth = vertex.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
- item->setPen(QPen {color});
- item->setBrush(QBrush {color});
- item->setRect( pose(0, 3) - lineWidth * verticesScaleFactor / 2,
- - pose(1, 3) - lineWidth * verticesScaleFactor / 2,
+ item->setPen(QPen{color});
+ item->setBrush(QBrush{color});
+ item->setRect(pose(0, 3) - lineWidth * verticesScaleFactor / 2,
+ -pose(1, 3) - lineWidth * verticesScaleFactor / 2,
lineWidth * verticesScaleFactor,
lineWidth * verticesScaleFactor);
}
- void Scene::updateEdge(GuiGraph::Edge& edge)
+ void
+ Scene::updateEdge(GuiGraph::Edge& edge)
{
QGraphicsLineItem* item = edge.attrib().graphicsItem;
ARMARX_CHECK_NOT_NULL(item)
- << edge.source().attrib().getName() << " -> " << edge.target().attrib().getName();
+ << edge.source().attrib().getName() << " -> " << edge.target().attrib().getName();
Eigen::Matrix4d sourcePose = edge.source().attrib().getPose().cast<qreal>();
Eigen::Matrix4d targetPose = edge.target().attrib().getPose().cast<qreal>();
@@ -88,16 +87,16 @@ namespace armarx::nav::locgrapheditor::graph_scene
QColor color = edge.attrib().highlighted ? colorSelected : colorDefault;
double lineWidth = edge.attrib().highlighted ? lineWidthSelected : lineWidthDefault;
- QPen pen {color};
+ QPen pen{color};
pen.setWidthF(lineWidth * lineScaleFactor);
item->setPen(pen);
- item->setLine(sourcePose(0, 3), - sourcePose(1, 3),
- targetPose(0, 3), - targetPose(1, 3));
+ item->setLine(sourcePose(0, 3), -sourcePose(1, 3), targetPose(0, 3), -targetPose(1, 3));
}
- void Scene::removeVertex(QGraphicsEllipseItem*& item)
+ void
+ Scene::removeVertex(QGraphicsEllipseItem*& item)
{
removeItem(item);
delete item;
@@ -105,7 +104,8 @@ namespace armarx::nav::locgrapheditor::graph_scene
}
- void Scene::removeEdge(QGraphicsLineItem*& item)
+ void
+ Scene::removeEdge(QGraphicsLineItem*& item)
{
removeItem(item);
delete item;
@@ -114,12 +114,13 @@ namespace armarx::nav::locgrapheditor::graph_scene
GraphVisualizerGraphicsEllipseItem::GraphVisualizerGraphicsEllipseItem(
- std::function<void(void)> onDoubleClicked,
- qreal x, qreal y,
- qreal width, qreal height,
- QGraphicsItem* parent) :
- QGraphicsEllipseItem {x, y, width, height, parent},
- onDoubleClicked{onDoubleClicked}
+ std::function<void(void)> onDoubleClicked,
+ qreal x,
+ qreal y,
+ qreal width,
+ qreal height,
+ QGraphicsItem* parent) :
+ QGraphicsEllipseItem{x, y, width, height, parent}, onDoubleClicked{onDoubleClicked}
{
}
@@ -129,19 +130,21 @@ namespace armarx::nav::locgrapheditor::graph_scene
}
- void GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ void
+ GraphVisualizerGraphicsEllipseItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
{
onDoubleClicked();
}
-
GraphVisualizerGraphicsLineItem::GraphVisualizerGraphicsLineItem(
- std::function<void(void)> onDoubleClicked,
- qreal x1, qreal y1, qreal x2, qreal y2,
- QGraphicsItem* parent) :
- QGraphicsLineItem {x1, y1, x2, y2, parent},
- onDoubleClicked{onDoubleClicked}
+ std::function<void(void)> onDoubleClicked,
+ qreal x1,
+ qreal y1,
+ qreal x2,
+ qreal y2,
+ QGraphicsItem* parent) :
+ QGraphicsLineItem{x1, y1, x2, y2, parent}, onDoubleClicked{onDoubleClicked}
{
}
@@ -151,10 +154,10 @@ namespace armarx::nav::locgrapheditor::graph_scene
}
- void GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
+ void
+ GraphVisualizerGraphicsLineItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent*)
{
onDoubleClicked();
}
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
index 6afc6a7f..390c1052 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -1,27 +1,25 @@
#pragma once
-#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
-#include <armarx/navigation/graph/Graph.h>
-
-#include <QGraphicsScene>
#include <QGraphicsEllipseItem>
#include <QGraphicsLineItem>
-
+#include <QGraphicsScene>
#include <functional>
+#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
namespace semrel
{
struct ShapeID;
}
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
class Scene : public QGraphicsScene
{
Q_OBJECT
public:
-
using QGraphicsScene::QGraphicsScene;
template <class VertexT>
@@ -30,20 +28,23 @@ namespace armarx::nav::locgrapheditor::graph_scene
{
return addVertex(vertex.objectID(), vertex.attrib());
}
- QGraphicsEllipseItem*
- addVertex(semrel::ShapeID vertexID, const graph::VertexAttribs& attribs);
+ QGraphicsEllipseItem* addVertex(semrel::ShapeID vertexID,
+ const graph::VertexAttribs& attribs);
template <class EdgeT>
QGraphicsLineItem*
addEdge(const EdgeT& edge)
{
- return addEdge(edge.sourceObjectID(), edge.source().attrib(),
- edge.targetObjectID(), edge.target().attrib());
+ return addEdge(edge.sourceObjectID(),
+ edge.source().attrib(),
+ edge.targetObjectID(),
+ edge.target().attrib());
}
- QGraphicsLineItem*
- addEdge(semrel::ShapeID sourceID, const graph::VertexAttribs& sourceAttrib,
- semrel::ShapeID targetID, const graph::VertexAttribs& targetAttrib);
+ QGraphicsLineItem* addEdge(semrel::ShapeID sourceID,
+ const graph::VertexAttribs& sourceAttrib,
+ semrel::ShapeID targetID,
+ const graph::VertexAttribs& targetAttrib);
void updateVertex(GuiGraph::Vertex& vertex);
@@ -63,7 +64,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
public:
-
double lineWidthDefault = 5;
double lineWidthSelected = 10;
@@ -73,11 +73,9 @@ namespace armarx::nav::locgrapheditor::graph_scene
QColor colorDefault = QColor::fromRgb(128, 128, 255);
QColor colorSelected = QColor::fromRgb(255, 128, 0);
-
};
-
/**
* @brief Required to override the double click event.
* This is required to toggle the select state.
@@ -85,26 +83,24 @@ namespace armarx::nav::locgrapheditor::graph_scene
class GraphVisualizerGraphicsEllipseItem : public QGraphicsEllipseItem
{
public:
-
- GraphVisualizerGraphicsEllipseItem(
- std::function<void(void)> onDoubleClicked,
- qreal x, qreal y, qreal width, qreal height,
- QGraphicsItem* parent = nullptr);
+ GraphVisualizerGraphicsEllipseItem(std::function<void(void)> onDoubleClicked,
+ qreal x,
+ qreal y,
+ qreal width,
+ qreal height,
+ QGraphicsItem* parent = nullptr);
~GraphVisualizerGraphicsEllipseItem() override;
protected:
-
void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
std::function<void(void)> onDoubleClicked;
-
};
-
/**
* @brief Required to override the double click event.
* This is required to toggle the select state.
@@ -112,26 +108,22 @@ namespace armarx::nav::locgrapheditor::graph_scene
class GraphVisualizerGraphicsLineItem : public QGraphicsLineItem
{
public:
-
- GraphVisualizerGraphicsLineItem(
- std::function<void(void)> onDoubleClicked,
- qreal x1, qreal y1,
- qreal x2, qreal y2,
- QGraphicsItem* parent = nullptr);
+ GraphVisualizerGraphicsLineItem(std::function<void(void)> onDoubleClicked,
+ qreal x1,
+ qreal y1,
+ qreal x2,
+ qreal y2,
+ QGraphicsItem* parent = nullptr);
~GraphVisualizerGraphicsLineItem() override;
protected:
-
-
void mouseDoubleClickEvent(QGraphicsSceneMouseEvent*) override;
std::function<void(void)> onDoubleClicked;
-
};
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
index 7cdcbcd8..cfcdc4d6 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
@@ -1,14 +1,15 @@
#include "Widget.h"
-#include "Scene.h"
-#include "ControlWidget.h"
#include <QGraphicsView>
#include <QHBoxLayout>
#include <QLabel>
#include <QVBoxLayout>
+#include "ControlWidget.h"
+#include "Scene.h"
+
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
Widget::Widget()
@@ -38,12 +39,9 @@ namespace armarx::nav::locgrapheditor::graph_scene
layout->addWidget(_view);
- connect(_control, &ControlWidget::angleChanged,
- this, &This::transform);
- connect(_control, &ControlWidget::zoomChanged,
- this, &This::transform);
- connect(_control, &ControlWidget::zoomAutoAdjustRequested,
- this, &This::autoAdjustZoom);
+ connect(_control, &ControlWidget::angleChanged, this, &This::transform);
+ connect(_control, &ControlWidget::zoomChanged, this, &This::transform);
+ connect(_control, &ControlWidget::zoomAutoAdjustRequested, this, &This::autoAdjustZoom);
// Initial transform.
@@ -96,19 +94,22 @@ namespace armarx::nav::locgrapheditor::graph_scene
}
- QGraphicsView* Widget::view()
+ QGraphicsView*
+ Widget::view()
{
return _view;
}
- Scene* Widget::scene()
+ Scene*
+ Widget::scene()
{
return _scene;
}
- void Widget::transform()
+ void
+ Widget::transform()
{
double angle = _control->angle();
double zoom = _control->zoom();
@@ -116,13 +117,13 @@ namespace armarx::nav::locgrapheditor::graph_scene
}
- void Widget::autoAdjustZoom()
+ void
+ Widget::autoAdjustZoom()
{
int margin = 3;
QRectF rect = _scene->sceneRect() + QMargins(margin, margin, margin, margin);
_view->fitInView(rect, Qt::AspectRatioMode::KeepAspectRatio);
- _control->setZoom(_view->transform().rotate(- _control->angle()).m11());
+ _control->setZoom(_view->transform().rotate(-_control->angle()).m11());
}
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
index c8a4b649..149f7cf4 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
@@ -5,7 +5,7 @@
class QGraphicsView;
-namespace armarx::nav::locgrapheditor::graph_scene
+namespace armarx::navigation::locgrapheditor::graph_scene
{
class ControlWidget;
class Scene;
@@ -18,7 +18,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
public:
-
Widget();
QGraphicsView* view();
@@ -32,7 +31,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
private:
-
/// Some buttons and input fields.
ControlWidget* _control = nullptr;
@@ -51,8 +49,6 @@ namespace armarx::nav::locgrapheditor::graph_scene
/// The view's rotation angle.
qreal _angle = 0;
-
};
-}
-
+} // namespace armarx::navigation::locgrapheditor::graph_scene
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
index 80b75d16..f39dcaa0 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -22,11 +22,10 @@
#include "utils.h"
#include <QTableWidget>
-
#include <set>
-namespace armarx::nav::locgrapheditor
+namespace armarx::navigation::locgrapheditor
{
// <- https://unicode-table.com/de/2190/
@@ -37,7 +36,6 @@ namespace armarx::nav::locgrapheditor
const QString utils::arrowBoth = QStringLiteral("\u21C4");
-
QList<QTableWidgetItem*>
utils::getSelectedItemsOfColumn(QTableWidget* widget, int column)
{
@@ -59,4 +57,4 @@ namespace armarx::nav::locgrapheditor
return list;
}
-}
+} // namespace armarx::navigation::locgrapheditor
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
index bd1358b3..1a879dd9 100644
--- a/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -28,7 +28,7 @@ class QTableWidget;
class QTableWidgetItem;
-namespace armarx::nav::locgrapheditor::utils
+namespace armarx::navigation::locgrapheditor::utils
{
/// <-
@@ -41,10 +41,12 @@ namespace armarx::nav::locgrapheditor::utils
enum class EdgeDirection
{
- From, To, Bidirectional
+ From,
+ To,
+ Bidirectional
};
QList<QTableWidgetItem*> getSelectedItemsOfColumn(QTableWidget* widget, int column);
-}
+} // namespace armarx::navigation::locgrapheditor::utils
diff --git a/source/armarx/navigation/location/CMakeLists.txt b/source/armarx/navigation/location/CMakeLists.txt
index 675377bd..45c89d57 100644
--- a/source/armarx/navigation/location/CMakeLists.txt
+++ b/source/armarx/navigation/location/CMakeLists.txt
@@ -1,32 +1,21 @@
-set(LIB_NAME ${PROJECT_NAME}Location)
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
+armarx_add_aron_library(location_aron
+ ARON_FILES
+ aron/Location.xml
+)
-armarx_add_library(
- LIBS
- ArmarXCoreInterfaces
+
+armarx_add_library(location
+ DEPENDENCIES
+ ArmarXCoreInterfaces
ArmarXCore
- # ${ProjectName}Libraries
armem
+ armarx_navigation::location_aron
SOURCES
constants.cpp
HEADERS
constants.h
)
-
-add_library(
- ${PROJECT_NAME}::Location
- ALIAS
- ${LIB_NAME}
-)
-
-armarx_enable_aron_file_generation_for_target(
- TARGET_NAME
- "${LIB_NAME}"
- ARON_FILES
- aron/Location.xml
-)
diff --git a/source/armarx/navigation/location/aron/Location.xml b/source/armarx/navigation/location/aron/Location.xml
index 4bc0d14f..9a0539d5 100644
--- a/source/armarx/navigation/location/aron/Location.xml
+++ b/source/armarx/navigation/location/aron/Location.xml
@@ -15,7 +15,7 @@
-->
- <Object name='armarx::nav::loc::arondto::ObjectRelativeLocation'>
+ <Object name='armarx::navigation::location::arondto::ObjectRelativeLocation'>
<ObjectChild key='objectInstanceID'>
<armarx::armem::arondto::MemoryID />
@@ -28,14 +28,14 @@
</Object>
- <Object name='armarx::nav::loc::arondto::Location'>
+ <Object name='armarx::navigation::location::arondto::Location'>
<ObjectChild key='globalRobotPose'>
<Pose />
</ObjectChild>
<ObjectChild key='relativeToObject'>
- <armarx::nav::loc::arondto::ObjectRelativeLocation optional="true" />
+ <armarx::navigation::location::arondto::ObjectRelativeLocation optional="true" />
</ObjectChild>
</Object>
diff --git a/source/armarx/navigation/location/constants.cpp b/source/armarx/navigation/location/constants.cpp
index 33b2bc98..5c06c99d 100644
--- a/source/armarx/navigation/location/constants.cpp
+++ b/source/armarx/navigation/location/constants.cpp
@@ -3,9 +3,9 @@
#include <RobotAPI/libraries/armem/core/MemoryID.h>
-namespace armarx::nav
+namespace armarx::navigation
{
- const armem::MemoryID loc::coreSegmentID { "Navigation", "Location" };
+ const armem::MemoryID location::coreSegmentID{"Navigation", "Location"};
}
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
index ac5f34ef..57a99e76 100644
--- a/source/armarx/navigation/location/constants.h
+++ b/source/armarx/navigation/location/constants.h
@@ -24,7 +24,7 @@
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
-namespace armarx::nav::loc
+namespace armarx::navigation::location
{
extern const armem::MemoryID coreSegmentID;
diff --git a/source/armarx/navigation/memory/CMakeLists.txt b/source/armarx/navigation/memory/CMakeLists.txt
index 51f8d9c1..e6e05d83 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -3,12 +3,16 @@ armarx_add_library(memory
#./memory.cpp
./client/stack_result/Writer.cpp
./client/parameterization/Reader.cpp
+ ./client/parameterization/Writer.cpp
./client/events/Writer.cpp
+ # ./client/events/Reader.cpp
HEADERS
#./memory.h
./client/stack_result/Writer.h
./client/parameterization/Reader.h
+ ./client/parameterization/Writer.h
./client/events/Writer.h
+ # ./client/events/Reader.h
DEPENDENCIES
ArmarXCoreInterfaces
ArmarXCore
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.cpp b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
index df26a8bc..64fff864 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -2,10 +2,10 @@
#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
-#include <Navigation/libraries/core/constants.h>
+#include <armarx/navigation/core/constants.h>
-namespace armarx::nav::mem::client::param
+namespace armarx::navigation::mem::client::param
{
bool
Writer::store(
@@ -65,4 +65,5 @@ namespace armarx::nav::mem::client::param
.providerName = "" // clientId
};
}
-} // namespace armarx::nav::mem::client::param
+
+} // namespace armarx::navigation::mem::client::param
diff --git a/source/armarx/navigation/memory/client/parameterization/Writer.h b/source/armarx/navigation/memory/client/parameterization/Writer.h
index 9c4db3de..aac8fea7 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.h
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -24,11 +24,10 @@
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
-#include <Navigation/libraries/core/constants.h>
-#include <Navigation/libraries/core/types.h>
+#include <armarx/navigation/core/constants.h>
+#include <armarx/navigation/core/types.h>
-
-namespace armarx::nav::mem::client::param
+namespace armarx::navigation::mem::client::param
{
class Writer : virtual public armem::client::util::SimpleWriterBase
@@ -48,4 +47,5 @@ namespace armarx::nav::mem::client::param
protected:
private:
};
-} // namespace armarx::nav::mem::client::param
+
+} // namespace armarx::navigation::mem::client::param
--
GitLab
From a5659d01ac9bfc2f7aa781d09adaba7bb4d54f2d Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 31 Aug 2021 23:55:34 +0200
Subject: [PATCH 31/33] fixes
---
source/armarx/navigation/graph/CMakeLists.txt | 3 +++
1 file changed, 3 insertions(+)
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index 261213e9..ba2c013d 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -18,6 +18,9 @@ armarx_add_library(graph
SemanticObjectRelations
# this package
armarx_navigation::graph_aron
+
+ Simox::SimoxUtility
+ Simox::VirtualRobot
DEPENDENCIES_LEGACY
VTK
SOURCES
--
GitLab
From 37cebf7bbfbcb54c2698d5ebc523981bcbbd797a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 1 Sep 2021 00:00:58 +0200
Subject: [PATCH 32/33] minor
---
source/armarx/navigation/graph/Visu.h | 1 +
1 file changed, 1 insertion(+)
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
index 8d98e616..482ae4ef 100644
--- a/source/armarx/navigation/graph/Visu.h
+++ b/source/armarx/navigation/graph/Visu.h
@@ -34,6 +34,7 @@ namespace armarx::viz
class Layer;
class Pose;
} // namespace armarx::viz
+
namespace armarx::navigation::graph
{
--
GitLab
From 05d1b51c39edf12a28d262bc1ec6adb10e7eaa48 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 1 Sep 2021 00:05:26 +0200
Subject: [PATCH 33/33] fix: adding definition
---
source/armarx/navigation/graph/CMakeLists.txt | 3 +++
1 file changed, 3 insertions(+)
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index ba2c013d..58169241 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -33,3 +33,6 @@ armarx_add_library(graph
Graph.h
Visu.h
)
+
+# THIS IS A HACK. Simox / other libs do not set this definition properly ...
+target_compile_definitions(graph PUBLIC -DEIGEN_STL_VECTOR_SPECIFICATION_DEFINED)
--
GitLab