source/armarx/navigation/server/execution/PlatformControllerExecutor.cpp

@@ -80,6 +80,11 @@ namespace armarx::navigation::server
 
         // sends the updated config to the controller and stores it in the memory
         ctrl->updateConfig();
+
+        if(not ctrl->ctrl()->isControllerActive())
+        {
+            start();
+        }
     }
 
     void

source/armarx/navigation/server/introspection/ArvizIntrospector.cpp

@@ -6,6 +6,7 @@
 #include <Eigen/Geometry>
 
 #include <SimoxUtility/algorithm/apply.hpp>
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <SimoxUtility/color/Color.h>
 #include <SimoxUtility/color/ColorMap.h>
 #include <SimoxUtility/color/cmaps/colormaps.h>
@@ -45,7 +46,7 @@ namespace armarx::navigation::server
         ARMARX_DEBUG << "ArvizIntrospector::onGlobalPlannerResult";
 
         drawGlobalTrajectory(result.trajectory);
-        arviz.commit(layers);
+        arviz.commit(simox::alg::get_values(layers));
 
 
         visualization.visualize(result.trajectory);
@@ -55,7 +56,7 @@ namespace armarx::navigation::server
     ArvizIntrospector::onLocalPlannerResult(const local_planning::LocalPlannerResult& result)
     {
         drawLocalTrajectory(result.trajectory);
-        arviz.commit(layers);
+        arviz.commit(simox::alg::get_values(layers));
     }
 
     // void
@@ -84,7 +85,7 @@ namespace armarx::navigation::server
 
         visualization.setTarget(goal);
     }
-
+    
     void
     ArvizIntrospector::onGlobalShortestPath(const std::vector<core::Pose>& path)
     {
@@ -143,7 +144,7 @@ namespace armarx::navigation::server
                     .color(cmap.at(tp.velocity / maxVelocity)));
         }
 
-        layers.emplace_back(std::move(layer));
+        layers[layer.data_.name] = std::move(layer);
     }
 
     void
@@ -182,8 +183,8 @@ namespace armarx::navigation::server
                 viz::Sphere("velocity_" + std::to_string(i)).position(pos).radius(50).color(color));
         }
 
-        layers.emplace_back(std::move(layer));
-        layers.emplace_back(std::move(velLayer));
+        layers[layer.data_.name] = std::move(layer);
+        layers[velLayer.data_.name] = std::move(velLayer);
     }
 
     void
@@ -196,7 +197,7 @@ namespace armarx::navigation::server
                               core::Pose(robot->getGlobalPose()) * twist.linear)
                       .color(simox::Color::orange()));
 
-        layers.emplace_back(std::move(layer));
+        layers[layer.data_.name] = std::move(layer);
     }
 
     void
@@ -209,7 +210,7 @@ namespace armarx::navigation::server
                               core::Pose(robot->getGlobalPose()) * twist.linear)
                       .color(simox::Color::green()));
 
-        layers.emplace_back(std::move(layer));
+        layers[layer.data_.name] = std::move(layer);
     }
 
     ArvizIntrospector::ArvizIntrospector(ArvizIntrospector&& other) noexcept :
@@ -220,6 +221,22 @@ namespace armarx::navigation::server
     {
     }
 
+    void ArvizIntrospector::clear()
+    {
+        // clear all layers
+        for(auto& [name, layer]: layers)
+        {
+            layer.markForDeletion();
+        }
+        arviz.commit(simox::alg::get_values(layers));
+        layers.clear();
+
+        // some special internal layers of TEB
+        arviz.commitDeleteLayer("local_planner_obstacles");
+        arviz.commitDeleteLayer("local_planner_velocity");
+        arviz.commitDeleteLayer("local_planner_path_alternatives");
+    }
+
     ArvizIntrospector&
     ArvizIntrospector::operator=(ArvizIntrospector&&) noexcept
     {
@@ -262,11 +279,15 @@ namespace armarx::navigation::server
     void
     ArvizIntrospector::success()
     {
+        clear();
+        
         visualization.success();
     }
     void
     ArvizIntrospector::failure()
     {
+        clear();
+
         visualization.failed();
     }
 

source/armarx/navigation/server/introspection/ArvizIntrospector.h

@@ -69,6 +69,8 @@ namespace armarx::navigation::server
         ArvizIntrospector(ArvizIntrospector&& other) noexcept;
         ArvizIntrospector& operator=(ArvizIntrospector&&) noexcept;
 
+        void clear();
+
     private:
         void drawGlobalTrajectory(const core::GlobalTrajectory& trajectory);
         void drawLocalTrajectory(const core::LocalTrajectory& trajectory);
@@ -78,7 +80,7 @@ namespace armarx::navigation::server
         viz::ScopedClient arviz;
         const VirtualRobot::RobotPtr robot;
 
-        std::vector<viz::Layer> layers;
+        std::map<std::string, viz::Layer> layers;
 
         util::Visualization visualization;
     };

