Fix visualization, fix human obstacles

Make visualization of LocalPlanner a ScopedClient so the layers are removed automatically when the navigator is stopped.

Fix proxemic zone generation and visualization of proxemic zones.

source/armarx/navigation/local_planning/TebObstacleManager.cpp

@@ -29,7 +29,7 @@ namespace armarx::navigation::local_planning
         auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
 
         const Eigen::Vector2d min = conv::toRos(bbox.getMin());
-        const Eigen::Vector2d max = conv::toRos(bbox.getMin());
+        const Eigen::Vector2d max = conv::toRos(bbox.getMax());
 
         obst->pushBackVertex(min);
         obst->pushBackVertex(min.x(), max.y());
@@ -39,6 +39,7 @@ namespace armarx::navigation::local_planning
         obst->finalizePolygon();
         container.push_back(obst);
 
+        // visualize bounding box if layer is available
         if (visLayer != nullptr)
         {
             const Eigen::Vector3f min3d = conv::fromRos(min);
@@ -58,6 +59,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);
@@ -82,16 +84,18 @@ namespace armarx::navigation::local_planning
 
             container.push_back(obst);
 
+            // visualize proxemic zone if layer is available
             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++;
         }
     }