source/armarx/navigation/components/human_simulator/Component.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::human_simulator
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#include "Component.h"
+
+#include <cstdlib>
+
+#include <VirtualRobot/Random.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+#include "armarx/navigation/simulation/SimulatedHuman.h"
+
+// Include headers you only need in function definitions in the .cpp.
+
+// #include <Eigen/Core>
+
+// #include <SimoxUtility/color/Color.h>
+
+
+namespace armarx::navigation::components::human_simulator
+{
+    Component::Component()
+    {
+        addPlugin(virtualRobotReaderPlugin);
+        addPlugin(costmapReaderPlugin);
+        addPlugin(humanWriterPlugin);
+    }
+
+
+    const std::string Component::defaultName = "human_simulator";
+
+
+    armarx::PropertyDefinitionsPtr
+    Component::createPropertyDefinitions()
+    {
+        armarx::PropertyDefinitionsPtr def =
+            new armarx::ComponentPropertyDefinitions(getConfigIdentifier());
+
+        // Publish to a topic (passing the TopicListenerPrx).
+        // def->topic(myTopicListener);
+
+        // Subscribe to a topic (passing the topic name).
+        // def->topic<PlatformUnitListener>("MyTopic");
+
+        // Use (and depend on) another component (passing the ComponentInterfacePrx).
+        // def->component(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.taskPeriodMs, "p.taskPeriodMs", "");
+        def->optional(properties.nHumans, "p.nHumans", "Number of humans to spawn.");
+        
+        def->optional(properties.humanParams.goalDistanceThreshold, "p.human.goalDistanceThreshold", "");
+        def->optional(properties.humanParams.minLinearVelocity, "p.human.maxLinearVelocity", "");
+        def->optional(properties.humanParams.maxLinearVelocity, "p.human.maxLinearVelocity", "");
+
+        return def;
+    }
+
+
+    void
+    Component::onInitComponent()
+    {
+        // Topics and properties defined above are automagically registered.
+
+        // Keep debug observer data until calling `sendDebugObserverBatch()`.
+        // (Requies the armarx::DebugObserverComponentPluginUser.)
+        // setDebugObserverBatchModeEnabled(true);
+    }
+
+
+    void
+    Component::onConnectComponent()
+    {
+        // Do things after connecting to topics and components.
+
+        /* (Requies the armarx::DebugObserverComponentPluginUser.)
+        // Use the debug observer to log data over time.
+        // The data can be viewed in the ObserverView and the LivePlotter.
+        // (Before starting any threads, we don't need to lock mutexes.)
+        {
+            setDebugObserverDatafield("numBoxes", properties.numBoxes);
+            setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+            sendDebugObserverBatch();
+        }
+        */
+
+        /* (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();
+        }
+        */
+
+
+        ///
+
+        //
+        //  Costmaps
+        //
+
+        ARMARX_INFO << "Querying costmap";
+
+        const algorithms::Costmap costmap = [&]() -> algorithms::Costmap
+        {
+            while (true)
+            {
+                const auto timestamp = Clock::Now();
+
+                const memory::client::costmap::Reader::Query costmapQuery{
+                    .providerName = "distance_to_obstacle_costmap_provider", // TODO check
+                    .name = "distance_to_obstacles",
+                    .timestamp = timestamp};
+
+                const memory::client::costmap::Reader::Result costmapResult =
+                    costmapReaderPlugin->get().query(costmapQuery);
+
+                // if costmap is not available yet, wait
+                if (costmapResult.status != memory::client::costmap::Reader::Result::Success)
+                {
+                    Clock::WaitFor(Duration::MilliSeconds(100));
+                    continue;
+                }
+
+                ARMARX_CHECK_EQUAL(costmapResult.status,
+                                   memory::client::costmap::Reader::Result::Success);
+
+                ARMARX_TRACE;
+                ARMARX_CHECK(costmapResult.costmap.has_value());
+                return costmapResult.costmap.value();
+            };
+        }();
+
+        /// create humans
+
+        simulatedHumans.clear();
+
+        for (std::size_t i = 0; i < properties.nHumans; i++)
+        {
+            simulatedHumans.emplace_back(costmap);
+        }
+
+
+        ///
+
+        task = new PeriodicTask<Component>(
+            this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
+        task->start();
+    }
+
+
+    void
+    Component::onDisconnectComponent()
+    {
+        task->stop();
+    }
+
+
+    void
+    Component::onExitComponent()
+    {
+    }
+
+
+    std::string
+    Component::getDefaultName() const
+    {
+        return Component::defaultName;
+    }
+
+
+    std::string
+    Component::GetDefaultName()
+    {
+        return Component::defaultName;
+    }
+
+    void
+    Component::runPeriodically()
+    {
+        // obtain data from perception
+
+        const DateTime timestamp = Clock::Now();
+
+        //
+        // Robot
+        //
+
+        // ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
+        // const core::Pose global_T_robot(robot->getGlobalPose());
+
+        // ARMARX_INFO << "Robot position: " << global_T_robot.translation().head<2>();
+
+
+        human::Humans humans;
+
+        for (auto& simulatedHuman : simulatedHumans)
+        {
+            humans.push_back(simulatedHuman.update());
+        }
+
+        ARMARX_VERBOSE << "Updating humans.";
+        humanWriterPlugin->get().store(humans, getName(), timestamp);
+    }
+
+
+    /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
+    void
+    Component::createRemoteGuiTab()
+    {
+        using namespace armarx::RemoteGui::Client;
+
+        // Setup the widgets.
+
+        tab.boxLayerName.setValue(properties.boxLayerName);
+
+        tab.numBoxes.setValue(properties.numBoxes);
+        tab.numBoxes.setRange(0, 100);
+
+        tab.drawBoxes.setLabel("Draw Boxes");
+
+        // Setup the layout.
+
+        GridLayout grid;
+        int row = 0;
+        {
+            grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
+            ++row;
+
+            grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
+            ++row;
+
+            grid.add(tab.drawBoxes, {row, 0}, {2, 1});
+            ++row;
+        }
+
+        VBoxLayout root = {grid, VSpacer()};
+        RemoteGui_createTab(getName(), root, &tab);
+    }
+
+
+    void
+    Component::RemoteGui_update()
+    {
+        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())
+        {
+            // 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);
+        }
+    }
+    */
+
+
+    /* (Requires the armarx::ArVizComponentPluginUser.)
+    void
+    Component::drawBoxes(const Component::Properties& p, viz::Client& arviz)
+    {
+        // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
+        // See the ArVizExample in RobotAPI for more examples.
+
+        viz::Layer layer = arviz.layer(p.boxLayerName);
+        for (int i = 0; i < p.numBoxes; ++i)
+        {
+            layer.add(viz::Box("box_" + std::to_string(i))
+                      .position(Eigen::Vector3f(i * 100, 0, 0))
+                      .size(20).color(simox::Color::blue()));
+        }
+        arviz.commit(layer);
+    }
+    */
+
+
+    ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
+
+} // namespace armarx::navigation::components::human_simulator

