From 80d0e49a33a273ec6eb83772e0f59861929e184f Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 19 Aug 2021 08:28:27 +0200
Subject: [PATCH] Implement graph visu
---
.../GraphImportExport/CMakeLists.txt | 2 +-
.../GraphImportExport/GraphImportExport.cpp | 23 +++-
.../GraphImportExport/GraphImportExport.h | 3 +
source/armarx/navigation/graph/CMakeLists.txt | 2 +
source/armarx/navigation/graph/Visu.cpp | 108 ++++++++++++++++++
source/armarx/navigation/graph/Visu.h | 92 +++++++++++++++
.../navigation/graph/forward_declarations.h | 39 +++++++
7 files changed, 265 insertions(+), 4 deletions(-)
create mode 100644 source/armarx/navigation/graph/Visu.cpp
create mode 100644 source/armarx/navigation/graph/Visu.h
create mode 100644 source/armarx/navigation/graph/forward_declarations.h
diff --git a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
index 16d673fe..e5d7c37d 100644
--- a/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
+++ b/source/armarx/navigation/components/GraphImportExport/CMakeLists.txt
@@ -23,7 +23,7 @@ armarx_add_component(
# RobotAPI
## RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
armem
# MemoryX
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
index 3804695b..14b6704b 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.cpp
@@ -26,9 +26,12 @@
#include <armarx/navigation/location/aron/Location.aron.generated.h>
#include <armarx/navigation/location/constants.h>
+
#include <armarx/navigation/graph/aron/Graph.aron.generated.h>
#include <armarx/navigation/graph/constants.h>
#include <armarx/navigation/graph/Graph.h>
+#include <armarx/navigation/graph/Visu.h>
+
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
@@ -114,6 +117,7 @@ namespace armarx::nav
tab.providerSegmentLine.setValue(getName());
tab.dryRun.setValue(false);
+ tab.visuEnabled.setValue(true);
tab.locationsMemoryxToArmemButton.setLabel("Locations MemoryX -> ArMem");
tab.locationsArmemToMemoryxButton.setLabel("Locations ArMem -> MemoryX (WIP)");
@@ -136,6 +140,9 @@ namespace armarx::nav
grid.add(Label("Dry Run:"), {row, 0}).add(tab.dryRun, {row, 1});
++row;
+ grid.add(Label("Enable visu:"), {row, 0}).add(tab.visuEnabled, {row, 1});
+ ++row;
+
grid.add(tab.locationsMemoryxToArmemButton, {row, 0}).add(tab.locationsArmemToMemoryxButton, {row, 1})
.add(tab.locationsClearArMemButton, {row, 2});
++row;
@@ -286,6 +293,18 @@ namespace armarx::nav
nav::graph::arondto::Graph aron;
toAron(aron, graph);
+
+ if (tab.visuEnabled.getValue())
+ {
+ nav::graph::GraphVisu visu;
+ viz::Layer layer = arviz.layer("Graph '" + sceneName + "'");
+ visu.draw(layer, graph);
+
+ ARMARX_VERBOSE << "Visualize graph '" << sceneName << "' ...";
+ arviz.commit(layer);
+ }
+
+
// Build commit
const armem::Time time = armem::Time::now();
armem::EntityUpdate update;
@@ -370,11 +389,9 @@ namespace armarx::nav
{
auto adjacent = memoryx::GraphNodeBasePtr::dynamicCast(node->getAdjacentNode(i)->getEntity());
ARMARX_CHECK_NOT_NULL(adjacent);
-
const auto& targetVertex = vertexMap.at(adjacent->getName());
- ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " -> \t" << adjacent->getName();
-
+ ARMARX_VERBOSE << "\n- Adding edge: \t" << node->getName() << " \t-> " << adjacent->getName();
nav::graph::Graph::Edge edge = graph.addEdge(sourceVertex, targetVertex);
edge.attrib().aron.sourceVertexID = static_cast<long>(sourceVertex.attrib().aron.vertexID);
edge.attrib().aron.targetVertexID = static_cast<long>(targetVertex.attrib().aron.vertexID);
diff --git a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
index 90878834..efd4aed1 100644
--- a/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
+++ b/source/armarx/navigation/components/GraphImportExport/GraphImportExport.h
@@ -30,6 +30,7 @@
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <RobotAPI/libraries/armem/client/ComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
@@ -55,6 +56,7 @@ namespace armarx::nav
virtual public armarx::Component
, virtual public armarx::DebugObserverComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ , virtual public armarx::ArVizComponentPluginUser
, virtual public armarx::armem::client::ComponentPluginUser
{
public:
@@ -151,6 +153,7 @@ namespace armarx::nav
armarx::RemoteGui::Client::Button graphClearArMemButton;
armarx::RemoteGui::Client::CheckBox dryRun;
+ armarx::RemoteGui::Client::CheckBox visuEnabled;
};
RemoteGuiTab tab;
diff --git a/source/armarx/navigation/graph/CMakeLists.txt b/source/armarx/navigation/graph/CMakeLists.txt
index 064597f7..530fe93e 100644
--- a/source/armarx/navigation/graph/CMakeLists.txt
+++ b/source/armarx/navigation/graph/CMakeLists.txt
@@ -16,11 +16,13 @@ armarx_add_library(
SOURCES
constants.cpp
Graph.cpp
+ Visu.cpp
HEADERS
constants.h
forward_declarations.h
Graph.h
+ Visu.h
)
diff --git a/source/armarx/navigation/graph/Visu.cpp b/source/armarx/navigation/graph/Visu.cpp
new file mode 100644
index 00000000..62ede64f
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.cpp
@@ -0,0 +1,108 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MemoryX::ArmarXObjects::GraphImportExport
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include <VirtualRobot/VirtualRobot.h>
+
+#include "Visu.h"
+
+#include <RobotAPI/components/ArViz/Client/Client.h>
+
+#include <SimoxUtility/color/Color.h>
+#include <SimoxUtility/math/pose.h>
+
+
+namespace armarx::nav::graph
+{
+
+ viz::Pose VertexVisu::Pose::draw(Graph::ConstVertex vertex) const
+ {
+ const arondto::Vertex& aron = vertex.attrib().aron;
+ return viz::Pose(aron.locationID.entityName)
+ .pose(aron.globalRobotPose)
+ .scale(scale);
+ }
+
+
+ viz::Arrow VertexVisu::ForwardArrow::draw(Graph::ConstVertex vertex) const
+ {
+ const arondto::Vertex& aron = vertex.attrib().aron;
+ return viz::Arrow(aron.locationID.entityName + " forward")
+ .fromTo(simox::math::position(aron.globalRobotPose),
+ simox::math::transform_position(aron.globalRobotPose, length * Eigen::Vector3f::UnitY()))
+ .color(color)
+ .width(width);
+ }
+
+
+ void VertexVisu::draw(viz::Layer& layer, Graph::ConstVertex vertex) const
+ {
+ if (pose.has_value())
+ {
+ layer.add(pose->draw(vertex));
+ }
+ if (forwardArrow.has_value())
+ {
+ layer.add(forwardArrow->draw(vertex));
+ }
+ }
+
+
+ viz::Arrow EdgeVisu::Arrow::draw(Graph::ConstEdge edge) const
+ {
+ const auto& sourceAron = edge.source().attrib().aron;
+ const auto& targetAron = edge.target().attrib().aron;
+ return viz::Arrow(sourceAron.locationID.entityName + " -> " +
+ targetAron.locationID.entityName)
+ .fromTo(simox::math::position(sourceAron.globalRobotPose),
+ simox::math::position(targetAron.globalRobotPose))
+ .width(width)
+ .color(color);
+ }
+
+
+ void EdgeVisu::draw(viz::Layer& layer, Graph::ConstEdge edge) const
+ {
+ if (arrow.has_value())
+ {
+ layer.add(arrow->draw(edge));
+ }
+ }
+
+
+ void GraphVisu::draw(viz::Layer& layer, const Graph& graph)
+ {
+ if (vertex.has_value())
+ {
+ for (Graph::ConstVertex v : graph.vertices())
+ {
+ vertex->draw(layer, v);
+ }
+ }
+ if (edge.has_value())
+ {
+ for (Graph::ConstEdge e : graph.edges())
+ {
+ edge->draw(layer, e);
+ }
+ }
+ }
+}
diff --git a/source/armarx/navigation/graph/Visu.h b/source/armarx/navigation/graph/Visu.h
new file mode 100644
index 00000000..6e2516bc
--- /dev/null
+++ b/source/armarx/navigation/graph/Visu.h
@@ -0,0 +1,92 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package MemoryX::ArmarXObjects::GraphImportExport
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <armarx/navigation/graph/Graph.h>
+
+#include <SimoxUtility/color/Color.h>
+
+#include <optional>
+
+
+namespace armarx::viz
+{
+ class Arrow;
+ class Layer;
+ class Pose;
+}
+
+namespace armarx::nav::graph
+{
+
+ struct VertexVisu
+ {
+ struct Pose
+ {
+ float scale = 1.0;
+
+ viz::Pose draw(Graph::ConstVertex vertex) const;
+ };
+ std::optional<Pose> pose = Pose {};
+
+ struct ForwardArrow
+ {
+ float width = 7.5;
+ float length = 100.0;
+ simox::Color color = simox::Color::cyan(220);
+
+ viz::Arrow draw(Graph::ConstVertex vertex) const;
+ };
+ std::optional<ForwardArrow> forwardArrow = ForwardArrow {};
+
+
+ void draw(viz::Layer& layer, Graph::ConstVertex vertex) const;
+ };
+
+
+ struct EdgeVisu
+ {
+ struct Arrow
+ {
+ float width = 5.0;
+ simox::Color color = simox::Color::azure(196);
+
+ viz::Arrow draw(Graph::ConstEdge edge) const;
+ };
+ std::optional<Arrow> arrow = Arrow {};
+
+
+ void draw(viz::Layer& layer, Graph::ConstEdge edge) const;
+ };
+
+
+ struct GraphVisu
+ {
+ std::optional<VertexVisu> vertex = VertexVisu {};
+ std::optional<EdgeVisu> edge = EdgeVisu {};
+
+
+ void draw(viz::Layer& layer, const Graph& graph);
+ };
+
+}
diff --git a/source/armarx/navigation/graph/forward_declarations.h b/source/armarx/navigation/graph/forward_declarations.h
new file mode 100644
index 00000000..fad17f2b
--- /dev/null
+++ b/source/armarx/navigation/graph/forward_declarations.h
@@ -0,0 +1,39 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+
+namespace semrel
+{
+ template <class VertexAttribT, class EdgeAttribT, class GraphAttribT>
+ class RelationGraph;
+}
+namespace armarx::nav::graph
+{
+
+ struct VertexAttribs;
+ struct EdgeAttribs;
+ struct GraphAttribs;
+
+ using Graph = semrel::RelationGraph<VertexAttribs, EdgeAttribs, GraphAttribs>;
+
+}
--
GitLab