diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5c4dfcbe1a38f714145b31140fff4ce0689b2444..151a197937e32c2536bd8c262c4b4ccd6b7cd46c 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 e726ba5d970fb7ebb6818e4ea97d2c168eb8daf4..39a4589e700e97fdd3c7ccde505d5856e199deab 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 c67cc423c370a71822e14f2c6bfd462a0a932560..2add21844e704bf696f1cc8659a5c6a063c240b7 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 d4b460469bbc99d6162e44df8a4170fedc7a97e6..a8879fb233cd1e017087f4da821dfff65bf08c5b 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/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e5d7c37d274b625cc17b2c0533bfd61e0187046d
--- /dev/null
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -0,0 +1,93 @@
+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()
+
+
+
+#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")
+
+# All target_include_directories must be guarded by if(Xyz_FOUND)
+# For multiple libraries write: if(X_FOUND AND Y_FOUND) ...
+#if(MyLib_FOUND)
+# target_include_directories(GraphImportExport PUBLIC ${MyLib_INCLUDE_DIRS})
+#endif()
+
+
+# Add ARON files
+#armarx_enable_aron_file_generation_for_target(
+# TARGET_NAME
+# ${ARMARX_COMPONENT_NAME}
+# ARON_FILES
+# aron/ExampleData.xml
+#)
+
+
+# Add unit tests
+# add_subdirectory(test)
+
+# Generate the application
+armarx_generate_and_add_component_executable(
+ # If your component is not defined in ::armarx, specify its namespace here:
+ COMPONENT_NAMESPACE "armarx::nav"
+)
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..92694a45fe17ac24788b9b4a9b55f8c404d05049
--- /dev/null
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -0,0 +1,431 @@
+/*
+ * 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 <iomanip>
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include <RobotAPI/libraries/armem/core/Commit.h>
+#include <RobotAPI/libraries/armem/core/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+#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
+{
+
+ GraphImportExport::GraphImportExport()
+ {
+ properties.locationCoreSegmentID = loc::coreSegmentID;
+ properties.graphCoreSegmentID = graph::coreSegmentID;
+ }
+
+
+ armarx::PropertyDefinitionsPtr
+ GraphImportExport::createPropertyDefinitions()
+ {
+ armarx::PropertyDefinitionsPtr def =
+ new ComponentPropertyDefinitions(getConfigIdentifier());
+
+ def->component(proxies.priorKnowledge);
+ def->component(proxies.graphNodePoseResolver, "GraphNodePoseResolver");
+
+ 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.locationWriter = memoryNameSystem.useWriter(properties.locationCoreSegmentID);
+ proxies.graphWriter = memoryNameSystem.useWriter(properties.graphCoreSegmentID);
+
+
+ // 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.visuEnabled.setValue(true);
+
+ 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");
+ 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(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;
+
+ 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())
+ {
+ locationsMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.locationsArmemToMemoryxButton.wasClicked())
+ {
+ locationsArmemToMemoryx(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphMemoryxToArmemButton.wasClicked())
+ {
+ graphMemoryxToArmem(tab.sceneComboBox.getValue());
+ }
+ if (tab.graphArmemToMemoryxButton.wasClicked())
+ {
+ graphArmemToMemoryx(tab.sceneComboBox.getValue());
+ }
+
+ if (tab.locationsClearArMemButton.wasClicked())
+ {
+ clearArMemProviderSegment(proxies.locationWriter, getLocationProviderSegmentID());
+ }
+ if (tab.graphClearArMemButton.wasClicked())
+ {
+ clearArMemProviderSegment(proxies.graphWriter, getGraphProviderSegmentID());
+ }
+ }
+
+
+ void
+ GraphImportExport::refreshScenes()
+ {
+ const ::Ice::StringSeq sceneNames = proxies.graphSegment->getScenes();
+ createRemoteGuiTab(sceneNames);
+ }
+
+
+ void
+ GraphImportExport::locationsMemoryxToArmem(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);
+ 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();
+
+ 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()};
+ }
+ }
+
+ if (not tab.dryRun.getValue())
+ {
+ armem::CommitResult result = proxies.locationWriter.commit(commit);
+ if (result.allSuccess())
+ {
+ ARMARX_IMPORTANT << "Successfully exported " << result.results.size()
+ << " locations from MemoryX to ArMem.";
+ }
+ else
+ {
+ ARMARX_WARNING << result.allErrorMessages();
+ }
+ }
+ else
+ {
+ ARMARX_VERBOSE << "Dry Run - skipping commit.";
+ }
+ }
+
+
+ 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);
+ navigation::graph::Graph graph = toArmemGraph(graphNodes);
+
+ if (tab.visuEnabled.getValue())
+ {
+ navigation::graph::GraphVisu visu;
+ viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
+ visu.draw(layer, graph);
+
+ ARMARX_VERBOSE << "Visualize graph '" << sceneName << "' ...";
+ arviz.commit(layer);
+ }
+
+ // Build ARON Graph
+ navigation::graph::arondto::Graph aron;
+ toAron(aron, graph);
+
+ // 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.graphWriter.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::graphArmemToMemoryx(const std::string& sceneName)
+ {
+ ARMARX_IMPORTANT << "graphArmemToMemoryx() is WIP!";
+ (void)sceneName;
+ }
+
+
+ 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 properties.locationCoreSegmentID.withProviderSegmentName(
+ tab.providerSegmentLine.getValue());
+ }
+
+
+ armem::MemoryID
+ GraphImportExport::getGraphProviderSegmentID()
+ {
+ return properties.graphCoreSegmentID.withProviderSegmentName(
+ tab.providerSegmentLine.getValue());
+ }
+
+
+ navigation::graph::Graph
+ GraphImportExport::toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes)
+ {
+ navigation::graph::Graph graph;
+ std::map<std::string, navigation::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;
+
+ 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));
+ vertex.attrib().setPose(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();
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..1f2a97588fff6c0a1819bc26c6bcae3fd79c18fc
--- /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 <ArmarXCore/core/Component.h>
+#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.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 <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
+{
+
+ /**
+ * @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::ArVizComponentPluginUser,
+ virtual public armarx::armem::client::MemoryNameSystemComponentPluginUser
+ {
+ public:
+ GraphImportExport();
+
+
+ /// @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 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(armem::client::Writer& writer,
+ const armem::MemoryID& providerSegmentID);
+
+ armem::MemoryID getLocationProviderSegmentID();
+ armem::MemoryID getGraphProviderSegmentID();
+
+ navigation::graph::Graph toArmemGraph(const memoryx::GraphNodeBaseList& graphNodes);
+
+
+ private:
+ struct Proxies
+ {
+ memoryx::PriorKnowledgeInterfacePrx priorKnowledge;
+ memoryx::GraphNodePoseResolverInterfacePrx graphNodePoseResolver;
+ memoryx::GraphMemorySegmentBasePrx graphSegment;
+
+ armem::client::Writer locationWriter;
+ armem::client::Writer graphWriter;
+ };
+ Proxies proxies;
+
+
+ /// Properties shown in the Scenario GUI.
+ struct Properties
+ {
+ armem::MemoryID locationCoreSegmentID;
+ armem::MemoryID graphCoreSegmentID;
+ };
+ 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;
+ armarx::RemoteGui::Client::CheckBox visuEnabled;
+ };
+ RemoteGuiTab tab;
+ };
+} // namespace armarx::nav
diff --git a/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1178a8b5b3f0e690748bc8c09f5fd9aeda8fd4a7
--- /dev/null
+++ b/source/armarx/navigation/components/GraphImportExport/test/CMakeLists.txt
@@ -0,0 +1,5 @@
+
+# Libs required for the tests
+SET(LIBS ${LIBS} ArmarXCore GraphImportExport)
+
+armarx_add_test(GraphImportExportTest GraphImportExportTest.cpp "${LIBS}")
diff --git a/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp b/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fa6c535cc4a3ffbba7b52f0feb6a13b3316b3739
--- /dev/null
+++ b/source/armarx/navigation/components/GraphImportExport/test/GraphImportExportTest.cpp
@@ -0,0 +1,37 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MemoryX::ArmarXObjects::GraphImportExport
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#define BOOST_TEST_MODULE MemoryX::ArmarXObjects::GraphImportExport
+
+#define ARMARX_BOOST_TEST
+
+#include <MemoryX/Test.h>
+#include "../GraphImportExport.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+ armarx::GraphImportExport instance;
+
+ BOOST_CHECK_EQUAL(true, true);
+}
diff --git a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
index 64f54400e02889d9b4eb91f738f04be4960070d0..2d2bd9cca05e4e3896f4da35699d559c2f86f798 100644
--- a/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
+++ b/source/armarx/navigation/components/NavigationMemory/CMakeLists.txt
@@ -1,24 +1,60 @@
+
+
+# If your component needs a special ice interface, define it here:
+# armarx_add_component_interface_lib(
+# SLICE_FILES
+# NavigationMemory.ice
+# ICE_LIBS
+# # RobotAPI
+#)
+
+
+# Add the component
armarx_add_component(NavigationMemory
DEPENDENCIES
# ArmarXCore
ArmarXCore
- # ArmarXCoreInterfaces
## ArmarXCoreComponentPlugins # For DebugObserver plugin.
# ArmarXGui
- ## ArmarXGuiComponentPlugins # For RemoteGui plugin.
+ ArmarXGuiComponentPlugins # For RemoteGui plugin.
# RobotAPI
RobotAPICore
armem
+ ## RobotAPIInterfaces
RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
# This component
## NavigationMemoryInterfaces # If you defined a component ice interface above.
- Simox::SimoxUtility
- Eigen3::Eigen
+ armarx_navigation::graph
+ armarx_navigation::location
+
SOURCES
NavigationMemory.cpp
+ Visu.cpp
+
HEADERS
NavigationMemory.h
+ Visu.h
)
+
+
+# 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(NavigationMemory 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
+#)
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
index 38de6ecec5b70aa9ae60753cdc04da9616b1958e..3858e9db0d4acc8fcf1d0d65ebd1fbb375001973 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp
@@ -22,18 +22,24 @@
#include "NavigationMemory.h"
+#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "Visu.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>
+#include <armarx/navigation/location/aron/Location.aron.generated.h>
+#include <armarx/navigation/location/constants.h>
-// Include headers you only need in function definitions in the .cpp.
-
-// #include <Eigen/Core>
-// #include <SimoxUtility/color/Color.h>
+namespace armarx::navigation
+{
+ ARMARX_DECOUPLED_REGISTER_COMPONENT(NavigationMemory);
-namespace armarx
-{
armarx::PropertyDefinitionsPtr
NavigationMemory::createPropertyDefinitions()
@@ -51,14 +57,15 @@ 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;
}
@@ -73,8 +80,7 @@ namespace armarx
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
- workingMemory.name() = p.memoryName;
- // longtermMemory.name() = p.memoryName;
+ setMemoryName(properties.memoryName);
workingMemory.addCoreSegment("Parameterization");
@@ -89,6 +95,12 @@ namespace armarx
workingMemory.addCoreSegment("Events"); //, armem::example::ExampleData::toAronType());
// workingMemory.addCoreSegment("Exceptions"); //, armem::example::ExampleData::toAronType());
+
+
+ workingMemory.addCoreSegment(navigation::location::coreSegmentID.coreSegmentName,
+ navigation::location::arondto::Location::toAronType());
+ workingMemory.addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
+ navigation::graph::arondto::Graph::toAronType());
}
@@ -109,19 +121,14 @@ 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();
}
@@ -144,80 +151,95 @@ namespace armarx
}
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- void NavigationMemory::createRemoteGuiTab()
+ 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())
- {
- 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())
+ if (visuLocations.hasValueChanged() or visuGraphEdges.hasValueChanged())
{
- // 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(navigation::location::coreSegmentID),
+ workingMemory.getCoreSegment(navigation::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
+
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
index 15d2088a135fa8ca371933a4050df0cdc40e3eb9..8c971578d45181fa6e1884df2d699c849e83d435 100644
--- a/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
+++ b/source/armarx/navigation/components/NavigationMemory/NavigationMemory.h
@@ -22,21 +22,20 @@
#pragma once
-
-// #include <mutex>
+#include <mutex>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
-
-// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.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 <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
-namespace armarx
+namespace armarx::navigation
{
/**
@@ -51,12 +50,12 @@ 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::LightweightRemoteGuiComponentPluginUser,
+ virtual public armarx::ArVizComponentPluginUser,
virtual public armarx::armem::server::ComponentPluginUser
-
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
@@ -80,63 +79,55 @@ 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::Button drawBoxes;
+ armarx::RemoteGui::Client::CheckBox visuLocations;
+ armarx::RemoteGui::Client::CheckBox visuGraphEdges;
+
+ 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
+
+} // namespace armarx::navigation
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.cpp b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..244db064c5f4d7a1ecd099b8c1d7fdeee354880e
--- /dev/null
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.cpp
@@ -0,0 +1,120 @@
+/*
+ * 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 <RobotAPI/libraries/armem/server/wm/memory_definitions.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/location/aron/Location.aron.generated.h>
+
+
+namespace armarx::navigation::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, location::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())
+ {
+ navigation::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
+ }
+ }
+
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/NavigationMemory/Visu.h b/source/armarx/navigation/components/NavigationMemory/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..f28fa6d8c7a2a208596e835003d76b19a49c2f33
--- /dev/null
+++ b/source/armarx/navigation/components/NavigationMemory/Visu.h
@@ -0,0 +1,63 @@
+/*
+ * 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 <memory>
+#include <vector>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+namespace armarx::navigation::graph
+{
+ class GraphVisu;
+}
+
+namespace armarx::navigation::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<navigation::graph::GraphVisu> visu;
+ };
+
+
+} // namespace armarx::navigation::memory
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index bd86572b20140414c145d246cc0c2e29d330273d..26eaba1fe1f14f26d7839dddb492f623228e5846 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
new file mode 100644
index 0000000000000000000000000000000000000000..5816924140bc85b5e4890840b4bf1b14e8ded5f9
--- /dev/null
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -0,0 +1,38 @@
+
+
+armarx_add_aron_library(graph_aron
+ ARON_FILES
+ aron/Graph.xml
+)
+
+armarx_add_library(graph
+ DEPENDENCIES
+ # ArmarXCore
+ ArmarXCoreInterfaces
+ ArmarXCore
+
+ # RobotAPI
+ aron
+ armem
+ # SemanticObjectRelations
+ SemanticObjectRelations
+ # this package
+ armarx_navigation::graph_aron
+
+ Simox::SimoxUtility
+ Simox::VirtualRobot
+ DEPENDENCIES_LEGACY
+ VTK
+ SOURCES
+ constants.cpp
+ Graph.cpp
+ Visu.cpp
+ HEADERS
+ constants.h
+ forward_declarations.h
+ 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)
diff --git a/source/armarx/navigation/graph/Graph.cpp b/source/armarx/navigation/graph/Graph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..db69eabb2ffc6122053e609709447267403fcf79
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.cpp
@@ -0,0 +1,117 @@
+/*
+ * 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 "Graph.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::navigation::graph
+{
+
+ std::string
+ VertexAttribs::getName() const
+ {
+ return aron.locationID.providerSegmentName + "/" + aron.locationID.entityName;
+ }
+
+ bool
+ VertexAttribs::hasPose() const
+ {
+ return _pose.has_value();
+ }
+
+ Eigen::Matrix4f
+ VertexAttribs::getPose() const
+ {
+ ARMARX_CHECK(_pose.has_value());
+ return _pose.value();
+ }
+
+ void
+ VertexAttribs::setPose(const Eigen::Matrix4f& pose)
+ {
+ this->_pose = pose;
+ }
+
+} // namespace armarx::navigation::graph
+
+
+namespace armarx::navigation
+{
+
+ void
+ graph::toAron(arondto::Graph& dto, const Graph& bo)
+ {
+ 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());
+
+ for (auto edge : bo.edges())
+ {
+ 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());
+ }
+
+
+ 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;
+ }
+ 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());
+ }
+
+
+ void
+ graph::resolveLocation(Graph::Vertex& vertex,
+ const aron::datanavigator::DictNavigatorPtr& locationData)
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..388d4eae8a8b1e36d1ef40498b112ca7f598c293
--- /dev/null
+++ b/source/armarx/navigation/graph/Graph.h
@@ -0,0 +1,100 @@
+/*
+ * 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/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::navigation::graph
+{
+
+ struct VertexAttribs : public semrel::ShapeVertex
+ {
+ armarx::navigation::graph::arondto::Vertex aron;
+
+ 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::navigation::graph::arondto::Edge aron;
+ };
+
+ struct GraphAttribs
+ {
+ };
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+
+ void toAron(arondto::Graph& dto, const Graph& bo);
+ void fromAron(const arondto::Graph& dto, Graph& bo);
+
+
+ // 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);
+ }
+ }
+
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..38b8a0811a6090fca76508fb15539a64e80c6fad
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -0,0 +1,142 @@
+/*
+ * 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 "Visu.h"
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armarx::navigation::graph
+{
+
+ 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
+ {
+ return viz::Pose(name).pose(pose).scale(scale);
+ }
+
+
+ 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
+ {
+ return viz::Arrow(name + " forward")
+ .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
+ {
+ draw(layer, vertex.attrib());
+ }
+
+
+ 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
+ {
+ if (this->pose.has_value())
+ {
+ layer.add(this->pose->draw(name, pose));
+ }
+ if (forwardArrow.has_value())
+ {
+ layer.add(forwardArrow->draw(name, pose));
+ }
+ }
+
+
+ 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
+ {
+ (void)edge;
+ return viz::Arrow(source.getName() + " -> " + target.getName())
+ .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
+ {
+ if (arrow.has_value())
+ {
+ layer.add(arrow->draw(edge));
+ }
+ }
+
+
+ void
+ GraphVisu::draw(viz::Layer& layer, const Graph& graph) const
+ {
+ 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);
+ }
+ }
+ }
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
new file mode 100644
index 0000000000000000000000000000000000000000..482ae4efd45a8d69f73b7da92caf7cbcaf15c081
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.h
@@ -0,0 +1,98 @@
+/*
+ * 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 <optional>
+
+#include <SimoxUtility/color/Color.h>
+
+#include <armarx/navigation/graph/Graph.h>
+
+
+namespace armarx::viz
+{
+ class Arrow;
+ class Layer;
+ class Pose;
+} // namespace armarx::viz
+
+namespace armarx::navigation::graph
+{
+
+ struct VertexVisu
+ {
+ struct Pose
+ {
+ 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{};
+
+ struct ForwardArrow
+ {
+ float width = 7.5;
+ float length = 100.0;
+ 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;
+ };
+
+
+ struct EdgeVisu
+ {
+ struct Arrow
+ {
+ float width = 5.0;
+ 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{};
+
+
+ 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) const;
+ };
+
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/graph/aron/Graph.xml b/source/armarx/navigation/graph/aron/Graph.xml
new file mode 100644
index 0000000000000000000000000000000000000000..beda2163d82f62f9e76f3d11e62d903699388ee9
--- /dev/null
+++ b/source/armarx/navigation/graph/aron/Graph.xml
@@ -0,0 +1,60 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <CodeIncludes>
+ </CodeIncludes>
+ <AronIncludes>
+ <!-- <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::navigation::graph::arondto::Vertex'>
+
+ <ObjectChild key='vertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='locationID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <!-- Only stored in Location -->
+ <!--ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild-->
+
+ </Object>
+
+
+ <Object name='armarx::navigation::graph::arondto::Edge'>
+
+ <ObjectChild key='sourceVertexID'>
+ <Long />
+ </ObjectChild>
+
+ <ObjectChild key='targetVertexID'>
+ <Long />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::navigation::graph::arondto::Graph'>
+
+ <ObjectChild key='vertices'>
+ <List>
+ <armarx::navigation::graph::arondto::Vertex />
+ </List>
+ </ObjectChild>
+
+ <ObjectChild key='edges'>
+ <List>
+ <armarx::navigation::graph::arondto::Edge />
+ </List>
+ </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 0000000000000000000000000000000000000000..a87fc00d8ba55fdcecd1bca47001aeec3df9e4d3
--- /dev/null
+++ b/source/armarx/navigation/graph/constants.cpp
@@ -0,0 +1,11 @@
+#include "constants.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::navigation
+{
+
+ const armem::MemoryID graph::coreSegmentID{"Navigation", "Graph"};
+
+}
diff --git a/source/armarx/navigation/graph/constants.h b/source/armarx/navigation/graph/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..f88b3cdedf62dbdd9518d27d72ad48cb0556e974
--- /dev/null
+++ b/source/armarx/navigation/graph/constants.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
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+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
new file mode 100644
index 0000000000000000000000000000000000000000..2f30f7839c6a80f9ad318344dc2151102518df6d
--- /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::navigation::graph
+{
+
+ struct VertexAttribs;
+ struct EdgeAttribs;
+ struct GraphAttribs;
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+} // namespace armarx::navigation::graph
diff --git a/source/armarx/navigation/gui-plugins/CMakeLists.txt b/source/armarx/navigation/gui-plugins/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0b821f039461902aa7dddab6d07b19ca82b6123c
--- /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 0000000000000000000000000000000000000000..0c9d633dccb4e1f929fc4afb99e0e5a11ef9c663
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/CMakeLists.txt
@@ -0,0 +1,85 @@
+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
+
+ FunctionalEventFilter.cpp
+ GuiGraph.cpp
+ Visu.cpp
+
+ 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
+
+ widgets/graph_scene/ControlWidget.cpp
+ widgets/graph_scene/Scene.cpp
+ widgets/graph_scene/Widget.cpp
+)
+set(HEADERS
+ LocationGraphEditorWidgetController.h
+
+ FunctionalEventFilter.h
+ GuiGraph.h
+ Visu.h
+
+ 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
+
+ widgets/graph_scene.h
+ widgets/graph_scene/ControlWidget.h
+ widgets/graph_scene/Scene.h
+ widgets/graph_scene/Widget.h
+)
+set(GUI_UIS
+ LocationGraphEditorWidget.ui
+)
+set(GUI_RCS
+ icons.qrc
+)
+
+# Add more libraries you depend on here, e.g. ${QT_LIBRARIES}.
+set(COMPONENT_LIBS
+ # ArmarXGui
+ SimpleConfigDialog
+
+ # RobotAPI
+ ArViz
+ armem
+
+ Navigation::Location
+ Navigation::Graph
+)
+
+
+if(ArmarXGui_FOUND)
+ armarx_gui_plugin("${LIB_NAME}" "${SOURCES}" "" "${GUI_UIS}" "${GUI_RCS}" "${COMPONENT_LIBS}")
+
+ #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/FunctionalEventFilter.cpp b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1ffe763d0c7060d2c8de191daa01c5ca3f1604f7
--- /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 0000000000000000000000000000000000000000..0b5fb30b720cdc5965d72622a244d84259a306ba
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/FunctionalEventFilter.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <QObject>
+
+#include <functional>
+
+class QEvent;
+
+
+namespace simox::gui
+{
+
+ class FunctionalEventFilter : public QObject
+ {
+ 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 0000000000000000000000000000000000000000..c60d7f2eea3c9992a3cfe20b71193897c9f6d75d
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp
@@ -0,0 +1,170 @@
+#include "GraphScene.h"
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+
+
+namespace armarx::navigation::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();
+ }
+
+} // namespace armarx::navigation::locgrapheditor
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 0000000000000000000000000000000000000000..df113b0fc859f065bed176f7287a1810a75ea844
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h
@@ -0,0 +1,108 @@
+#pragma once
+
+#include <QGraphicsEllipseItem>
+#include <QGraphicsLineItem>
+#include <QGraphicsScene>
+#include <functional>
+
+#include "GuiGraph.h"
+#include <armarx/navigation/graph/Graph.h>
+
+
+namespace semrel
+{
+ struct ShapeID;
+}
+namespace armarx::navigation::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;
+ };
+
+
+} // namespace armarx::navigation::locgrapheditor
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 0000000000000000000000000000000000000000..034b0609246071b088cf973bfd9303bbb5335f4d
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.cpp
@@ -0,0 +1,146 @@
+/*
+ * 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/mat4f_to_rpy.h>
+#include <SimoxUtility/math/convert/rad_to_deg.h>
+
+
+namespace armarx::navigation::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)
+ {
+ 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;
+ }
+
+
+ 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::navigation::locgrapheditor
+
+namespace armarx::nav
+{
+
+ 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)
+ {
+ 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()});
+ }
+ return gui;
+ }
+
+
+ navigation::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;
+ }
+
+} // namespace armarx::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 0000000000000000000000000000000000000000..9355d9621349e5d34d27f94406477e94ecb3f94a
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h
@@ -0,0 +1,105 @@
+/*
+ * 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 <map>
+#include <optional>
+
+#include <SemanticObjectRelations/RelationGraph/RelationGraph.h>
+#include <armarx/navigation/graph/Graph.h>
+
+
+class QGraphicsEllipseItem;
+class QGraphicsLineItem;
+class QTableWidgetItem;
+
+
+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 navigation::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;
+
+ /// Whether the vertex was changed since loading or committing.
+ bool changed = 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 navigation::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 navigation::graph::GraphAttribs
+ {
+ /// Whether the graph structure was changed since loading or committing.
+ bool edgesChanged = false;
+ };
+
+
+ class GuiGraph : public semrel::RelationGraph<VertexData, EdgeData, GraphData>
+ {
+ public:
+ using RelationGraph::RelationGraph;
+
+
+ bool hasChanged() const;
+
+ std::optional<Vertex> getVertexFromTableItem(QTableWidgetItem* item);
+
+ std::map<QTableWidgetItem*, Vertex> getTableItemToVertexMap();
+ std::map<QTableWidgetItem*, Edge> getTableItemToEdgeMap();
+ };
+
+
+ GuiGraph toGuiGraph(const graph::Graph& graph);
+ 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/LocationGraphEditorWidget.ui b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
new file mode 100644
index 0000000000000000000000000000000000000000..db72fa0c83009a8fa6c55cd40cf5e2ef39ae552c
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidget.ui
@@ -0,0 +1,252 @@
+<?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>684</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string>LocationGraphEditorWidget</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_2">
+ <item>
+ <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="createGraphButton">
+ <property name="toolTip">
+ <string>Reloads the list of scenes from the memory</string>
+ </property>
+ <property name="text">
+ <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>
+ <item>
+ <widget class="QComboBox" name="graphsComboBox">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="loadGraphButton">
+ <property name="toolTip">
+ <string>Draws the selected scene</string>
+ </property>
+ <property name="text">
+ <string>Load</string>
+ </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>
+ <item>
+ <widget class="QSplitter" name="verticalSplitter">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <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="horizontalSplitter">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <widget class="QWidget" name="verticalLayout_12Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_12">
+ <item>
+ <widget class="QGroupBox" name="locationDataGroupBox">
+ <property name="title">
+ <string>Location Data</string>
+ </property>
+ <layout class="QVBoxLayout" name="verticalLayout_3">
+ <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>
+ <item>
+ <widget class="QGroupBox" name="robotVisuGroupBox">
+ <property name="title">
+ <string>Robot State</string>
+ </property>
+ <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>
+ </widget>
+ <widget class="QWidget" name="verticalLayout_13Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_13">
+ <item>
+ <widget class="QGroupBox" name="locationsTableGroupBox">
+ <property name="title">
+ <string>Locations (right click for options)</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_6Widget">
+ <layout class="QVBoxLayout" name="verticalLayout_6">
+ <item>
+ <widget class="QGroupBox" name="edgesTableGroupBox">
+ <property name="title">
+ <string>Graph Edges (right click for options)</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="QSplitter" name="horizontalSplitter_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </widget>
+ </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>
+ <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/>
+ <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 0000000000000000000000000000000000000000..0a050d5667436ad7ed073436a7edba7d78e67ae1
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.cpp
@@ -0,0 +1,1128 @@
+/*
+ * 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 <VirtualRobot/VirtualRobot.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"
+#include "widgets/VertexDataWidget.h"
+#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/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>
+
+// Qt headers
+#include <QDialog>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QLineEdit>
+#include <QMenu>
+#include <QMessageBox>
+#include <QMouseEvent>
+#include <QObject>
+#include <QPushButton>
+#include <QSignalBlocker>
+
+// std
+#include <sstream>
+
+
+static const QString SETTING_LAST_SCENE = "lastScene";
+
+
+namespace armarx::navigation::locgrapheditor
+{
+
+ QString
+ LocationGraphEditorWidgetController::GetWidgetName()
+ {
+ return "Navigation.LocationGraphEditor";
+ }
+ QIcon
+ LocationGraphEditorWidgetController::GetWidgetIcon()
+ {
+ return QIcon{"://icons/location_graph_editor.svg"};
+ }
+
+
+ LocationGraphEditorWidgetController::LocationGraphEditorWidgetController() :
+ settings{"KIT", "LocationGraphEditorWidgetController"}
+ {
+ widget.setupUi(getWidget());
+
+ loadAutomaticSettings();
+
+ widget.loadGraphButton->setEnabled(false);
+
+ view.vertexData = new VertexDataWidget();
+ 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()))
+ {
+ 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);
+
+ 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::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);
+ 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,
+ [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);
+
+ // 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);
+ }
+
+
+ LocationGraphEditorWidgetController::~LocationGraphEditorWidgetController()
+ {
+ saveAutomaticSettings();
+ }
+
+
+ QPointer<QDialog>
+ LocationGraphEditorWidgetController::getConfigDialog(QWidget* parent)
+ {
+ if (not configDialog)
+ {
+ configDialog = new SimpleConfigDialog(parent);
+ configDialog->addProxyFinder<armem::mns::MemoryNameSystemInterfacePrx>(
+ "MemoryNameSystem", "Memory Name System", remote.memoryNameSystemName);
+ }
+ return qobject_cast<SimpleConfigDialog*>(configDialog);
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::configured()
+ {
+ remote.memoryNameSystemName = configDialog->getProxyName("MemoryNameSystem");
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::loadSettings(QSettings* settings)
+ {
+ remote.memoryNameSystemName =
+ settings
+ ->value("memoryNameSystemName", QString::fromStdString(remote.memoryNameSystemName))
+ .toString()
+ .toStdString();
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::saveSettings(QSettings* settings)
+ {
+ 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(remote.memoryNameSystemName);
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::onConnectComponent()
+ {
+ remote.connect(*this);
+ {
+ std::stringstream ss;
+ ss << "Navigation Graphs (Entities from Core Segment "
+ << navigation::graph::coreSegmentID << ")";
+ widget.graphGroupBox->setTitle(QString::fromStdString(ss.str()));
+ widget.graphGroupBox->setEnabled(true);
+ }
+
+ emit connected();
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::Remote::connect(Component& parent)
+ {
+ auto mnsProxy =
+ parent.getProxy<armem::mns::MemoryNameSystemInterfacePrx>(memoryNameSystemName);
+ memoryNameSystem = armem::client::MemoryNameSystem(mnsProxy, &parent);
+
+ locationReader = memoryNameSystem.useReader(navigation::location::coreSegmentID);
+ locationWriter = memoryNameSystem.useWriter(navigation::location::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()
+ {
+ 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(navigation::location::coreSegmentID);
+ if (result.success)
+ {
+ model.locationsMemory = std::move(result.memory);
+ emit locationMemoryChanged();
+ }
+ return result;
+ }
+
+
+ armem::client::QueryResult
+ LocationGraphEditorWidgetController::queryGraphs()
+ {
+ armem::client::QueryResult result =
+ remote.graphReader.getLatestSnapshotsIn(navigation::graph::coreSegmentID);
+ if (result.success)
+ {
+ model.graphMemory = std::move(result.memory);
+ emit graphMemoryChanged();
+ }
+ return result;
+ }
+
+
+ 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)
+ {
+ previousIndex = i;
+ }
+ ++i;
+ });
+ if (previousIndex >= 0)
+ {
+ widget.graphsComboBox->setCurrentIndex(previousIndex);
+ }
+ widget.loadGraphButton->setEnabled(widget.graphsComboBox->count() > 0);
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::loadGraph()
+ {
+ if (model.graph.hasChanged())
+ {
+ if (not loadGraphDialog())
+ {
+ return;
+ }
+ }
+
+ const armem::MemoryID entityID = armem::MemoryID::fromString(
+ widget.graphsComboBox->currentData().toString().toStdString());
+
+ // Refresh local memory.
+ queryMemory();
+
+ const armem::wm::EntityInstance* instance = model.graphMemory.findLatestInstance(entityID);
+ 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.";
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+
+ navigation::graph::Graph nav;
+ {
+ navigation::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<semrel::ShapeID> remove;
+ for (navigation::graph::Graph::Vertex vertex : nav.vertices())
+ {
+ if (not vertex.attrib().hasPose())
+ {
+ remove.push_back(vertex.objectID());
+ }
+ }
+ if (not remove.empty())
+ {
+ for (semrel::ShapeID vertexID : remove)
+ {
+ nav.removeVertex(nav.vertex(vertexID));
+ }
+ std::stringstream ss;
+ ss << "Dropped " << remove.size() << " locations which could not be resolved.";
+ widget.statusLabel->setText(QString::fromStdString(ss.str()));
+ }
+ }
+
+ ARMARX_VERBOSE << "Loading graph " << nav.str();
+ setGraph(entityID, nav);
+ }
+
+
+ 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()
+ {
+ armem::CommitResult locResults = commitLocations();
+ armem::EntityUpdateResult graphResult = commitGraph();
+
+ {
+ std::stringstream ss;
+ if (locResults.allSuccess())
+ {
+ 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;
+ }
+ 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();
+
+ navigation::location::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()
+ {
+ navigation::graph::arondto::Graph dto;
+ {
+ navigation::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)
+ {
+ // Clear dirty flag
+ model.graph.attrib().edgesChanged = false;
+ }
+ return result;
+ }
+
+
+ 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);
+
+ 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()),
+ model.graph.vertex(edge.targetObjectID()),
+ {edge.attrib()});
+ }
+ }
+
+ // Trigger a view update.
+ emit graphChanged();
+ }
+
+
+ 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)
+ {
+ ARMARX_CHECK(not model.graph.hasVertex(vertexID));
+
+ GuiGraph::Vertex vertex = model.graph.addVertex(vertexID, defaultAttribs);
+ vertex.attrib().graphicsItem = view.graph->scene()->addVertex(vertex);
+ vertex.attrib().tableWidgetItem = view.vertexTable->addVertex();
+
+ emit graphChanged();
+
+ return vertex;
+ }
+
+
+ GuiGraph::Vertex
+ LocationGraphEditorWidgetController::addVertex(
+ semrel::ShapeID vertexID,
+ const armem::wm::EntityInstance& locationInstance)
+ {
+ navigation::location::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,
+ 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);
+
+ emit graphChanged();
+
+ return edge;
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::updateGraphView()
+ {
+ for (auto vertex : model.graph.vertices())
+ {
+ updateVertexView(vertex);
+ }
+ for (auto edge : model.graph.edges())
+ {
+ 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();
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::updateVertexView(GuiGraph::Vertex 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;
+ for (auto v : model.graph.vertices())
+ {
+ if (v.attrib().highlighted)
+ {
+ highlightedVertices.insert(v.objectID());
+ }
+ }
+
+ for (auto edge : model.graph.edges())
+ {
+ 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;
+ }
+ }
+ }
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::updateEdgeView(GuiGraph::Edge edge)
+ {
+ view.graph->scene()->updateEdge(edge);
+ view.edgeTable->updateEdge(edge);
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::updateArViz()
+ {
+ if (remote.arviz)
+ {
+ std::vector<viz::Layer> layers;
+ {
+ 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("Graph 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);
+ }
+ }
+
+
+ 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()
+ {
+ {
+ QSignalBlocker blocker(this);
+ clearEdges();
+ clearVertices();
+ }
+
+ // Clear data structure
+ ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
+ ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
+
+ emit graphChanged();
+ }
+
+
+ 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()};
+ {
+ 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);
+ ARMARX_CHECK_EQUAL(model.graph.numEdges(), 0);
+
+ 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()};
+ {
+ 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);
+ ARMARX_CHECK_EQUAL(model.graph.numVertices(), 0);
+
+ emit graphChanged();
+ }
+
+
+ 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
+ {
+ QSignalBlocker blocker(view.edgeTable);
+ view.edgeTable->removeEdge(edge);
+ }
+ view.graph->scene()->removeEdge(edge.attrib().graphicsItem);
+
+ // 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 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();
+
+ // Remove view elements
+ {
+ QSignalBlocker blocker(view.vertexTable);
+ view.vertexTable->removeVertex(vertex);
+ }
+ view.graph->scene()->removeVertex(vertex.attrib().graphicsItem);
+ if (view.vertexData->vertex().has_value() and view.vertexData->vertex().value() == vertex)
+ {
+ view.vertexData->clearVertex();
+ }
+
+ // Remove from model
+ model.graph.removeVertex(vertex);
+
+ emit graphChanged();
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::createVertexDialog()
+ {
+ NewEntityIdDialog dialog(navigation::location::coreSegmentID, nullptr);
+ switch (dialog.exec())
+ {
+ 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});
+
+ // Initiaize attributes
+ VertexData attribs;
+ toAron(attribs.aron.locationID, dialog.entityID());
+ attribs.setPose(Eigen::Matrix4f::Identity());
+ attribs.changed = true;
+
+ addVertex(vertexID, attribs);
+ }
+
+
+ void
+ LocationGraphEditorWidgetController::addEdges(
+ QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> vertexItems)
+ {
+ const auto itemToVertexMap = model.graph.getTableItemToVertexMap();
+ {
+ QSignalBlocker blocker(this);
+
+ 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)
+ {
+ 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::removeEdgesOfVertex(QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction)
+ {
+ 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::createGraphDialog()
+ {
+ NewEntityIdDialog dialog(navigation::graph::coreSegmentID, nullptr);
+ switch (dialog.exec())
+ {
+ case QDialog::Accepted:
+ break;
+ case QDialog::Rejected:
+ return;
+ }
+
+ const armem::MemoryID entityID = dialog.entityID();
+
+ 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);
+
+ setEmptyGraph(entityID);
+ }
+
+
+ QString
+ LocationGraphEditorWidgetController::getGraphDisplayName(const armem::MemoryID& entityID,
+ bool changed) const
+ {
+ QString name =
+ QString::fromStdString(entityID.providerSegmentName + "/" + entityID.entityName);
+ if (changed)
+ {
+ name += "*";
+ }
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..30ee5941f0d02333a8a3430b7e337dbb1a92aeaf
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/LocationGraphEditorWidgetController.h
@@ -0,0 +1,265 @@
+/*
+ * 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 <memory>
+#include <string>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/system/ImportExportComponent.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/ArmarXComponentWidgetController.h>
+#include <ArmarXGui/libraries/SimpleConfigDialog/SimpleConfigDialog.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 "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;
+
+
+namespace armarx::viz
+{
+ class Client;
+}
+namespace armarx::navigation::locgrapheditor::utils
+{
+ enum class EdgeDirection;
+}
+namespace armarx::navigation::locgrapheditor
+{
+ class EdgeTableWidget;
+ class RobotVisuWidget;
+ class VertexDataWidget;
+ class VertexTableWidget;
+
+
+ /**
+ \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
+
+ using This = LocationGraphEditorWidgetController;
+
+
+ public:
+ 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();
+
+
+ signals:
+
+ void connected();
+
+ void locationMemoryChanged();
+ void graphMemoryChanged();
+ void memoryChanged();
+
+ void graphChanged();
+
+
+ private slots:
+
+ // Load
+ void queryMemory();
+ armem::client::QueryResult queryLocations();
+ armem::client::QueryResult queryGraphs();
+
+ void loadGraph();
+ bool loadGraphDialog();
+
+ // Commit
+ void commit();
+ armem::CommitResult commitLocations();
+ armem::EntityUpdateResult commitGraph();
+
+
+ // View & Tables
+ void updateGraphList();
+ void updateGraphView();
+ void updateVertexView(GuiGraph::Vertex vertex);
+ void updateEdgeView(GuiGraph::Edge edge);
+ void updateArViz();
+
+
+ // Selection & Highlighting
+ void selectVertex(QTableWidgetItem* vertexItem);
+ void selectVertex(GuiGraph::Vertex vertex);
+ 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 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);
+
+ 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;
+
+
+ private:
+ /// Widget Form
+ Ui::LocationGraphEditorWidget widget;
+ QPointer<SimpleConfigDialog> configDialog;
+
+
+ struct Remote
+ {
+ std::string memoryNameSystemName = "MemoryNameSystem";
+ armem::client::MemoryNameSystem memoryNameSystem;
+
+ armem::client::Reader locationReader;
+ armem::client::Writer locationWriter;
+
+ armem::client::Reader graphReader;
+ armem::client::Writer graphWriter;
+
+ std::unique_ptr<viz::Client> arviz;
+
+ void connect(Component& parent);
+ };
+ Remote remote;
+
+
+ struct Model
+ {
+ armem::wm::Memory locationsMemory;
+
+ armem::wm::Memory graphMemory;
+ armem::MemoryID graphEntityID;
+ GuiGraph graph;
+
+ GraphVisu visu;
+ };
+ Model model;
+
+
+ struct View
+ {
+ VertexTableWidget* vertexTable = nullptr;
+ EdgeTableWidget* edgeTable = nullptr;
+ VertexDataWidget* vertexData = nullptr;
+
+ GraphSceneWidget* graph = nullptr;
+
+ RobotVisuWidget* robotVisu = nullptr;
+ };
+ View view;
+
+
+ private:
+ QSettings settings;
+ QString lastSelectedSceneName;
+ };
+
+} // namespace armarx::navigation::locgrapheditor
+
+namespace armarx
+{
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..bbfb5d6c7490e4de0b9ac48f790e06283ae66f49
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.cpp
@@ -0,0 +1,134 @@
+/*
+ * 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 "Visu.h"
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+#include <VirtualRobot/VirtualRobot.h>
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+
+namespace armarx::navigation::locgrapheditor
+{
+
+ const simox::Color visu::defaultColorHighlighted = simox::Color::orange();
+
+
+ viz::Pose
+ VertexVisu::Pose::draw(const VertexData& attribs) const
+ {
+ viz::Pose pose = navigation::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 = navigation::graph::VertexVisu::ForwardArrow::draw(attribs);
+ if (attribs.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ arrow.width(width * 1.5f);
+ }
+ return arrow;
+ }
+
+
+ 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())
+ {
+ 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 = navigation::graph::EdgeVisu::Arrow::draw(edge, source, target);
+ if (edge.highlighted)
+ {
+ arrow.color(colorHighlighted);
+ arrow.width(width * 1.5f);
+ }
+ 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) const
+ {
+ if (vertex.has_value())
+ {
+ for (GuiGraph::ConstVertex v : graph.vertices())
+ {
+ vertex->draw(layer, v);
+ }
+ }
+ if (edge.has_value())
+ {
+ for (GuiGraph::ConstEdge e : graph.edges())
+ {
+ edge->draw(layer, e);
+ }
+ }
+ }
+
+} // namespace armarx::navigation::locgrapheditor
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 0000000000000000000000000000000000000000..77193be2babbbfd30f4390a10b7ef565729f65c1
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/Visu.h
@@ -0,0 +1,103 @@
+/*
+ * 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 <optional>
+
+#include <SimoxUtility/color/Color.h>
+
+#include "GuiGraph.h"
+#include <armarx/navigation/graph/Visu.h>
+
+
+namespace armarx::navigation::locgrapheditor::visu
+{
+ extern const simox::Color defaultColorHighlighted;
+}
+namespace armarx::navigation::locgrapheditor
+{
+
+ struct VertexVisu
+ {
+ struct Pose : public navigation::graph::VertexVisu::Pose
+ {
+ float scaleFactorHighlighted = 1.5;
+
+ viz::Pose draw(const VertexData& attribs) const;
+ };
+ std::optional<Pose> pose = Pose{};
+
+ struct ForwardArrow : public navigation::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, GuiGraph::ConstVertex vertex) const;
+ void draw(viz::Layer& layer, const VertexData& attribs) const;
+ };
+
+
+ struct EdgeVisu
+ {
+ 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;
+ };
+ 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) const;
+ };
+
+
+ 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);
+ }
+ }
+ }
+
+} // namespace armarx::navigation::locgrapheditor
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 0000000000000000000000000000000000000000..14569cdf95896f59a34352a155e82c82f9ea7bc9
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons.qrc
@@ -0,0 +1,5 @@
+<RCC>
+ <qresource prefix="/">
+ <file>icons/location_graph_editor.svg</file>
+ </qresource>
+</RCC>
diff --git a/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/location_graph_editor.svg b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/location_graph_editor.svg
new file mode 100644
index 0000000000000000000000000000000000000000..6f27578f044c95c6c897ff3dc70d76fd4280e280
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/icons/location_graph_editor.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
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 0000000000000000000000000000000000000000..3752fc450fd809ee04420779af0f70ceac654dc5
--- /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::navigation::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 0000000000000000000000000000000000000000..2090fea9c5108448ffa8a945032b90c2a6a746b1
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/ConnectDialog.h
@@ -0,0 +1,82 @@
+/*
+ * 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 <QDialog>
+#include <QDialogButtonBox>
+
+#include <ArmarXCore/core/ManagedIceObject.h>
+
+#include <ArmarXGui/libraries/ArmarXGuiBase/widgets/IceProxyFinder.h>
+
+
+namespace armarx::navigation::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;
+ };
+
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..13c436fea69e956af28183e7a0893e5508b13126
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.cpp
@@ -0,0 +1,155 @@
+/*
+ * 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 <QAction>
+#include <QHeaderView>
+#include <QMenu>
+
+#include "utils.h"
+
+
+namespace armarx::navigation::locgrapheditor
+{
+
+ EdgeTableWidget::EdgeTableWidget()
+ {
+ QStringList columns{"Source", "Target"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setResizeMode(1, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ 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);
+ }
+
+
+ QTableWidgetItem*
+ EdgeTableWidget::addEdge(const graph::VertexAttribs& sourceAttrib,
+ const graph::VertexAttribs& targetAttrib)
+ {
+ QTableWidgetItem* result = nullptr;
+
+ 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 result;
+ }
+
+
+ void
+ EdgeTableWidget::updateEdge(GuiGraph::Edge edge)
+ {
+ QColor bgColor = edge.attrib().highlighted ? bgColorSelected : bgColorDefault;
+ QFont font;
+ font.setBold(edge.attrib().highlighted);
+
+ setSortingEnabled(false);
+ {
+ 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);
+ }
+ }
+ setSortingEnabled(true);
+ }
+
+
+ void
+ EdgeTableWidget::removeEdge(GuiGraph::Edge& edge)
+ {
+ if (currentItem() == edge.attrib().tableWidgetItem)
+ {
+ setCurrentItem(nullptr);
+ }
+
+ removeRow(row(edge.attrib().tableWidgetItem));
+ edge.attrib().tableWidgetItem = nullptr;
+ }
+
+
+ QList<QTableWidgetItem*>
+ EdgeTableWidget::selectedEdgeItems()
+ {
+ return utils::getSelectedItemsOfColumn(this, 0);
+ }
+
+
+ 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));
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..9e1aae1579a698837047813ed34ecb68449de8ac
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/EdgeTableWidget.h
@@ -0,0 +1,81 @@
+/*
+ * 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 <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::navigation::locgrapheditor
+{
+
+ class EdgeTableWidget : public QTableWidget
+ {
+ Q_OBJECT
+ using This = EdgeTableWidget;
+
+
+ public:
+ EdgeTableWidget();
+
+
+ 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);
+
+ void removeEdge(GuiGraph::Edge& edge);
+
+ void clearEdges();
+
+
+ QList<QTableWidgetItem*> selectedEdgeItems();
+
+
+ signals:
+
+ void edgeRemovalRequested(QList<QTableWidgetItem*> edgeItems);
+
+
+ public slots:
+
+ void makeContextMenu(QPoint pos);
+
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..7fdac3c4640f712cd2f84d54b985c99b3f5cebac
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.cpp
@@ -0,0 +1,133 @@
+/*
+ * 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 <QDialogButtonBox>
+#include <QGridLayout>
+#include <QLabel>
+#include <QLineEdit>
+#include <QPushButton>
+#include <QVBoxLayout>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::navigation::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;
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..2a0aad3d469e7116149b9389b5a734fa5a9fc97a
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/NewEntityIdDialog.h
@@ -0,0 +1,60 @@
+/*
+ * 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 <QDialog>
+#include <memory>
+
+#include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
+
+class QLineEdit;
+class QDialogButtonBox;
+
+
+namespace armarx::navigation::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;
+ };
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..afe0a4a15a9e983a2b1b347e0ef22bdb3b393c69
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.cpp
@@ -0,0 +1,234 @@
+/*
+ * 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 <QCheckBox>
+#include <QDialog>
+#include <QDialogButtonBox>
+#include <QPushButton>
+#include <QVBoxLayout>
+
+#include <VirtualRobot/Robot.h>
+
+#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)
+ {
+ _enabled = new QCheckBox("Enable Visu", this);
+ _enabled->setChecked(true);
+
+ _collisionModel = new QCheckBox("Use 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);
+
+ const int margin = 0;
+ setContentsMargins(margin, margin, margin, margin);
+ }
+
+
+ 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(
+ std::make_unique<RobotStateComponentInterfacePrx>(robotStateComponent)),
+ _settings(settings)
+ {
+ ARMARX_CHECK_NOT_NULL(robotStateComponent);
+ _filename = robotStateComponent->getRobotFilename();
+ _robot = RemoteRobot::createLocalClone(
+ robotStateComponent, "", {}, VirtualRobot::RobotIO::RobotDescription::eStructure);
+ ARMARX_CHECK_NOT_NULL(_robot);
+ }
+
+
+ robotvisu::Connection::~Connection()
+ {
+ }
+
+
+ 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;
+ }
+
+
+ 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, getRobotStateComponent());
+ }
+ }
+
+
+ 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]()
+ {
+ QString name = QString::fromStdString(connection().getConnectedName());
+ _statusLabel->setText("Connected to '" + 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.emplace(robotStateComponent, _settings);
+ emit connected();
+ }
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..1a7083dc58fd37deb1bd8d09d322b5c96dc10e59
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/RobotVisuWidget.h
@@ -0,0 +1,159 @@
+/*
+ * 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 <QWidget>
+#include <map>
+#include <memory>
+#include <optional>
+#include <string>
+
+#include <Eigen/Core>
+
+
+class QCheckBox;
+class QLabel;
+class QPushButton;
+
+
+namespace VirtualRobot
+{
+ using RobotPtr = std::shared_ptr<class Robot>;
+}
+namespace IceInternal
+{
+ template <typename T>
+ class ProxyHandle;
+}
+namespace IceProxy::armarx
+{
+ class RobotStateComponentInterface;
+}
+namespace armarx
+{
+ class ManagedIceObject;
+ using RobotStateComponentInterfacePrx =
+ ::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface>;
+} // namespace armarx
+namespace armarx::viz
+{
+ class Robot;
+}
+
+
+namespace armarx::navigation::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);
+ ~Connection();
+
+
+ RobotStateComponentInterfacePrx getRobotStateComponent() const;
+ std::string getConnectedName() const;
+
+
+ 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);
+
+
+ private:
+ void _synchronize(bool synchronize);
+
+ std::unique_ptr<RobotStateComponentInterfacePrx> _robotStateComponent;
+ robotvisu::SettingsWidget* _settings = nullptr;
+
+ std::string _filename;
+ VirtualRobot::RobotPtr _robot;
+ };
+
+} // namespace armarx::navigation::locgrapheditor::robotvisu
+
+namespace armarx::navigation::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;
+ };
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..f1a25de4940dc40e54b9b5fcbcead1faa92dca75
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.cpp
@@ -0,0 +1,384 @@
+/*
+ * 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 <QList>
+#include <QPushButton>
+#include <QRadioButton>
+#include <QSignalBlocker>
+
+#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>
+
+#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::navigation::locgrapheditor
+{
+
+ VertexDataWidget::VertexDataWidget()
+ {
+ setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+
+ name = new QLineEdit(this);
+ name->setReadOnly(true);
+ locationID = new QLineEdit(this);
+ locationID->setReadOnly(true);
+
+ 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");
+
+ _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())
+ {
+ pos->setSuffix(" mm");
+ pos->setMinimum(-1e6);
+ pos->setMaximum(+1e6);
+ pos->setSingleStep(50);
+ }
+ for (QDoubleSpinBox* spinBox : _allSpinBoxes())
+ {
+ spinBox->setValue(0);
+ spinBox->setAlignment(Qt::AlignRight);
+ }
+
+ QHBoxLayout* angleUnitLayout = new QHBoxLayout();
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ angleUnitLayout->addWidget(angleUnit);
+ }
+
+ QFormLayout* layout = new QFormLayout();
+ this->setLayout(layout);
+
+ layout->addRow("Name", name);
+ layout->addRow("Entity 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);
+ layout->addRow("Angle Units", angleUnitLayout);
+ layout->addRow(_useCurrentRobotPose);
+
+
+ // Connect
+
+ for (QRadioButton* angleUnit : {angleUnitDeg, angleUnitRad})
+ {
+ 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);
+ }
+
+ connect(_useCurrentRobotPose, &QPushButton::pressed, this, &This::_setFromCurrentRobotPose);
+
+
+ angleUnitDeg->click();
+ }
+
+
+ std::optional<GuiGraph::Vertex>
+ VertexDataWidget::vertex()
+ {
+ return _vertex;
+ }
+
+
+ void
+ VertexDataWidget::setVertex(GuiGraph::Vertex vertex)
+ {
+ QSignalBlocker blocker(this);
+
+ _vertex = vertex;
+ _setFromVertex(vertex);
+ setEnabled(true);
+ }
+
+
+ void
+ VertexDataWidget::clearVertex()
+ {
+ _vertex = std::nullopt;
+ setEnabled(false);
+ }
+
+
+ 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::setRobotConnection(robotvisu::Connection* connection)
+ {
+ this->_robotConnection = connection;
+ _useCurrentRobotPose->setEnabled(_robotConnection != nullptr);
+ }
+
+
+ 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()));
+ frame->setText("<WIP>");
+ _setPose(attrib.getPose().cast<qreal>());
+ }
+
+
+ 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;
+ 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
+ {
+ ARMARX_CHECK(angleUnitDeg->isChecked());
+ convertValue = [](double rad) { return simox::math::rad_to_deg(rad); };
+ }
+
+ std::vector<QSignalBlocker> blockers;
+ for (QDoubleSpinBox* angle : _angleSpinBoxes())
+ {
+ blockers.emplace_back(angle);
+
+ 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>()));
+ }
+
+ // blockers will disable blocking for all spin boxes on destruction.
+ }
+
+
+ void
+ VertexDataWidget::_updateVertexAttribs()
+ {
+ if (not signalsBlocked() and _vertex.has_value())
+ {
+ _getToVertex(_vertex.value());
+ _vertex->attrib().changed = true;
+ emit vertexDataChanged();
+ }
+ }
+
+
+ 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::_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());
+ 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);
+ }
+ }
+
+
+ void
+ VertexDataWidget::_setFromCurrentRobotPose()
+ {
+ ARMARX_CHECK_NOT_NULL(_robotConnection);
+ {
+ QSignalBlocker blocker(this);
+ _setPose(_robotConnection->getGlobalPose().cast<qreal>());
+ }
+ _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
new file mode 100644
index 0000000000000000000000000000000000000000..6b5c2587f0ce7dd233310a27cde2f0cb159fd085
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexDataWidget.h
@@ -0,0 +1,120 @@
+/*
+ * 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 <QWidget>
+
+#include <Eigen/Core>
+
+#include <armarx/navigation/gui-plugins/LocationGraphEditor/GuiGraph.h>
+
+
+class QLineEdit;
+class QDoubleSpinBox;
+class QPushButton;
+class QRadioButton;
+
+
+namespace armarx::navigation::locgrapheditor::robotvisu
+{
+ class Connection;
+}
+namespace armarx::navigation::locgrapheditor
+{
+
+ class VertexDataWidget : public QWidget
+ {
+ Q_OBJECT
+ using This = VertexDataWidget;
+
+
+ public:
+ VertexDataWidget();
+
+
+ std::optional<GuiGraph::Vertex> vertex();
+ void setVertex(GuiGraph::Vertex vertex);
+ void clearVertex();
+
+ Eigen::Vector3d xyz() const;
+ Eigen::Vector3d rpyDeg() const;
+ Eigen::Vector3d rpyRad() const;
+
+
+ void setRobotConnection(robotvisu::Connection* connection);
+
+
+ signals:
+
+ void vertexDataChanged();
+
+
+ private slots:
+
+ void _updateAngleUnit();
+ void _updateVertexAttribs();
+
+
+ private:
+ void _setFromVertex(const GuiGraph::Vertex& vertex);
+ void _getToVertex(GuiGraph::Vertex& vertex);
+
+ std::vector<QDoubleSpinBox*> _positionSpinBoxes();
+ std::vector<QDoubleSpinBox*> _angleSpinBoxes();
+ std::vector<QDoubleSpinBox*> _allSpinBoxes();
+
+ void _setPose(const Eigen::Matrix4d& pose);
+ 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;
+
+ void _setFromCurrentRobotPose();
+
+
+ private:
+ std::optional<GuiGraph::Vertex> _vertex;
+
+
+ QLineEdit* name = nullptr;
+ 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;
+
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..ecaaca3c7c11a4a690d7e7258de8d82fa767097b
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.cpp
@@ -0,0 +1,308 @@
+/*
+ * 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 <QAction>
+#include <QHeaderView>
+#include <QMenu>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+#include "utils.h"
+
+
+namespace armarx::navigation::locgrapheditor
+{
+
+ VertexTableWidget::VertexTableWidget()
+ {
+ QStringList columns{"Name", "X [mm]", "Y [mm]", "Yaw [\u00b0]"};
+ setColumnCount(columns.size());
+ setHorizontalHeaderLabels(columns);
+ horizontalHeader()->setResizeMode(0, QHeaderView::Stretch);
+ horizontalHeader()->setVisible(true);
+
+ setEditTriggers(QAbstractItemView::NoEditTriggers);
+ setSortingEnabled(true);
+
+ 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);
+ }
+
+
+ QTableWidgetItem*
+ VertexTableWidget::addVertex()
+ {
+ // We need to disable sorting to prevent the new row from being moved away.
+ setSortingEnabled(false);
+
+ QTableWidgetItem* result = nullptr;
+ {
+ 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;
+ }
+ }
+ }
+
+ // Enable sorting - `row` could now be moved away.
+ setSortingEnabled(true);
+
+ ARMARX_CHECK_NOT_NULL(result);
+ return result;
+ }
+
+
+ void
+ VertexTableWidget::updateVertex(GuiGraph::Vertex vertex)
+ {
+ const Eigen::Matrix4d pose = vertex.attrib().getPose().cast<qreal>();
+ char format = 'f';
+ const int precision = 2;
+
+ // Changing the values may trigger a re-sort.
+ setSortingEnabled(false);
+
+
+ int row = this->row(vertex.attrib().tableWidgetItem);
+
+ 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;
+ font.setBold(vertex.attrib().highlighted);
+ 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);
+ }
+ }
+
+
+ void
+ VertexTableWidget::removeVertex(GuiGraph::Vertex& vertex)
+ {
+ if (currentItem() == vertex.attrib().tableWidgetItem)
+ {
+ setCurrentItem(nullptr);
+ }
+
+ removeRow(row(vertex.attrib().tableWidgetItem));
+ vertex.attrib().tableWidgetItem = nullptr;
+ }
+
+
+ QList<QTableWidgetItem*>
+ VertexTableWidget::selectedVertexItems()
+ {
+ return utils::getSelectedItemsOfColumn(this, 0);
+ }
+
+
+ QString
+ VertexTableWidget::_nameOf(QTableWidgetItem* item)
+ {
+ return item->data(Qt::UserRole).toString();
+ }
+
+
+ void
+ VertexTableWidget::makeContextMenu(QPoint pos)
+ {
+ QList<QTableWidgetItem*> items = selectedVertexItems();
+
+ QMenu menu;
+ if (items.size() == 0)
+ {
+ QAction* action = menu.addSection("No locations selected");
+ action->setEnabled(false);
+ }
+
+ // 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 _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}});
+ });
+ }
+
+ // 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";
+
+ 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 addBulkAddEdgeActions = [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(_nameOf(action));
+ 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);
+ });
+ }
+ }
+ };
+
+ 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)
+ {
+ 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));
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..0ce3c98da3cf8fcabdc31672faf426d9b69dd174
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/VertexTableWidget.h
@@ -0,0 +1,82 @@
+/*
+ * 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 <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::navigation::locgrapheditor::utils
+{
+ enum class EdgeDirection;
+}
+namespace armarx::navigation::locgrapheditor
+{
+ class VertexTableWidget : public QTableWidget
+ {
+ Q_OBJECT
+ using This = VertexTableWidget;
+
+
+ public:
+ VertexTableWidget();
+
+
+ QTableWidgetItem* addVertex();
+
+ void updateVertex(GuiGraph::Vertex vertex);
+
+ void removeVertex(GuiGraph::Vertex& vertex);
+
+
+ QList<QTableWidgetItem*> selectedVertexItems();
+
+
+ signals:
+
+ void newVertexRequested();
+
+ void newEdgesRequested(QList<QPair<QTableWidgetItem*, QTableWidgetItem*>> edges);
+ void edgeRemovalRequested(QList<QTableWidgetItem*> vertexItems,
+ utils::EdgeDirection direction);
+
+
+ public slots:
+
+ void makeContextMenu(QPoint pos);
+
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..130485d76bfa3acb10bb04688e1a0fdf4112b224
--- /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::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
new file mode 100644
index 0000000000000000000000000000000000000000..e8067a33da5a8a260dcc57e93a379a9b5023dfb1
--- /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::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
new file mode 100644
index 0000000000000000000000000000000000000000..5baadfdea87ef21728bbd291e7aba2c155b3f030
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene.h
@@ -0,0 +1,16 @@
+#pragma once
+
+
+namespace armarx::navigation::locgrapheditor::graph_scene
+{
+ class Scene;
+ class Widget;
+} // namespace armarx::navigation::locgrapheditor::graph_scene
+
+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
new file mode 100644
index 0000000000000000000000000000000000000000..ee81158baca7ac20a370ac91d487c97a92d5653f
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.cpp
@@ -0,0 +1,113 @@
+#include "ControlWidget.h"
+
+// #include <ArmarXCore/core/logging/Logging.h>
+
+#include <QDoubleSpinBox>
+#include <QFrame>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QPushButton>
+#include <cmath>
+
+
+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/
+ 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);
+ 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);
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..7a2af02426b15691a2f5f2a1f91c8dc2262908a2
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/ControlWidget.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <QWidget>
+
+class QDoubleSpinBox;
+class QHBoxLayout;
+class QPushButton;
+
+
+namespace armarx::navigation::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;
+ };
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..dcd520687591d6e87cbabf9607aeeb940f23124f
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.cpp
@@ -0,0 +1,163 @@
+#include "Scene.h"
+
+#include <Eigen/Core>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <SemanticObjectRelations/Shapes/Shape.h>
+
+
+namespace armarx::navigation::locgrapheditor::graph_scene
+{
+
+ QGraphicsEllipseItem*
+ Scene::addVertex(semrel::ShapeID vertexID, const graph::VertexAttribs& attrib)
+ {
+ 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};
+ 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 '" + attrib.getName() + "'"));
+
+ return item;
+ }
+
+
+ QGraphicsLineItem*
+ 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{
+ [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 '" << sourceAttrib.getName() << "' -> '" << targetAttrib.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)
+ << 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>();
+
+ 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();
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..390c105291bb52c1a6b242ece61fe2a231c33e69
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Scene.h
@@ -0,0 +1,129 @@
+#pragma once
+
+#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::navigation::locgrapheditor::graph_scene
+{
+ class Scene : public QGraphicsScene
+ {
+ Q_OBJECT
+
+ public:
+ using QGraphicsScene::QGraphicsScene;
+
+ 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);
+
+ 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;
+ };
+
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..cfcdc4d6b0123021831d4a81cfcb09b55206d011
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.cpp
@@ -0,0 +1,129 @@
+#include "Widget.h"
+
+#include <QGraphicsView>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QVBoxLayout>
+
+#include "ControlWidget.h"
+#include "Scene.h"
+
+
+namespace armarx::navigation::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();
+
+#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
+ }
+
+
+ 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());
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..149f7cf471cd3e4b313819d86bdf6813efdd7742
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/graph_scene/Widget.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <QWidget>
+
+class QGraphicsView;
+
+
+namespace armarx::navigation::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;
+ };
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..f39dcaa019661566cfca159c9b71823131ccbed3
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.cpp
@@ -0,0 +1,60 @@
+/*
+ * 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::navigation::locgrapheditor
+{
+
+ // <- https://unicode-table.com/de/2190/
+ const QString utils::arrowLeft = QStringLiteral("\u2190");
+ // -> https://unicode-table.com/de/2192/
+ const QString utils::arrowRight = QStringLiteral("\u2192");
+ // <-> https://unicode-table.com/de/21C4/
+ const QString utils::arrowBoth = QStringLiteral("\u21C4");
+
+
+ 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;
+ }
+
+} // 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
new file mode 100644
index 0000000000000000000000000000000000000000..1a879dd97044df42c61d5bd02321858dcfd9f7ff
--- /dev/null
+++ b/source/armarx/navigation/gui-plugins/LocationGraphEditor/widgets/utils.h
@@ -0,0 +1,52 @@
+/*
+ * 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>
+#include <QString>
+
+class QTableWidget;
+class QTableWidgetItem;
+
+
+namespace armarx::navigation::locgrapheditor::utils
+{
+
+ /// <-
+ extern const QString arrowLeft;
+ /// ->
+ extern const QString arrowRight;
+ /// <->
+ extern const QString arrowBoth;
+
+
+ enum class EdgeDirection
+ {
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..45c89d57565840faccda75b923d16624ac70157b
--- /dev/null
+++ b/source/armarx/navigation/location/CMakeLists.txt
@@ -0,0 +1,21 @@
+
+
+armarx_add_aron_library(location_aron
+ ARON_FILES
+ aron/Location.xml
+)
+
+
+armarx_add_library(location
+ DEPENDENCIES
+ ArmarXCoreInterfaces
+ ArmarXCore
+
+ armem
+
+ armarx_navigation::location_aron
+ SOURCES
+ constants.cpp
+ HEADERS
+ constants.h
+)
diff --git a/source/armarx/navigation/location/aron/Location.xml b/source/armarx/navigation/location/aron/Location.xml
new file mode 100644
index 0000000000000000000000000000000000000000..9a0539d5d40a3b3b91dc32ad2034077a815437ff
--- /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::navigation::location::arondto::ObjectRelativeLocation'>
+
+ <ObjectChild key='objectInstanceID'>
+ <armarx::armem::arondto::MemoryID />
+ </ObjectChild>
+
+ <ObjectChild key='relativeRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ </Object>
+
+
+ <Object name='armarx::navigation::location::arondto::Location'>
+
+ <ObjectChild key='globalRobotPose'>
+ <Pose />
+ </ObjectChild>
+
+ <ObjectChild key='relativeToObject'>
+ <armarx::navigation::location::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 0000000000000000000000000000000000000000..5c06c99d049bb83c68045f48af71c1f304fd3df8
--- /dev/null
+++ b/source/armarx/navigation/location/constants.cpp
@@ -0,0 +1,11 @@
+#include "constants.h"
+
+#include <RobotAPI/libraries/armem/core/MemoryID.h>
+
+
+namespace armarx::navigation
+{
+
+ const armem::MemoryID location::coreSegmentID{"Navigation", "Location"};
+
+}
diff --git a/source/armarx/navigation/location/constants.h b/source/armarx/navigation/location/constants.h
new file mode 100644
index 0000000000000000000000000000000000000000..57a99e76c0ce7969c6bced71ac7cbf0028a754dd
--- /dev/null
+++ b/source/armarx/navigation/location/constants.h
@@ -0,0 +1,32 @@
+/**
+ * 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>
+
+
+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 18b8f00913336123bdf078a7877395b04985fae8..e6e05d83ba509797a784cdb3d8266a0f6fe8f151 100644
--- a/source/armarx/navigation/memory/CMakeLists.txt
+++ b/source/armarx/navigation/memory/CMakeLists.txt
@@ -2,15 +2,17 @@ armarx_add_library(memory
SOURCES
#./memory.cpp
./client/stack_result/Writer.cpp
- ./client/parameterization/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/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 677df950880389729ceb5e7aecd762a4763b7e86..64fff8641baa9a2c664f684be3a25d4246f4aa28 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.cpp
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.cpp
@@ -1,16 +1,17 @@
#include "Writer.h"
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/AllNavigators.h>
#include <armarx/navigation/core/constants.h>
+
namespace armarx::navigation::mem::client::param
{
bool
- Writer::store(const std::unordered_map<core::StackLayer,
- aron::datanavigator::DictNavigator::PointerType>& stack,
- const std::string& clientID,
- const core::TimestampUs& timestamp)
+ 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());
@@ -64,4 +65,5 @@ namespace armarx::navigation::mem::client::param
.providerName = "" // clientId
};
}
+
} // 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 fb2982abc8464a09286a33b78223d6921543fd94..aac8fea7640efc921937b639cdb518b339272591 100644
--- a/source/armarx/navigation/memory/client/parameterization/Writer.h
+++ b/source/armarx/navigation/memory/client/parameterization/Writer.h
@@ -22,12 +22,11 @@
#pragma once
#include <RobotAPI/libraries/armem/client/util/SimpleWriterBase.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/container/Dict.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/forward_declarations.h>
#include <armarx/navigation/core/constants.h>
#include <armarx/navigation/core/types.h>
-
namespace armarx::navigation::mem::client::param
{
@@ -38,7 +37,7 @@ namespace armarx::navigation::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);