source/armarx/navigation/components/human_simulator/Component.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    navigation::ArmarXObjects::human_simulator
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#pragma once
+
+
+// #include <mutex>
+
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include <ArmarXCore/core/Component.h>
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+
+#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
+
+// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+
+// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
+
+// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+
+#include "armarx/navigation/simulation/SimulatedHuman.h"
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+#include "armarx/navigation/memory/client/human/Writer.h"
+#include <armarx/navigation/components/human_simulator/ComponentInterface.h>
+
+
+namespace armarx::navigation::components::human_simulator
+{
+
+    class Component :
+        virtual public armarx::Component,
+        virtual public armarx::navigation::components::human_simulator::ComponentInterface
+    // , virtual public armarx::DebugObserverComponentPluginUser
+    // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+    // , virtual public armarx::ArVizComponentPluginUser
+    {
+    public:
+        Component();
+
+        /// @see armarx::ManagedIceObject::getDefaultName()
+        std::string getDefaultName() const override;
+
+        /// Get the component's default name.
+        static std::string GetDefaultName();
+
+
+    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;
+
+
+        /* (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 runPeriodically();
+
+
+
+    private:
+        static const std::string defaultName;
+
+        PeriodicTask<Component>::pointer_type task;
+
+
+
+        // Private member variables go here.
+
+
+        /// Properties shown in the Scenario GUI.
+        struct Properties
+        {
+            int taskPeriodMs = 100;
+
+
+            std::size_t nHumans = 4;
+
+            human::simulation::SimulatedHumanParams humanParams;
+        };
+        Properties properties;
+        /* Use a mutex if you access variables from different threads
+         * (e.g. ice functions and RemoteGui_update()).
+        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;
+
+            armarx::RemoteGui::Client::Button drawBoxes;
+        };
+        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;
+        */
+
+        template <typename T>
+        using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
+
+
+        ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
+            nullptr;
+
+        ReaderWriterPlugin<memory::client::costmap::Reader>* costmapReaderPlugin = nullptr;
+
+
+        ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+
+    
+        std::vector<human::simulation::SimulatedHuman> simulatedHumans;
+
+
+    };
+
+} // namespace armarx::navigation::components::human_simulator

