Fix proxemic zone visualization, draw them in different colors

Use the proper pose to draw the proxemic zone ellipse.
Use different colors for the intimate and personal zones.

source/armarx/navigation/local_planning/TebObstacleManager.cpp

@@ -58,6 +58,7 @@ namespace armarx::navigation::local_planning
     {
         auto proxemicZones = proxemics.createProxemicZones(human);
 
+        int i = 0;
         for (const auto& proxemicZone : proxemicZones)
         {
             auto pose = conv::toRos(proxemicZone.pose);
@@ -85,13 +86,14 @@ namespace armarx::navigation::local_planning
             if (visLayer != nullptr)
             {
                 const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
-                const core::Pose pose3d = conv::to3D(human.pose);
+                const core::Pose pose3d = conv::to3D(proxemicZone.pose);
 
                 visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
                                   .pose(pose3d)
                                   .axisLengths(axisLength)
-                                  .color(simox::Color::blue()));
+                                  .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
             }
+            i++;
         }
     }
 

source/armarx/navigation/local_planning/TebObstacleManager.h

@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
         teb_local_planner::ObstContainer& container;
         human::ProxemicZoneCreator proxemics;
         int visualizationIndex;
+
+        const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+                                                                 simox::Color::blue()};
     };