Implement graph visu

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

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);

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;
 

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
 )
 
 

source/armarx/navigation/graph/Visu.cpp

+/*
+ * 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);
+            }
+        }
+    }
+}

source/armarx/navigation/graph/Visu.h

+/*
+ * 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);
+    };
+
+}

source/armarx/navigation/graph/forward_declarations.h

+/**
+ * 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>;
+
+}