Add laser scanner features to teb planning

This contains the move of the LaserScannerFeatureExtraction into navigation (!72 (merged)).

Implement merging of the features from individual sensors into one list while removing duplicates. Add the features to the dynamic scene and use them as teb obstacles.

source/armarx/navigation/components/laser_scanner_feature_extraction/ArVizDrawer.cpp

@@ -34,7 +34,7 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
                       const std::string& frame,
                       const Eigen::Isometry3f& globalSensorPose)
     {
-        // drawCircles(features, frame, globalSensorPose);
+        drawCircles(features, frame, globalSensorPose);
         drawConvexHulls(features, frame, globalSensorPose);
         drawEllipsoids(features, frame, globalSensorPose);
         drawChains(features, frame, globalSensorPose);
@@ -76,19 +76,18 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
     {
 
         const Eigen::Vector3f position =
-            globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), -1.F);
+            globalSensorPose * Eigen::Vector3f(circle.center.x(), circle.center.y(), 10.F);
 
         layer.add(viz::Ellipsoid("circle_" + std::to_string(layer.size()))
                       .axisLengths(Eigen::Vector3f{circle.radius, circle.radius, 0.F})
                       .position(position)
-                      .color(simox::Color::red(200, 100)));
+                      .color(color));
     }
 
     void
     ArVizDrawer::drawCircles(const std::vector<Features>& features,
                              const std::string& frame,
-                             const Eigen::Isometry3f& globalSensorPose,
-                             const simox::Color& color)
+                             const Eigen::Isometry3f& globalSensorPose)
     {
         auto layer = arviz.layer("circles_" + frame);
 
@@ -100,7 +99,8 @@ namespace armarx::navigation::components::laser_scanner_feature_extraction
                           {
                               return;
                           }
-                          drawCircle(layer, *f.circle, globalSensorPose, color);
+                          drawCircle(
+                              layer, *f.circle, globalSensorPose, simox::Color::red(200, 100));
                       });
 
         arviz.commit(layer);