source/armarx/navigation/components/human_simulator/ComponentInterface.ice

+/*
+ * 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::human_simulator
+ * author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * date       2022
+ * copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *            GNU General Public License
+ */
+
+
+#pragma once
+
+
+module armarx {  module navigation {  module components {  module human_simulator 
+{
+
+    interface ComponentInterface
+    {
+	// Define your interface here.
+    };
+
+};};};};

source/armarx/navigation/components/navigation_memory/Component.h

@@ -101,7 +101,7 @@ namespace armarx::navigation::components::navigation_memory
                 bool visuCostmaps = true;
                 bool visuHumans = true;
                 
-                float visuFrequency = 2;
+                float visuFrequency = 10;
             };
             LocationGraph locationGraph;
         };

source/armarx/navigation/components/navigation_memory/Visu.cpp

@@ -21,12 +21,15 @@
  */
 
 #include "Visu.h"
+#include <Eigen/src/Geometry/Translation.h>
 
 #include <SimoxUtility/color/Color.h>
 #include <SimoxUtility/color/cmaps/colormaps.h>
 
 #include "RobotAPI/components/ArViz/Client/Elements.h"
 #include "RobotAPI/components/ArViz/Client/Layer.h"
+#include "RobotAPI/components/ArViz/Client/elements/Color.h"
+#include "RobotAPI/components/ArViz/Client/elements/Robot.h"
 #include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
 
 #include "armarx/navigation/conversions/eigen.h"
