Merge remote-tracking branch 'origin/master' into feature/costmap-integration

source/armarx/navigation/algorithms/CMakeLists.txt

@@ -27,6 +27,7 @@ armarx_add_library(algorithms
         ./smoothing/ChainApproximation.cpp
         ./smoothing/CircularPathSmoothing.cpp
     HEADERS
+        ./types.h
         ./algorithms.h
         ./Costmap.h
         ./CostmapBuilder.h

source/armarx/navigation/client/CMakeLists.txt

@@ -31,6 +31,7 @@ armarx_add_library(client
         ./services/SimpleEventHandler.cpp
         ./services/IceNavigator.cpp
     HEADERS
+        ./types.h
         ../client.h
         ./Navigator.h
         ./NavigationStackConfig.h

source/armarx/navigation/client/ComponentPlugin.cpp

@@ -81,22 +81,6 @@ armarx::navigation::client::ComponentPlugin::configureNavigator(
     navigator = std::make_unique<Navigator>(
         Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
 }
-void
-armarx::navigation::client::ComponentPlugin::configureNavigator(
-    const armarx::navigation::client::NavigationStackConfig& stackConfig,
-    const std::string& configId,
-    armarx::armem::client::MemoryNameSystem& memoryNameSystem)
-{
-    ARMARX_TRACE;
-
-    // ARMARX_CHECK_NULL(eventHandler) << "`configureNavigator()` can only be called once!";
-    eventHandler = std::make_unique<MemorySubscriber>(configId, memoryNameSystem);
-
-    iceNavigator.createConfig(stackConfig, configId);
-
-    navigator = std::make_unique<Navigator>(
-        Navigator::InjectedServices{.navigator = &iceNavigator, .subscriber = eventHandler.get()});
-}
 
 // ComponentPluginUser
 

source/armarx/navigation/client/ComponentPlugin.h

@@ -37,10 +37,6 @@ namespace armarx::navigation::client
         void configureNavigator(const client::NavigationStackConfig& stackConfig,
                                 const std::string& configId);
 
-        void configureNavigator(const client::NavigationStackConfig& stackConfig,
-                                const std::string& configId,
-                                armarx::armem::client::MemoryNameSystem& memoryNameSystem);
-
     private:
         static constexpr const char* PROPERTY_NAME = "nav.NavigatorName";
 

source/armarx/navigation/client/Navigator.cpp

@@ -28,6 +28,7 @@ namespace armarx::navigation::client
         srv.subscriber->onSafetyStopTriggered(stopped_callback);
         srv.subscriber->onUserAbortTriggered(stopped_callback);
         srv.subscriber->onInternalError(stopped_callback);
+        srv.subscriber->onGlobalPlanningFailed(stopped_callback);
     }
 
 

source/armarx/navigation/client/Navigator.h

@@ -105,7 +105,8 @@ namespace armarx::navigation::client
         using StopEventVar = std::variant<core::GoalReachedEvent,
                                           core::SafetyStopTriggeredEvent,
                                           core::UserAbortTriggeredEvent,
-                                          core::InternalErrorEvent>;
+                                          core::InternalErrorEvent,
+                                          core::GlobalPlanningFailedEvent>;
         StopEventVar event;
     };
 

source/armarx/navigation/client/services/EventSubscriptionInterface.h

@@ -51,6 +51,8 @@ namespace armarx::navigation::client
 
         virtual void onInternalError(const OnInternalErrorCallback& callback) = 0;
 
+        virtual void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) = 0;
+
         // Non-API.
     public:
         virtual ~EventSubscriptionInterface() = default;

source/armarx/navigation/client/services/SimpleEventHandler.cpp

@@ -54,6 +54,11 @@ armarx::navigation::client::SimpleEventHandler::SimpleEventHandler::onMovementSt
     subscriptions.movementStartedCallbacks.push_back(callback);
 }
 
+void
+armarx::navigation::client::SimpleEventHandler::onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback)
+{
+    subscriptions.globalPlanningFailedCallbacks.push_back(callback);
+}
 
 void
 armarx::navigation::client::SimpleEventHandler::goalReached(const core::GoalReachedEvent& event)

source/armarx/navigation/client/services/SimpleEventHandler.h

@@ -25,6 +25,7 @@ namespace armarx::navigation::client
         void onInternalError(const OnInternalErrorCallback& callback) override;
 
         void onMovementStarted(const OnMovementStartedCallback& callback) override;