source/armarx/navigation/server/monitoring/GoalReachedMonitor.h

@@ -34,7 +34,7 @@ namespace armarx::navigation::server
     struct GoalReachedMonitorConfig
     {
         float posTh{70.F};                                  // [mm]
-        float oriTh{VirtualRobot::MathTools::deg2rad(5.F)}; // [rad]
+        float oriTh{VirtualRobot::MathTools::deg2rad(3.F)}; // [rad]
 
         float linearVelTh{100.F};                                  // [mm/s]
         float angularVelTh{VirtualRobot::MathTools::deg2rad(5.F)}; // [rad/s]

source/armarx/navigation/simulation/CMakeLists.txt

+armarx_add_library(simulation
+    SOURCES
+        simulation.cpp
+        SimulatedHuman.cpp
+    HEADERS
+        simulation.h
+        SimulatedHuman.h
+    DEPENDENCIES_PUBLIC
+        ArmarXCoreInterfaces
+        ArmarXCore
+        # armarx_navigation
+        armarx_navigation::core
+        armarx_navigation::conversions
+        armarx_navigation::global_planning
+)
+
+ 
+armarx_add_test(simulationTest 
+    TEST_FILES
+        test/simulationTest.cpp
+    DEPENDENCIES
+        navigation::simulation
+)

source/armarx/navigation/simulation/SimulatedHuman.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/>.
+ *
+ * @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 "SimulatedHuman.h"
+
+#include <VirtualRobot/Random.h>
+
+#include "ArmarXCore/core/logging/Logging.h"
+
+#include "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/algorithms/util.h"
+#include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/core/StaticScene.h"
+#include "armarx/navigation/core/types.h"
+#include "armarx/navigation/global_planning/SPFA.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human::simulation
+{
+
+
+    human::Human
+    SimulatedHuman::update()
+    {
+        switch (state_)
+        {
+            case State::Idle:
+                ARMARX_DEBUG << "State:Idle";
+                initialize();
+                state_ = State::Walking;
+                break;
+            case State::Walking:
+            {
+                ARMARX_DEBUG << "State:Walking";
+
+                step();
+
+                const Eigen::Vector2f goal =
+                    conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
+
+                if ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold)
+                {
+                    ARMARX_INFO << "Human reached goal";
+                    state_ = State::GoalReached;
+                }
+
+                break;
+            }
+            case State::GoalReached:
+                ARMARX_DEBUG << "State:GoalReached";
+
+                // TODO(fabian.reister): simulate "perform action at goal": wait a while until movement is started again.
+                state_ = State::Idle;
+                break;
+        }
+
+        return human_;
+    }
+
+    void
+    SimulatedHuman::initialize()
+    {
+        global_planning::SPFA::Params params;
+        params.linearVelocity = this->params_.maxLinearVelocity;
+
+        core::Scene scene;
+
+        scene.staticScene.emplace(core::StaticScene{nullptr, std::nullopt});
+        scene.staticScene->distanceToObstaclesCostmap.emplace(distanceField_);
+
+        global_planning::SPFA planner(params, scene);
+
+        const auto start = human_.pose;
+
+        while (true)
+        {
+            const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+            ARMARX_CHECK(sampledPose.has_value());
+            const auto& goal = sampledPose.value();
+
+            const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
+
+            // check if plan could be created. otherwise try another goal
+            if (plan.has_value())
+            {
+                globalTrajectory_ = plan->trajectory;
+
+                human_ = human::Human{.pose = start,
+                                      .linearVelocity = Eigen::Vector2f::Zero(),
+                                      .detectionTime = Clock::Now()};
+                return;
+            }
+        }
+    }
+
+    void
+    SimulatedHuman::step()
+    {
+        const auto updateTime = Clock::Now();
+
+        const auto duration = updateTime - human_.detectionTime;
+
+        // move according to old state
+        human_.pose.translation() += duration.toSecondsDouble() * human_.linearVelocity;
+
+        const auto projection =
+            globalTrajectory_.getProjection(conv::to3D(human_.pose.translation()),
+                                            core::VelocityInterpolation::LinearInterpolation);
+
+        human_.pose = conv::to2D(projection.projection.waypoint.pose);
+
+        const auto wpBefore = projection.wayPointBefore.waypoint.pose.translation();
+        const auto wpAfter = projection.wayPointAfter.waypoint.pose.translation();
+
+        human_.linearVelocity =
+            (wpAfter - wpBefore).head<2>().normalized() * projection.projection.velocity;
+
+        human_.linearVelocity =
+            human_.linearVelocity.normalized() * std::clamp(human_.linearVelocity.norm(),
+                                                            params_.minLinearVelocity,
+                                                            params_.maxLinearVelocity);
+
+        human_.detectionTime = updateTime;
+    }
+
+
+    SimulatedHuman::SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params) :
+        distanceField_(distanceField), params_(params)
+    {
+        const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+        ARMARX_CHECK(sampledPose.has_value());
+        human_.pose = sampledPose.value();
+    }
+} // namespace armarx::navigation::human::simulation

