Fix laser scanner features memory reader/writer, store features

Fix usage of laser scanner features reader/writer to the new ones moved
into the navigation memory.
Store unused features in another provider segment in the memory.

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

@@ -34,20 +34,18 @@
 
 #include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 
-#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
-
 #include <armarx/navigation/components/dynamic_scene_provider/ArVizDrawer.h>
 #include <armarx/navigation/core/basic_types.h>
 #include <armarx/navigation/memory/client/costmap/Reader.h>
 #include <armarx/navigation/util/util.h>
 
+#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
 
     const std::string Component::defaultName = "dynamic_scene_provider";
 
-
     Component::Component()
     {
         addPlugin(humanPoseReaderPlugin);
@@ -56,6 +54,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         addPlugin(costmapReaderPlugin);
         addPlugin(occupancyGridReaderPlugin);
         addPlugin(humanWriterPlugin);
+        addPlugin(laserScannerFeaturesWriterPlugin);
     }
 
     armarx::PropertyDefinitionsPtr
@@ -100,7 +99,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
         return def;
     }
 
-
     void
     Component::onInitComponent()
     {
@@ -111,7 +109,6 @@ namespace armarx::navigation::components::dynamic_scene_provider
         setDebugObserverBatchModeEnabled(true);
     }
 
-
     void
     Component::onConnectComponent()
     {
@@ -163,27 +160,23 @@ namespace armarx::navigation::components::dynamic_scene_provider
         task->start();
     }
 
-
     void
     Component::onDisconnectComponent()
     {
         task->stop();
     }
 
-
     void
     Component::onExitComponent()
     {
     }
 
-
     std::string
     Component::getDefaultName() const
     {
         return Component::defaultName;
     }
 
-
     std::string
     Component::GetDefaultName()
     {
@@ -246,22 +239,20 @@ namespace armarx::navigation::components::dynamic_scene_provider
         // Laser scanner features
         //
         ARMARX_TRACE;
-        const std::vector<armem::vision::LaserScannerFeatures> laserFeatures = [&]
+        const std::vector<memory::LaserScannerFeatures> laserFeatures = [&]
         {
             armarx::core::time::ScopedStopWatch sw(makeSWlog("dynamic_scene.read_laserscanner"));
 
             ARMARX_INFO << "Querying laser scanner features";
-            const armem::vision::laser_scanner_features::client::Reader::Query laserFeaturesQuery{
+            const memory::client::laser_scanner_features::Reader::Query laserFeaturesQuery{
                 .providerName = properties.laserScannerFeatures.providerName,
                 .name = properties.laserScannerFeatures.name,
                 .timestamp = timestamp};
 
-            const armem::vision::laser_scanner_features::client::Reader::Result
-                laserFeaturesResult =
-                    laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
-            ARMARX_CHECK_EQUAL(
-                laserFeaturesResult.status,
-                armem::vision::laser_scanner_features::client::Reader::Result::Success)
+            const memory::client::laser_scanner_features::Reader::Result laserFeaturesResult =
+                laserScannerFeaturesReaderPlugin->get().queryData(laserFeaturesQuery);
+            ARMARX_CHECK_EQUAL(laserFeaturesResult.status,
+                               memory::client::laser_scanner_features::Reader::Result::Success)
                 << laserFeaturesResult.errorMessage;
 
             ARMARX_VERBOSE << laserFeaturesResult.features.size() << " clusters/features";
@@ -396,7 +387,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
         }
 
         ARMARX_TRACE;
-        const std::vector<armem::vision::LaserScannerFeature> unusedFeatures = [&]
+        const std::vector<memory::LaserScannerFeature> unusedFeatures = [&]
         {
             armarx::core::time::ScopedStopWatch sw(
                 makeSWlog("dynamic_scene.human_tracker.laserscanner"));
@@ -404,7 +395,7 @@ namespace armarx::navigation::components::dynamic_scene_provider
             ARMARX_INFO << "Running human tracker with lasersensor measurements";
 
             //TODO why is result a vector of LSFs and not a vector of LSF?
-            std::vector<armem::vision::LaserScannerFeature> flattened;
+            std::vector<memory::LaserScannerFeature> flattened;
             for (auto const& fs : laserFeatures)
             {
                 flattened.insert(flattened.end(), fs.features.begin(), fs.features.end());
@@ -431,12 +422,20 @@ namespace armarx::navigation::components::dynamic_scene_provider
                 ARMARX_INFO << "Detected " << humans.size() << " humans";
                 humanWriterPlugin->get().store(humans, getName(), timestamp);
             }
+
+            if (not unusedFeatures.empty())
+            {
+                ARMARX_INFO << "Detected " << unusedFeatures.size()
+                            << " laser scanner features not associated with humans";
+                //TODO(groeger): check frame, do we need it?
+                laserScannerFeaturesWriterPlugin->get().store(
+                    memory::LaserScannerFeatures{.features = unusedFeatures}, getName(), timestamp);
+            }
         }
 
         sendDebugObserverBatch();
     }
 
-
     /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
     void
     Component::createRemoteGuiTab()

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

@@ -51,6 +51,7 @@
 #include <armarx/navigation/human/HumanTracker.h>
 #include <armarx/navigation/memory/client/costmap/Reader.h>
 #include <armarx/navigation/memory/client/human/Writer.h>
+#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
 
 namespace armarx::navigation::components::dynamic_scene_provider
 {
@@ -201,6 +202,8 @@ namespace armarx::navigation::components::dynamic_scene_provider
             occupancyGridReaderPlugin = nullptr;
 
         ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
+        ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
+            laserScannerFeaturesWriterPlugin = nullptr;
 
 
         human::HumanTracker humanTracker;