+        void onGlobalPlanningFailed(const GlobalPlanningFailedCallback& callback) override;
 
         // EventPublishingInterface
     public:

source/armarx/navigation/core/CMakeLists.txt

@@ -19,6 +19,11 @@ armarx_add_library(core
         json_conversions.cpp
         time/ChronoMonotonicTimeServer.cpp
     HEADERS
+        Trajectory.h
+        constants.h
+        basic_types.h
+        forward_declarations.h
+        fwd.h
         types.h
         events.h
         NavigatorInterface.h

source/armarx/navigation/factories/CMakeLists.txt

@@ -11,6 +11,7 @@ armarx_add_library(factories
         ./SafetyControllerFactory.h
         ./TrajectoryControllerFactory.h
         ./NavigationStackFactory.h
+        ./fwd.h
     DEPENDENCIES
         ArmarXCoreInterfaces
         ArmarXCore

source/armarx/navigation/global_planning/CMakeLists.txt

@@ -26,6 +26,7 @@ armarx_add_library(global_planning
         ./aron_conversions.cpp
         ./optimization/OrientationOptimizer.cpp
     HEADERS
+        ./core.h
         ./GlobalPlanner.h
         ./AStar.h
         ./SPFA.h

source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.cpp

-#include "GraphScene.h"
-
-#include <Eigen/Core>
-
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-
-#include <SemanticObjectRelations/Shapes/Shape.h>
-
-
-namespace armarx::navigation::qt_plugins::location_graph_editor
-{
-
-    QGraphicsEllipseItem*
-    GraphScene::addVertex(const graph::Graph::Vertex& vertex)
-    {
-        const core::Pose& 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();
-
-        core::Pose_d sourcePose = edge.source().attrib().getPose().cast<qreal>();
-        core::Pose_d 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 core::Pose_d 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);
-
-        core::Pose_d sourcePose = edge.source().attrib().getPose().cast<qreal>();
-        core::Pose_d 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::qt_plugins::location_graph_editor

source/armarx/navigation/gui-plugins/LocationGraphEditor/GraphScene.h

-#pragma once
-
-#include <QGraphicsEllipseItem>
-#include <QGraphicsLineItem>
-#include <QGraphicsScene>
-#include <functional>
-
-#include "GuiGraph.h"
-#include <armarx/navigation/core/Graph.h>
-
-
-namespace semrel
-{
-    struct ShapeID;
-}
-namespace armarx::navigation::qt_plugins::location_graph_editor
-{
-
-    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::qt_plugins::location_graph_editor

source/armarx/navigation/local_planning/CMakeLists.txt

@@ -4,8 +4,11 @@ armarx_add_library(local_planning
         ArmarXCoreInterfaces
         ArmarXCore
         armarx_navigation::core
-    SOURCES ./LocalPlanner.cpp
-            ./TimedElasticBands.cpp
-    HEADERS ./LocalPlanner.h
-            ./TimedElasticBands.h
+    SOURCES 
+        ./LocalPlanner.cpp
+        ./TimedElasticBands.cpp
+    HEADERS
+        ./LocalPlanner.h
+        ./TimedElasticBands.h
+        core.h
 )

source/armarx/navigation/memory/CMakeLists.txt

@@ -8,7 +8,7 @@ armarx_add_library(memory
         ./client/events/Writer.cpp
         # ./client/events/Reader.cpp
     HEADERS
-        #./memory.h
+        ./memory.h
         ./client/stack_result/Writer.h
         ./client/parameterization/Reader.h
         ./client/parameterization/Writer.h

source/armarx/navigation/memory/memory.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    Navigation::ArmarXObjects::memory
- * @author     Fabian Reister ( fabian dot reister at kit dot edu )
- * @date       2021
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-#include "memory.h"
-
-namespace armarx
-{
-
-}

source/armarx/navigation/safety_control/CMakeLists.txt

@@ -7,4 +7,5 @@ armarx_add_library(safety_control
             ./LaserBasedProximity.cpp
     HEADERS ./SafetyController.h
             ./LaserBasedProximity.h
+            core.h
 )

source/armarx/navigation/trajectory_control/CMakeLists.txt

@@ -16,6 +16,7 @@ armarx_add_library(trajectory_control
         ./TrajectoryFollowingController.h
         ./WaypointController.h
         ./aron_conversions.h
+        ./core.h
     DEPENDENCIES
         ArmarXCoreInterfaces
         ArmarXCore