navigation memory: new core segments + visu

source/armarx/navigation/components/NavigationMemory/CMakeLists.txt

@@ -18,6 +18,7 @@ armarx_add_component(navigation_memory
         armarx_navigation::graph
         armarx_navigation::location
         armarx_navigation::algorithms
+        armarx_navigation::teb_human
 
     SOURCES
         NavigationMemory.cpp

source/armarx/navigation/components/NavigationMemory/NavigationMemory.cpp

@@ -48,6 +48,7 @@
 #include <armarx/navigation/core/aron/Location.aron.generated.h>
 #include <armarx/navigation/core/aron/Trajectory.aron.generated.h>
 #include <armarx/navigation/core/aron/Twist.aron.generated.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
 #include <armarx/navigation/graph/constants.h>
 #include <armarx/navigation/location/constants.h>
 
@@ -127,6 +128,11 @@ namespace armarx::navigation
         workingMemory().addCoreSegment(navigation::graph::coreSegmentID.coreSegmentName,
                                        navigation::core::arondto::Graph::ToAronType());
 
+        workingMemory().addCoreSegment(memory::constants::HumanCoreSegmentName,
+                                       navigation::human::arondto::Human::ToAronType());
+
+        // workingMemory().addCoreSegment(memory::constants::HumanGroupCoreSegmentName,
+        //                                navigation::human::arondto::Human::ToAronType());
 
         if (not properties.snapshotToLoad.empty())
         {
@@ -432,7 +438,8 @@ namespace armarx::navigation
         memory::Visu visu{arviz,
                           workingMemory().getCoreSegment(navigation::location::coreSegmentID),
                           workingMemory().getCoreSegment(navigation::graph::coreSegmentID),
-                          workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName)};
+                          workingMemory().getCoreSegment(memory::constants::CostmapCoreSegmentName),
+                          workingMemory().getCoreSegment(memory::constants::HumanCoreSegmentName)};
 
 
         Properties::LocationGraph p;
@@ -462,6 +469,9 @@ namespace armarx::navigation
             // Costmaps
             visu.drawCostmaps(layers, p.visuCostmaps);
 
+            // Humans
+            visu.drawHumans(layers, p.visuHumans);
+
             arviz.commit(layers);
 
             metronome.waitForNextTick();

source/armarx/navigation/components/NavigationMemory/NavigationMemory.h

@@ -97,6 +97,7 @@ namespace armarx::navigation
                 bool visuLocations = true;
                 bool visuGraphEdges = true;
                 bool visuCostmaps = true;
+                bool visuHumans = true;
                 
                 float visuFrequency = 2;
             };

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

@@ -22,18 +22,24 @@
 
 #include "Visu.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/libraries/armem/server/wm/memory_definitions.h>
 
 #include "armarx/navigation/conversions/eigen.h"
+#include "armarx/navigation/human/aron/Human.aron.generated.h"
+#include "armarx/navigation/human/aron_conversions.h"
+#include "armarx/navigation/human/types.h"
 #include <armarx/navigation/algorithms/aron/Costmap.aron.generated.h>
 #include <armarx/navigation/algorithms/aron_conversions.h>
 #include <armarx/navigation/core/Graph.h>
 #include <armarx/navigation/core/aron/Graph.aron.generated.h>
 #include <armarx/navigation/core/aron/Location.aron.generated.h>
 #include <armarx/navigation/graph/Visu.h>
+#include <armarx/navigation/human/aron/Human.aron.generated.h>
 
 
 namespace armarx::navigation::memory
@@ -42,11 +48,13 @@ namespace armarx::navigation::memory
     Visu::Visu(viz::Client arviz,
                const armem::server::wm::CoreSegment& locSegment,
                const armem::server::wm::CoreSegment& graphSegment,
-               const armem::server::wm::CoreSegment& costmapSegment) :
+               const armem::server::wm::CoreSegment& costmapSegment,
+               const armem::server::wm::CoreSegment& humanSegment) :
         arviz(arviz),
         locSegment(locSegment),
         graphSegment(graphSegment),
         costmapSegment(costmapSegment),
+        humanSegment(humanSegment),
         visu(std::make_unique<graph::GraphVisu>())
     {
     }
@@ -165,6 +173,24 @@ namespace armarx::navigation::memory
             layer.add(mesh);
         }
 
+        void
+        visualize(const human::Humans& humans, viz::Layer& layer)
+        {
+            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});
+
+
+                cylinder.color(simox::Color::orange());
+                cylinder.radius(300);
+
+                layer.add(cylinder);
+            }
+        }
+
     } // namespace
 
     void
@@ -207,5 +233,45 @@ namespace armarx::navigation::memory
         }
     }
 
+    void
+    Visu::drawHumans(std::vector<viz::Layer>& layers, bool enabled)
+    {
+        if (not enabled)
+        {
+            return;
+        }
+
+        std::map<std::string, navigation::human::Humans> namedProviderHumans;
+
+        humanSegment.doLocked(
+            [&]()
+            {
+                using namespace armem::server;
+
+                humanSegment.forEachEntity(
+                    [&](const wm::Entity& entity)
+                    {
+                        entity.getLatestSnapshot().forEachInstance(
+                            [&namedProviderHumans](const armarx::armem::wm::EntityInstance& instance)
+                            {
+                                const auto dto =
+                                    navigation::human::arondto::Human::FromAron(instance.data());
+
+                                navigation::human::Human human;
+                                fromAron(dto, human);
+
+                                namedProviderHumans[instance.id().providerSegmentName]
+                                    .emplace_back(std::move(human));
+                            });
+                    });
+            });
+
+        for (const auto& [providerName, humans] : namedProviderHumans)
+        {
+            viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+            visualize(humans, layer);
+        }
+    }
+
 
 } // namespace armarx::navigation::memory

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

@@ -28,6 +28,7 @@
 #include "RobotAPI/libraries/armem/server/wm/memory_definitions.h"
 #include <RobotAPI/components/ArViz/Client/Client.h>
 #include <RobotAPI/libraries/armem/core/forward_declarations.h>
+
 #include "armarx/navigation/algorithms/Costmap.h"
 
 
@@ -45,13 +46,15 @@ namespace armarx::navigation::memory
         Visu(viz::Client arviz,
              const armem::server::wm::CoreSegment& locSegment,
              const armem::server::wm::CoreSegment& graphSegment,
-             const armem::server::wm::CoreSegment& costmapSegment);
+             const armem::server::wm::CoreSegment& costmapSegment,
+             const armem::server::wm::CoreSegment& humanSegment);
         ~Visu();
 
 
         void drawLocations(std::vector<viz::Layer>& layers, bool enabled);
         void drawGraphs(std::vector<viz::Layer>& layers, bool enabled);
         void drawCostmaps(std::vector<viz::Layer>& layers, bool enabled);
+        void drawHumans(std::vector<viz::Layer>& layers, bool enabled);
 
 
     public:
@@ -60,6 +63,7 @@ namespace armarx::navigation::memory
         const armem::server::wm::CoreSegment& locSegment;
         const armem::server::wm::CoreSegment& graphSegment;
         const armem::server::wm::CoreSegment& costmapSegment;
+        const armem::server::wm::CoreSegment& humanSegment;
 
         std::unique_ptr<navigation::graph::GraphVisu> visu;
     };