Draw proxemic zones to own layer & clean up

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

@@ -187,16 +187,6 @@ namespace armarx::navigation::memory
         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);
-
-
             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);
@@ -220,7 +210,14 @@ namespace armarx::navigation::memory
                 arrow.width(10);
                 layer.add(arrow);
             }
+        }
+    }
 
+    void
+    Visu::visualizeProxemicZones(const human::Humans& humans, viz::Layer& layer)
+    {
+        for (const auto& human : humans)
+        {
             auto proxemicZones = human.proxemicZones;
 
             int i = 0;
@@ -230,10 +227,13 @@ namespace armarx::navigation::memory
                     proxemicZone.shape.a, proxemicZone.shape.b, 10.f - i);
                 const core::Pose pose3d = conv::to3D(proxemicZone.pose);
 
-                layer.add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex))
-                              .pose(pose3d)
-                              .axisLengths(axisLength)
-                              .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
+                viz::Ellipsoid zoneEllipse("proxemicZone_" + std::to_string(visualizationIndex));
+                zoneEllipse.pose(pose3d);
+                zoneEllipse.axisLengths(axisLength);
+                zoneEllipse.color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]);
+
+                layer.add(zoneEllipse);
+
                 i++;
             }
         }
@@ -325,11 +325,13 @@ namespace armarx::navigation::memory
                             [&namedProviderHumans](
                                 const armarx::armem::wm::EntityInstance& instance)
                             {
+                                ARMARX_TRACE;
                                 const auto dto =
                                     navigation::human::arondto::Human::FromAron(instance.data());
-
+                                ARMARX_TRACE;
                                 navigation::human::Human human;
                                 fromAron(dto, human);
+                                ARMARX_TRACE;
 
                                 namedProviderHumans[instance.id().providerSegmentName].emplace_back(
                                     std::move(human));
@@ -340,7 +342,10 @@ namespace armarx::navigation::memory
         for (const auto& [providerName, humans] : namedProviderHumans)
         {
             viz::Layer& layer = layers.emplace_back(arviz.layer("humans_" + providerName));
+            viz::Layer& proxZoneLayer =
+                layers.emplace_back(arviz.layer("proxemic_zones_" + providerName));
             visualize(humans, layer, visuTransparent);
+            visualizeProxemicZones(humans, proxZoneLayer);
         }
     }
 

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

@@ -78,6 +78,7 @@ namespace armarx::navigation::memory
         void visualize(const navigation::human::Humans& humans,
                        viz::Layer& layer,
                        const bool visuTransparent);
+        void visualizeProxemicZones(const navigation::human::Humans& humans, viz::Layer& layer);
         void visualize(const human::HumanGroups& humanGroups, viz::Layer& layer);
         void
         visualize(const algorithms::Costmap& costmap, viz::Layer& layer, const std::string& name);