@@ -176,18 +179,29 @@ namespace armarx::navigation::memory
         void
         visualize(const human::Humans& humans, viz::Layer& layer)
         {
+
+            const Eigen::Translation3f human_T_mmm(Eigen::Vector3f{0,0, 1000});
+
             ARMARX_INFO << "Visualizing " << humans.size() << " humans";
             for (const auto& human : humans)
             {
-                viz::Cylinder cylinder(std::to_string(layer.size()));
-                cylinder.fromTo(conv::to3D(human.pose.translation()),
-                                conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+                // viz::Cylinder cylinder(std::to_string(layer.size()));
+                // cylinder.fromTo(conv::to3D(human.pose.translation()),
+                //                 conv::to3D(human.pose.translation()) + Eigen::Vector3f{0, 0, 10});
+
+
+                // cylinder.color(simox::Color::orange());
+                // cylinder.radius(300);
+                // layer.add(cylinder);
 
 
-                cylinder.color(simox::Color::orange());
-                cylinder.radius(300);
+                viz::Robot mmm(std::to_string(layer.size()));
+                mmm.file("RobotAPI", "RobotAPI/robots/MMM/mmm.xml");
+                mmm.pose(conv::to3D(human.pose) * human_T_mmm);
+                mmm.scale(1.7); // 1.7m 
+                mmm.overrideColor(viz::Color::orange(255, 100));
+                layer.add(mmm);
 
-                layer.add(cylinder);
             }
         }
 

source/armarx/navigation/components/navigation_memory/Visu.h

@@ -25,6 +25,7 @@
 #include <memory>
 #include <vector>
 
+#include "RobotAPI/components/ArViz/Client/ScopedClient.h"
 #include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
 #include <RobotAPI/components/ArViz/Client/Client.h>
 #include <RobotAPI/libraries/armem/core/forward_declarations.h>
@@ -57,8 +58,7 @@ namespace armarx::navigation::memory
         void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
 
 
-    public:
-        viz::Client arviz;
+        viz::ScopedClient arviz;
 
         const armem::server::wm::CoreSegment& locSegment;
         const armem::server::wm::CoreSegment& graphSegment;

source/armarx/navigation/core/aron/Events.xml

@@ -46,6 +46,12 @@
             </ObjectChild>
         </Object>
 
+        <Object name='armarx::navigation::core::arondto::LocalPlanningFailedEvent'>
+            <ObjectChild key='message'>
+                <String />
+            </ObjectChild>
+        </Object>
+
         <Object name='armarx::navigation::core::arondto::SafetyThrottlingTriggeredEvent'>
             <ObjectChild key='pose'>
                 <Pose />

source/armarx/navigation/core/aron_conversions.cpp

 #include "aron_conversions.h"
 
-#include <range/v3/range/conversion.hpp>
-#include <range/v3/view/transform.hpp>
-
 #include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 #include <RobotAPI/libraries/core/Trajectory.h>
@@ -10,6 +7,8 @@
 #include <armarx/navigation/core/Trajectory.h>
 #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
 #include <armarx/navigation/core/types.h>
+#include <range/v3/range/conversion.hpp>
+#include <range/v3/view/transform.hpp>
 
 namespace armarx::navigation::core
 {
@@ -272,4 +271,18 @@ namespace armarx::navigation::core
         aron::fromAron(dto.pose, bo.pose.matrix());
     }
 
+    void
+    fromAron(const armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+             armarx::navigation::core::LocalPlanningFailedEvent& bo)
+    {
+        aron::fromAron(dto.message, bo.message);
+    }
+
+    void
+    toAron(armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+           const armarx::navigation::core::LocalPlanningFailedEvent& bo)
+    {
+        aron::toAron(dto.message, bo.message);
+    }
+
 } // namespace armarx::navigation::core

source/armarx/navigation/core/aron_conversions.h

@@ -82,6 +82,12 @@ namespace armarx::navigation::core
     void toAron(armarx::navigation::core::arondto::GlobalPlanningFailedEvent& dto,
                 const armarx::navigation::core::GlobalPlanningFailedEvent& bo);
 
+      void fromAron(const armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+                  armarx::navigation::core::LocalPlanningFailedEvent& bo);
+
+    void toAron(armarx::navigation::core::arondto::LocalPlanningFailedEvent& dto,
+                const armarx::navigation::core::LocalPlanningFailedEvent& bo);
+
     void fromAron(const armarx::navigation::core::arondto::MovementStartedEvent& dto,
                   armarx::navigation::core::MovementStartedEvent& bo);
 

source/armarx/navigation/core/events.h

