Visualize path alternatives inside TimedElasticBands

source/armarx/navigation/local_planning/TimedElasticBands.cpp

 #include "TimedElasticBands.h"
 
+#include <SimoxUtility/algorithm/apply.hpp>
 #include <SimoxUtility/json/json.hpp>
 #include <VirtualRobot/MathTools.h>
 #include <VirtualRobot/Robot.h>
@@ -128,6 +129,32 @@ namespace armarx::navigation::local_planning
 
         core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
 
+        // visualize path alternatives
+        if (arviz != nullptr)
+        {
+            auto layer = arviz->layer("local_planner_path_alternatives");
+
+            int i = 0;
+            int bestTebIdx = hcp_->bestTebIdx();
+            for (const auto& teb : hcp_->getTrajectoryContainer())
+            {
+                if (i == bestTebIdx)
+                {
+                    continue;
+                }
+                const core::LocalTrajectory trajectory = conv::fromRos(teb->teb());
+                const std::vector<Eigen::Vector3f> points =
+                    simox::alg::apply(trajectory.points(),
+                                      [](const core::LocalTrajectoryPoint& pt) -> Eigen::Vector3f
+                                      { return pt.pose.translation(); });
+                layer.add(viz::Path("path_alternative_" + std::to_string(i))
+                              .points(points)
+                              .color(simox::Color::gray()));
+                i++;
+            }
+            arviz->commit(layer);
+        }
+
         return {{.trajectory = best}};
     }
 
@@ -165,7 +192,7 @@ namespace armarx::navigation::local_planning
             ARMARX_CHECK(scene.dynamicScene.has_value());
             for (const auto& obst : scene.dynamicScene.value().humans)
             {
-              obstManager.addHumanObstacle(obst, visPtr);
+                obstManager.addHumanObstacle(obst, visPtr);
             }
         }