dynamic_distance_to_obstacle_costmap_provider: draft

source/armarx/navigation/components/dynamic_distance_to_obstacle_costmap_provider/Component.cpp

@@ -23,8 +23,12 @@
 
 #include "Component.h"
 
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
+#include "ArmarXCore/core/time/Clock.h"
 #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
+#include "armarx/navigation/memory/client/costmap/Reader.h"
+
 // Include headers you only need in function definitions in the .cpp.
 
 // #include <Eigen/Core>
@@ -58,9 +62,9 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
         // def->required(properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
 
         // Add an optionalproperty.
-        def->optional(
-            properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
-        def->optional(properties.numBoxes, "p.box.Number", "Number of boxes to draw in ArViz.");
+        // def->optional(
+        //     properties.boxLayerName, "p.box.LayerName", "Name of the box layer in ArViz.");
+        def->optional(properties.updatePeriodMs, "p.updatePeriodMs", "");
 
         return def;
     }
@@ -110,6 +114,30 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
             RemoteGui_startRunningTask();
         }
         */
+
+        ARMARX_CHECK(readStaticCostmap());
+
+        updateCostmapTask =
+            new PeriodicTask<Component>(this, &Component::updateCostmap, properties.updatePeriodMs);
+    }
+
+    bool
+    Component::readStaticCostmap()
+    {
+        const memory::client::costmap::Reader::Query query{
+            .providerName = properties.staticCostmapProviderName,
+            .name = properties.staticCostmapName,
+            .timestamp = armarx::core::time::Clock::Now()};
+        const auto result = costmapReader.query(query);
+
+        if (result.costmap.has_value())
+        {
+            staticCostmap.emplace(result.costmap.value());
+            return true;
+        }
+
+        ARMARX_WARNING << "Could not read static costmap";
+        return false;
     }
 
 
@@ -139,6 +167,17 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
     }
 
 
+    void
+    Component::updateCostmap()
+    {
+
+        ARMARX_CHECK(staticCostmap.has_value());
+
+        
+
+    }
+
+
     /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
     void
     Component::createRemoteGuiTab()

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

@@ -26,6 +26,7 @@
 
 // #include <mutex>
 
+#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
 #include <ArmarXCore/core/Component.h>
 
 #include "RobotAPI/libraries/armem/client/forward_declarations.h"
@@ -82,6 +83,11 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
         void onExitComponent() override;
 
 
+        void updateCostmap();
+
+        bool readStaticCostmap();
+
+
         /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
         /// This function should be called once in onConnect() or when you
         /// need to re-create the Remote GUI tab.
@@ -116,8 +122,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
         /// Properties shown in the Scenario GUI.
         struct Properties
         {
-            std::string boxLayerName = "boxes";
-            int numBoxes = 10;
+            // std::string boxLayerName = "boxes";
+            std::string staticCostmapProviderName = "navigator";
+            std::string staticCostmapName = "distance_to_obstacles";
+            int updatePeriodMs = 100;
         };
         Properties properties;
         /* Use a mutex if you access variables from different threads
@@ -147,6 +155,10 @@ namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_p
 
         memory::client::costmap::Reader costmapReader;
         memory::client::costmap::Writer costmapWriter;
+
+        std::optional<algorithms::Costmap> staticCostmap;
+
+        PeriodicTask<Component>::pointer_type updateCostmapTask;
     };
 
 } // namespace armarx::navigation::components::dynamic_distance_to_obstacle_costmap_provider