@@ -13,6 +13,7 @@ namespace armarx::navigation::core
     namespace event_names
     {
         inline const std::string GlobalPlanningFailed = "GlobalPlanningFailedEvent";
+        inline const std::string LocalPlanningFailed = "LocalPlanningFailedEvent";
         inline const std::string MovementStarted = "MovementStartedEvent";
         inline const std::string GoalReached = "GoalReachedEvent";
         inline const std::string WaypointReached = "WaypointReachedEvent";
@@ -31,6 +32,11 @@ namespace armarx::navigation::core
     {
         std::string message;
     };
+    
+    struct LocalPlanningFailedEvent : public Event
+    {
+        std::string message;
+    };
 
     struct MovementStartedEvent : Event
     {

source/armarx/navigation/global_planning/SPFA.cpp

@@ -91,6 +91,9 @@ namespace armarx::navigation::global_planning
         // FIXME check if costmap is available
 
         algorithms::spfa::ShortestPathFasterAlgorithm::Parameters spfaParams;
+
+        ARMARX_CHECK(scene.staticScene.has_value());
+        ARMARX_CHECK(scene.staticScene->distanceToObstaclesCostmap.has_value());
         algorithms::spfa::ShortestPathFasterAlgorithm planner(*scene.staticScene->distanceToObstaclesCostmap,
                                                               spfaParams);
 

source/armarx/navigation/global_planning/optimization/OrientationOptimizer.cpp

@@ -15,8 +15,6 @@
 #include <ceres/rotation.h>
 #include <ceres/sized_cost_function.h>
 
-#include <Eigen/src/Geometry/Rotation2D.h>
-
 #include <SimoxUtility/math/convert/mat4f_to_xyyaw.h>
 #include <SimoxUtility/math/convert/rpy_to_mat3f.h>
 #include <SimoxUtility/math/periodic/periodic_clamp.h>

source/armarx/navigation/local_planning/TebObstacleManager.cpp

@@ -87,7 +87,7 @@ namespace armarx::navigation::local_planning
             // visualize proxemic zone if layer is available
             if (visLayer != nullptr)
             {
-                const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
+                const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 10.f-i);
                 const core::Pose pose3d = conv::to3D(proxemicZone.pose);
 
                 visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))

source/armarx/navigation/local_planning/TimedElasticBands.cpp

 #include "TimedElasticBands.h"
+#include <optional>
 
 #include <SimoxUtility/algorithm/apply.hpp>
 #include <SimoxUtility/json/json.hpp>
@@ -124,6 +125,13 @@ namespace armarx::navigation::local_planning
             ARMARX_DEBUG << "Caugth exception while planning: " << e.what();
             return {};
         }
+
+        if(hcp_->getTrajectoryContainer().empty())
+        {
+            ARMARX_VERBOSE << "Did not find any trajectory!";
+            return std::nullopt;
+        }
+
         ARMARX_VERBOSE << "Planned successfully (found " << hcp_->getTrajectoryContainer().size()
                        << " Trajectories)";
 

source/armarx/navigation/memory/client/events/Writer.cpp

@@ -89,6 +89,14 @@ namespace armarx::navigation::memory::client::events
             event, core::event_names::GlobalPlanningFailed, clientID);
     }
 
+        bool
+    Writer::store(const core::LocalPlanningFailedEvent& event, const std::string& clientID)
+    {
+        return storeImpl<core::arondto::LocalPlanningFailedEvent>(
+            event, core::event_names::LocalPlanningFailed, clientID);
+    }
+
+
     bool
     Writer::store(const core::MovementStartedEvent& event, const std::string& clientID)
     {

source/armarx/navigation/memory/client/events/Writer.h

@@ -42,6 +42,7 @@ namespace armarx::navigation::memory::client::events
         bool store(const core::InternalErrorEvent& event, const std::string& clientID);
         bool store(const core::UserAbortTriggeredEvent& event, const std::string& clientID);
         bool store(const core::GlobalPlanningFailedEvent& event, const std::string& clientID);
+        bool store(const core::LocalPlanningFailedEvent& event, const std::string& clientID);
         bool store(const core::MovementStartedEvent& event, const std::string& clientID);
         bool store(const core::SafetyThrottlingTriggeredEvent& event, const std::string& clientID);
         bool store(const core::SafetyStopTriggeredEvent& event, const std::string& clientID);

source/armarx/navigation/server/Navigator.cpp

@@ -2,6 +2,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <cstddef>
 #include <optional>
 #include <thread>
 
@@ -805,9 +806,10 @@ namespace armarx::navigation::server
                 armarx::core::time::StopWatch::measure([&]() { updateScene(); });
 
             ARMARX_DEBUG << deactivateSpam(0.2)
-                           << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
+                         << "Scene update: " << duration.toMilliSecondsDouble() << "ms.";
 
-            srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]", duration.toMilliSecondsDouble());
+            srv.debugObserverHelper->setDebugObserverDatafield("scene update [ms]",
+                                                               duration.toMilliSecondsDouble());
         }
 
         // global planner update if goal has changed