source/armarx/navigation/simulation/SimulatedHuman.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     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 "armarx/navigation/algorithms/Costmap.h"
+#include "armarx/navigation/core/Trajectory.h"
+#include "armarx/navigation/core/basic_types.h"
+#include "armarx/navigation/human/types.h"
+
+namespace armarx::navigation::human::simulation
+{
+
+    struct SimulatedHumanParams
+    {
+        float goalDistanceThreshold = 100;
+
+        float minLinearVelocity = 100;
+        float maxLinearVelocity = 200;
+    };
+
+    class SimulatedHuman
+    {
+    public:
+        using Params = SimulatedHumanParams;
+
+        SimulatedHuman(const algorithms::Costmap& distanceField, const Params& params = Params());
+
+        Human update();
+
+    protected:
+        void initialize();
+
+        void step();
+
+        enum class State
+        {
+            Idle,
+            Walking,
+            GoalReached
+        };
+
+    private:
+        const algorithms::Costmap distanceField_;
+
+        State state_ = State::Idle;
+
+        human::Human human_;
+
+        core::GlobalTrajectory globalTrajectory_;
+
+        const Params params_;
+    };
+
+} // namespace armarx::navigation::human::simulation

source/armarx/navigation/simulation/simulation.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::simulation
+ * @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 "simulation.h"
+
+namespace armarx
+{
+
+}

source/armarx/navigation/simulation/simulation.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::simulation
+ * @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
+
+
+namespace armarx
+{
+    /**
+    * @defgroup Library-simulation simulation
+    * @ingroup navigation
+    * A description of the library simulation.
+    *
+    * @class simulation
+    * @ingroup Library-simulation
+    * @brief Brief description of class simulation.
+    *
+    * Detailed description of class simulation.
+    */
+    class simulation
+    {
+    public:
+
+    };
+
+}

source/armarx/navigation/simulation/test/simulationTest.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::simulation
+ * @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
+ */
+
+#define BOOST_TEST_MODULE navigation::ArmarXLibraries::simulation
+
+#define ARMARX_BOOST_TEST
+
+#include <navigation/Test.h>
+#include "../simulation.h"
+
+#include <iostream>
+
+BOOST_AUTO_TEST_CASE(testExample)
+{
+
+    BOOST_CHECK_EQUAL(true, true);
+}