@@ -882,9 +884,10 @@ namespace armarx::navigation::server
                     }
                 });
             ARMARX_DEBUG << deactivateSpam(0.2)
-                           << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
+                         << "Local planner update: " << duration.toMilliSecondsDouble() << "ms.";
 
-            srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]", duration.toMilliSecondsDouble());
+            srv.debugObserverHelper->setDebugObserverDatafield("local planner update [ms]",
+                                                               duration.toMilliSecondsDouble());
         }
 
         // monitor update
@@ -895,9 +898,10 @@ namespace armarx::navigation::server
                 armarx::core::time::StopWatch::measure([&]() { updateMonitor(); });
 
             ARMARX_DEBUG << deactivateSpam(0.2)
-                           << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
+                         << "Monitor update: " << duration.toMilliSecondsDouble() << "ms.";
 
-            srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]", duration.toMilliSecondsDouble());
+            srv.debugObserverHelper->setDebugObserverDatafield("monitor update [ms]",
+                                                               duration.toMilliSecondsDouble());
         }
     }
 
@@ -913,18 +917,28 @@ namespace armarx::navigation::server
     {
         ARMARX_CHECK(hasLocalPlanner());
 
-        localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
-
-        if (localPlan.has_value())
+        try
         {
-            srv.publisher->localTrajectoryUpdated(localPlan.value());
+            localPlan = config.stack.localPlanner->plan(globalPlan->trajectory);
+            if (localPlan.has_value())
+            {
+                srv.publisher->localTrajectoryUpdated(localPlan.value());
+            }
+            return localPlan;
         }
-        else
+        catch (...)
         {
-            // srv.publisher->localTrajectoryPlanningFailed();
+            ARMARX_WARNING << "Failure in local planner: " << GetHandledExceptionString();
+            srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
+                {.timestamp = armarx::Clock::Now()}, {GetHandledExceptionString()}});
+
+            return std::nullopt;
         }
 
-        return localPlan;
+        srv.publisher->localPlanningFailed(core::LocalPlanningFailedEvent{
+            {.timestamp = armarx::Clock::Now()}, {"Unknown reason"}});
+
+        return std::nullopt;
     }
 
     void
@@ -947,6 +961,7 @@ namespace armarx::navigation::server
         {
             ARMARX_INFO << "Local plan is invalid!";
             srv.executor->stop();
+            return;
         }
 
 

source/armarx/navigation/server/event_publishing/EventPublishingInterface.h

@@ -24,6 +24,7 @@ namespace armarx::navigation::server
         // trajectoryControllerUpdated(const traj_ctrl::local::TrajectoryControllerResult& res) = 0;
 
         virtual void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) = 0;
+        virtual void localPlanningFailed(const core::LocalPlanningFailedEvent& event) = 0;
 
 
         virtual void movementStarted(const core::MovementStartedEvent& event) = 0;

source/armarx/navigation/server/event_publishing/MemoryPublisher.cpp

@@ -70,6 +70,11 @@ namespace armarx::navigation::server
     {
         eventsWriter->store(event, clientId);
     }
+    
+    void MemoryPublisher::localPlanningFailed(const core::LocalPlanningFailedEvent& event) 
+    {
+        eventsWriter->store(event, clientId);
+    }
 
     void
     MemoryPublisher::movementStarted(const core::MovementStartedEvent& event)

source/armarx/navigation/server/event_publishing/MemoryPublisher.h

@@ -22,6 +22,8 @@ namespace armarx::navigation::server
         //     const traj_ctrl::local::TrajectoryControllerResult& res) override;
 
         void globalPlanningFailed(const core::GlobalPlanningFailedEvent& event) override;
+        void localPlanningFailed(const core::LocalPlanningFailedEvent& event) override;
+
 
         void movementStarted(const core::MovementStartedEvent& event) override;
         void goalReached(const core::GoalReachedEvent& event) override;