Component.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotComponents::ArmarXObjects::LaserScannerFeatureExtraction
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "Component.h"
 
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <iterator>
#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <variant>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <SimoxUtility/algorithm/apply.hpp>
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/util/Throttler.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
#include <ArmarXCore/libraries/logging/FrequencyReporter.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/armem/core/forward_declarations.h>
#include <RobotAPI/libraries/armem_laser_scans/client/common/Reader.h>
#include <RobotAPI/libraries/armem_laser_scans/types.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/geometry.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/laser_scanner_conversion.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/types.h>
 
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
#include "conversions/eigen.h"
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
 
    const std::string LaserScannerFeatureExtraction::defaultName = "LaserScannerFeatureExtraction";
 
    armarx::PropertyDefinitionsPtr
    LaserScannerFeatureExtraction::createPropertyDefinitions()
    {
        armarx::PropertyDefinitionsPtr def =
            new ComponentPropertyDefinitions(getConfigIdentifier());
 
        def->required(properties.robotName, "p.robotName");
 
        // // Add an optional property.
        def->optional(
            properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
 
        def->required(properties.robotName, "p.robotName", "Name of the robot");
 
        def->optional(properties.robotHull.shape, "p.robotHull.shape", "Shape of the robot area.")
            .map({std::make_pair("Rectangle", Properties::RobotHull::RECTANGLE),
                  std::make_pair("Circle", Properties::RobotHull::CIRCLE)});
        def->optional(properties.robotHull.radius,
                      "p.robotHull.radius",
                      "The radius of the robot when using the circle shape.");
        def->optional(
            properties.robotHull.robotConvexHullMargin,
            "p.robotHull.robotConvexHullMargin",
            "Parameter to increase the robot's convex hull when using the rectangle shape.");
 
        def->optional(properties.cableFix.enabled,
                      "p.cableFix.enabled",
                      "Try to supress clusters belonging to the power supply cable.");
        def->optional(properties.cableFix.cableAreaWidth,
                      "p.cableFix.cableAreaWidth",
                      "Width of the area where to search for the cable.");
 
        def->optional(
            properties.cableFix.maxAreaTh,
            "p.cableFix.maxAreaTh",
            "The cable will only be removed if the cluster area is below this threshold.");
 
        def->optional(
            properties.chainApproximationParams.distanceTh, "p.chainApproximation.distanceTh", "");
        def->optional(properties.chainApproximationParams.maxIterations,
                      "p.chainApproximation.maxIterations",
                      "");
 
        def->optional(properties.scanClusteringParams.angleThreshold,
                      "p.scanClustering.angleThreshold",
                      "Angular distance between consecutive points in laser scan for clustering.");
        def->optional(properties.scanClusteringParams.distanceThreshold,
                      "p.scanClustering.distanceThreshold",
                      "Radial distance between consecutive points in laser scan for clustering.");
        def->optional(properties.scanClusteringParams.maxDistance,
                      "p.scanClustering.maxDistance",
                      "Maximum radius around sensors to detect clusters.");
 
        def->optional(properties.arviz.drawRawPoints,
                      "p.arviz.drawRawPoints",
                      "If true, the laser scans will be drawn.");
 
        def->optional(properties.arviz.visualizeSeparateFeatures,
                      "p.arviz.visualizeSeparateFeatures",
                      "If true, the features from each sensor will be drawn.");
 
        def->optional(properties.arviz.visualizeMergedFeatures,
                      "p.arviz.visualizeMergedFeatures",
                      "If true, the merged features from all sensors will be drawn.");
 
        def->optional(properties.filtering.filterLaserScansWithCostmap,
                      "p.filtering.filterLaserScansWithCostmap",
                      "It true, the laser scans are filtered and anything too close to an obstacle "
                      "is removed.");
 
        def->optional(
            properties.filtering.distance,
            "p.filtering.distance",
            "The minimum distance of laser scanner features to obstacles so they are not removed.");
 
        def->optional(
            properties.filtering.costmapProviderName, "p.filtering.costmapProviderName", "");
 
        def->optional(properties.filtering.costmapName, "p.filtering.costmapName", "");
 
        def->optional(properties.arviz.arvizDrawer.drawCircles, "p.arviz.drawer.drawCircles", "");
        def->optional(
            properties.arviz.arvizDrawer.drawConvexHulls, "p.arviz.drawer.drawConvexHulls", "");
        def->optional(
            properties.arviz.arvizDrawer.drawEllipsoids, "p.arviz.drawer.drawEllipsoids", "");
        def->optional(properties.arviz.arvizDrawer.drawChains, "p.arviz.drawer.drawChains", "");
 
        return def;
    }
 
    LaserScannerFeatureExtraction::LaserScannerFeatureExtraction()
    {
        addPlugin(laserScannerReaderPlugin);
        addPlugin(laserScannerFeatureWriterPlugin);
        addPlugin(virtualRobotReaderPlugin);
        addPlugin(costmapReaderPlugin);
    }
 
    void
    LaserScannerFeatureExtraction::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    navigation::algorithms::Costmap
    LaserScannerFeatureExtraction::fetchDistanceToObstacleCostmapSmallMargin(
        navigation::memory::client::costmap::Reader& costmapReader)
    {
        while (true)
        {
            const auto timestamp = Clock::Now();
 
            const navigation::memory::client::costmap::Reader::Query costmapQuery{
                .providerName = properties.filtering.costmapProviderName,
                .name = properties.filtering.costmapName,
                .timestamp = timestamp};
 
            const navigation::memory::client::costmap::Reader::Result costmapResult =
                costmapReader.query(costmapQuery);
 
            // if costmap is not available yet, wait
            if (costmapResult.status !=
                navigation::memory::client::costmap::Reader::Result::Success)
            {
                Clock::WaitFor(Duration::MilliSeconds(100));
                continue;
            }
 
            ARMARX_CHECK_EQUAL(costmapResult.status,
                               navigation::memory::client::costmap::Reader::Result::Success);
 
            ARMARX_TRACE;
            ARMARX_CHECK(costmapResult.costmap.has_value());
            return costmapResult.costmap.value();
        };
    }
 
    void
    LaserScannerFeatureExtraction::onConnectComponent()
    {
        ARMARX_INFO << "Connecting";
 
        frequencyReporterPublish = std::make_unique<FrequencyReporter>(
            getDebugObserver(), "LaserScannerFeatureExtraction_publish");
        frequencyReporterRun = std::make_unique<FrequencyReporter>(
            getDebugObserver(), "LaserScannerFeatureExtraction_run");
 
        arVizDrawer = std::make_unique<ArVizDrawer>(getArvizClient(), properties.arviz.arvizDrawer);
 
        featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
                                                              properties.chainApproximationParams);
 
        ARMARX_INFO << "Connected";
 
        ARMARX_CHECK_NOT_NULL(virtualRobotReaderPlugin);
        robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName);
        ARMARX_CHECK_NOT_NULL(robot);
 
        switch (properties.robotHull.shape)
        {
            case Properties::RobotHull::RECTANGLE:
            {
                // initialize robot platform convex hull
                const Eigen::Vector2f ptFrontLeft = conversions::to2D(
                    robot->getRobotNode("PlatformCornerFrontLeft")->getPositionInRootFrame());
                const Eigen::Vector2f ptFrontRight = conversions::to2D(
                    robot->getRobotNode("PlatformCornerFrontRight")->getPositionInRootFrame());
                const Eigen::Vector2f ptBackRight = conversions::to2D(
                    robot->getRobotNode("PlatformCornerBackRight")->getPositionInRootFrame());
                const Eigen::Vector2f ptBackLeft = conversions::to2D(
                    robot->getRobotNode("PlatformCornerBackLeft")->getPositionInRootFrame());
 
                const auto addMargin = [&](const Eigen::Vector2f& pt) -> Eigen::Vector2f
                {
                    Eigen::Vector2f ptWithMargin = pt;
                    ptWithMargin.x() +=
                        std::copysignf(properties.robotHull.robotConvexHullMargin, pt.x());
                    ptWithMargin.y() +=
                        std::copysignf(properties.robotHull.robotConvexHullMargin, pt.y());
 
                    return ptWithMargin;
                };
 
                {
                    std::vector<Eigen::Vector2f> hullPoints;
                    hullPoints.push_back(addMargin(ptFrontLeft));
                    hullPoints.push_back(addMargin(ptFrontRight));
                    hullPoints.push_back(addMargin(ptBackRight));
                    hullPoints.push_back(addMargin(ptBackLeft));
 
                    robotHull = VirtualRobot::MathTools::createConvexHull2D(hullPoints);
                }
 
                // cable fix is only applicable when the robot shape is a rectangle
                if (properties.cableFix.enabled)
                {
                    // the cable area and the robot hull overlap slightly. as all points must be within one region,
                    // the cable area should start at approx. the position of the wheels
 
                    std::vector<Eigen::Vector2f> cableAreaPoints;
                    cableAreaPoints.emplace_back(addMargin(ptBackRight) +
                                                 100 * Eigen::Vector2f::UnitY());
                    cableAreaPoints.emplace_back(addMargin(ptBackLeft) +
                                                 100 * Eigen::Vector2f::UnitY());
                    cableAreaPoints.emplace_back(addMargin(ptBackRight) -
                                                 properties.cableFix.cableAreaWidth *
                                                     Eigen::Vector2f::UnitY());
                    cableAreaPoints.emplace_back(addMargin(ptBackLeft) -
                                                 properties.cableFix.cableAreaWidth *
                                                     Eigen::Vector2f::UnitY());
 
                    cableArea = VirtualRobot::MathTools::createConvexHull2D(cableAreaPoints);
                }
 
                break;
            }
            case Properties::RobotHull::CIRCLE:
            {
                // cable fix is not supported for circular robots
                if (properties.cableFix.enabled)
                {
                    ARMARX_ERROR << "Cable fix is not supported for circular robots!";
                }
                const Eigen::Vector2f root =
                    conversions::to2D(robot->getRootNode()->getPositionInRootFrame());
                robotHull = root;
                break;
            }
        }
 
        // initialize costmap
        obstacleDistancesCostmapSmallMargin.emplace(
            fetchDistanceToObstacleCostmapSmallMargin(costmapReaderPlugin->get()));
 
 
        task = new PeriodicTask<LaserScannerFeatureExtraction>(
            this,
            &LaserScannerFeatureExtraction::runPeriodically,
            properties.taskPeriodMs,
            false,
            "runningTask");
        task->start();
    }
 
    memory::LaserScannerFeature
    toArmemFeature(const Features& features)
    {
        memory::LaserScannerFeature armemFeature;
 
        if (features.chain)
        {
            armemFeature.chain = features.chain.value();
        }
 
        if (features.circle)
        {
            armemFeature.circle = features.circle.value();
        }
 
        if (features.convexHull)
        {
            armemFeature.convexHull = features.convexHull->vertices;
        }
 
        if (features.ellipsoid)
        {
            armemFeature.ellipsoid = features.ellipsoid.value();
        }
 
        armemFeature.points = features.points;
 
        return armemFeature;
    }
 
    namespace
    {
        memory::LaserScannerFeatures
        toArmemFeatures(const std::vector<Features>& features,
                        const Eigen::Isometry3f& global_T_sensor,
                        const std::string& sensorFrame)
        {
            memory::LaserScannerFeatures armemFeatures;
            armemFeatures.frame = sensorFrame;
            armemFeatures.frameGlobalPose = global_T_sensor;
 
 
            armemFeatures.features = simox::alg::apply(features, toArmemFeature);
 
            return armemFeatures;
        }
 
        std::vector<Features>
        removeInvalidFeatures(std::vector<Features> features)
        {
            const auto isInvalid = [](const Features& features) -> bool
            {
                if (features.points.size() < 2)
                {
                    return true;
                }
 
                if (not features.convexHull.has_value())
                {
                    return true;
                }
 
                // if(not features.circle.has_value())
                // {
                //     return true;
                // }
 
                // if(not features.ellipsoid.has_value())
                // {
                //     return true;
                // }
 
                return false;
            };
 
 
            features.erase(std::remove_if(features.begin(), features.end(), isInvalid),
                           features.end());
 
            return features;
        }
    } // namespace
 
    std::vector<Features>
    LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
                                              const std::vector<Features>& featuresFromExtractor)
    {
        ARMARX_DEBUG << "Publishing data";
 
        // obtain sensor pose
        const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
 
        ARMARX_CHECK(virtualRobotReader.synchronizeRobot(*robot, data.header.timestamp));
 
        const Eigen::Isometry3f global_T_sensor(
            robot->getRobotNode(data.header.frame)->getGlobalPose());
        const Eigen::Isometry3f robot_T_sensor(
            robot->getRobotNode(data.header.frame)->getPoseInRootFrame());
 
        //Eigen::AlignedBox2f box;
        //box.extend(pt1).extend(pt2).extend(pt3).extend(pt4);
 
        const auto transformPoints =
            [](const std::vector<Eigen::Vector2f>& points, const Eigen::Isometry3f& tf)
        {
            std::vector<Eigen::Vector2f> out;
            out.reserve(points.size());
 
            std::transform(points.begin(),
                           points.end(),
                           std::back_inserter(out),
                           [&tf](const auto& pt)
                           { return conversions::to2D(tf * conversions::to3D(pt)); });
 
            return out;
        };
 
        const auto isPointInsideRobot = [&](const Eigen::Vector2f& pt) -> bool
        {
            if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
            {
                return VirtualRobot::MathTools::isInside(
                    pt, std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull));
            }
            else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
            {
                return (std::get<Eigen::Vector2f>(robotHull) - pt).norm() <
                       properties.robotHull.radius;
            }
            return false;
        };
        const auto isPointInsideCableArea = [&](const Eigen::Vector2f& pt) -> bool
        { return VirtualRobot::MathTools::isInside(pt, cableArea); };
        const auto isClusterInvalid = [&](const Features& features) -> bool
        {
            // either use convex hull (compact representation) or raw points as fallbacck
            const auto points = [&]()
            {
                if (features.convexHull)
                {
                    return transformPoints(features.convexHull->vertices, robot_T_sensor);
                }
 
                return transformPoints(features.points, robot_T_sensor);
            }();
 
            const bool allPointsInsideRobot =
                std::all_of(points.begin(), points.end(), isPointInsideRobot);
            if (allPointsInsideRobot)
            {
                return true;
            }
 
            if (properties.cableFix.enabled)
            {
 
                const bool allPointsInCableArea =
                    std::all_of(points.begin(), points.end(), isPointInsideCableArea);
                if (allPointsInCableArea)
                {
                    if (geometry::ConvexHull(points).area() < properties.cableFix.maxAreaTh)
                    {
                        return true;
                    }
                }
            }
 
            return false;
        };
 
        const auto removeFeaturesOnRobotOrCable = [&](std::vector<Features> features)
        {
            features.erase(std::remove_if(features.begin(), features.end(), isClusterInvalid),
                           features.end());
            return features;
        };
 
        ARMARX_VERBOSE << featuresFromExtractor.size() << " features from extractor";
 
        const auto features = removeFeaturesOnRobotOrCable(featuresFromExtractor);
        ARMARX_VERBOSE << features.size() << " features without cable region";
 
        const auto validFeatures = removeInvalidFeatures(features);
        ARMARX_VERBOSE << validFeatures.size() << " valid features without cable region";
 
        const auto armemFeatures =
            toArmemFeatures(validFeatures, global_T_sensor, data.header.frame);
 
        ARMARX_VERBOSE << "Reporting " << armemFeatures.features.size() << " features";
 
        // report the features
        publishFeatures(armemFeatures, data.header.timestamp);
 
        if (properties.arviz.visualizeSeparateFeatures)
        {
            drawFeatures(
                features, &data, data.header.frame, data.header.timestamp, global_T_sensor);
        }
 
        return validFeatures;
    }
 
    Throttler&
    LaserScannerFeatureExtraction::getOrCreateThrottler(std::string frame)
    {
        const auto it = throttlers.find(frame);
        if (it == throttlers.end())
        {
            throttlers.emplace(frame, Throttler(10.F));
        }
 
        return throttlers.at(frame);
    }
 
    void
    LaserScannerFeatureExtraction::drawRobot(const armem::Time& timestamp)
    {
        // check if arviz should be triggered
        if (getOrCreateThrottler(robot->getName()).check(timestamp))
        {
            const Eigen::Isometry3f global_T_robot(robot->getRootNode()->getGlobalPose());
 
            if (std::holds_alternative<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull))
            {
                VirtualRobot::MathTools::ConvexHull2DPtr& convexHullPtr =
                    std::get<VirtualRobot::MathTools::ConvexHull2DPtr>(robotHull);
                if (convexHullPtr != nullptr)
                {
                    arVizDrawer->draw("robot_convex_hull",
                                      *convexHullPtr,
                                      global_T_robot,
                                      simox::Color::azure(255, 80));
                }
            }
            else if (std::holds_alternative<Eigen::Vector2f>(robotHull))
            {
                Eigen::Vector2f& root = std::get<Eigen::Vector2f>(robotHull);
                arVizDrawer->draw("robot_circle_hull",
                                  Circle{root, properties.robotHull.radius},
                                  global_T_robot,
                                  simox::Color::azure(255, 80));
            }
 
            if (cableArea != nullptr)
            {
                arVizDrawer->draw(
                    "cable_area", *cableArea, global_T_robot, simox::Color::blue(255, 80));
            }
        }
    }
 
    void
    LaserScannerFeatureExtraction::drawFeatures(
        const std::vector<Features>& features,
        const armem::laser_scans::LaserScanStamped* laserPoints,
        const std::string frame,
        const armem::Time& timestamp,
        const Eigen::Isometry3f& global_T_sensor)
    {
        // check if arviz should be triggered
        if (getOrCreateThrottler(frame).check(timestamp))
        {
            arVizDrawer->draw(features, frame, global_T_sensor);
 
            if (properties.arviz.drawRawPoints && laserPoints != nullptr)
            {
                arVizDrawer->draw(*laserPoints, global_T_sensor, simox::Color::magenta(255, 100));
            }
        }
    }
 
    void
    LaserScannerFeatureExtraction::publishFeatures(const memory::LaserScannerFeatures& features,
                                                   const armem::Time& timestamp)
    {
        // store in memory
        ARMARX_CHECK_NOT_NULL(laserScannerFeatureWriterPlugin);
        laserScannerFeatureWriterPlugin->get().store(features, getName(), timestamp);
 
        // // legacy - topic
        // LineSegment2DChainSeq chains;
        // for (const auto& feature : features.features)
        // {
        //     if (not feature.chain.empty())
        //     {
        //         LineSegment2DChain chain;
        //         for (const auto& pt : feature.chain)
        //         {
        //             chain.push_back(
        //                 conversions::to2D(features.frameGlobalPose * conversions::to3D(pt)));
        //         }
        //         chains.push_back(chain);
        //     }
        // }
 
        // featuresTopic->reportExtractedLineSegments(chains);
    }
 
    void
    LaserScannerFeatureExtraction::onDisconnectComponent()
    {
        task->stop();
    }
 
    void
    LaserScannerFeatureExtraction::onExitComponent()
    {
    }
 
    std::string
    LaserScannerFeatureExtraction::getDefaultName() const
    {
        return defaultName;
    }
 
    std::string
    LaserScannerFeatureExtraction::GetDefaultName()
    {
        return defaultName;
    }
 
    void
    LaserScannerFeatureExtraction::runPeriodically()
    {
        const armem::laser_scans::client::Reader::Query laserScanQuery{
            .agent = robot->getName(), .timeRange = std::nullopt, .sensorList = {}};
 
        armem::laser_scans::client::Reader::Result laserScanResult =
            laserScannerReaderPlugin->get().queryData(laserScanQuery);
 
        if (laserScanResult.status != armem::laser_scans::client::Reader::Result::Success)
        {
            ARMARX_INFO << deactivateSpam(10) << "Failed to read laser scanner data. Reason: "
                        << laserScanResult.errorMessage;
            return;
        }
 
        // "Happy" case: data is available
        ARMARX_CHECK_EQUAL(laserScanResult.status,
                           armem::laser_scans::client::Reader::Result::Success)
            << laserScanResult.errorMessage;
 
        ARMARX_DEBUG << "Received laser scan data from " << laserScanResult.laserScans.size()
                     << " sensors";
 
        frequencyReporterRun->add(IceUtil::Time::now().toMicroSeconds());
 
        std::vector<FramedFeatures> allFeatures;
        allFeatures.reserve(laserScanResult.laserScans.size());
        armem::Time mostRecentTimestamp{0};
 
        for (auto& scan : laserScanResult.laserScans)
        {
            mostRecentTimestamp = std::max(mostRecentTimestamp, scan.header.timestamp);
 
            // Remove scan steps that belong to the environment (see distance_to_obstacle_costmap_small_margin)
            if (properties.filtering.filterLaserScansWithCostmap)
            {
                if (not filterLaserScannerData(scan))
                {
                    ARMARX_INFO << "Failed to filter laser scanner data.";
                    return;
                }
            }
 
            // run clustering for every sensor
            setDebugObserverDatafield(getDefaultName() + scan.header.frame, scan.data.size());
            const auto features = featureExtractor->onData(scan);
 
            auto& framedFeatures = allFeatures.emplace_back();
 
            // onFeatures filters out some clusters (cable area, inside robot hull etc.), we only want to keep the filtered points
            framedFeatures.features = onFeatures(scan, features);
            framedFeatures.frame = scan.header.frame;
        }
 
        if (!allFeatures.empty())
        {
            mergeFeatures(allFeatures, mostRecentTimestamp);
        }
 
        // visualize robot with bounding box/cable area etc.
        drawRobot(mostRecentTimestamp);
 
        // some debugobserver reporting
        frequencyReporterPublish->add(IceUtil::Time::now().toMicroSeconds());
    }
 
    bool
    LaserScannerFeatureExtraction::filterLaserScannerData(
        armem::laser_scans::LaserScanStamped& laserScan)
    {
        // filter laser scanner data
        // Problem: Distance to obstacle costmap uses robot radius to invalidate points that are too close to an obstacle.
        // But we want to keep a larger distance
 
        const auto size_before_filtering = laserScan.data.size();
        // obtain sensor pose
        const auto& virtualRobotReader = virtualRobotReaderPlugin->get();
        if (not virtualRobotReader.synchronizeRobot(*robot, laserScan.header.timestamp))
        {
            ARMARX_INFO << "Failed to synchronize robot to timestamp `"
                        << laserScan.header.timestamp << "`.";
            return false;
        }
 
        const Eigen::Isometry3f global_T_sensor(
            robot->getRobotNode(laserScan.header.frame)->getGlobalPose());
        /*const Eigen::Isometry3f robot_T_sensor(
            robot->getRobotNode(scan.header.frame)->getPoseInRootFrame());*/
 
        const auto converted = toCartesian<Eigen::Vector2f>(laserScan.data);
        std::vector<Eigen::Vector2f> laserScanPositionsGlobal;
 
        std::transform(converted.begin(),
                       converted.end(),
                       std::back_inserter(laserScanPositionsGlobal),
                       [&global_T_sensor](const Eigen::Vector2f& pt)
                       { return conversions::to2D(global_T_sensor * conversions::to3D(pt)); });
 
        ARMARX_CHECK(obstacleDistancesCostmapSmallMargin.has_value());
        const auto& distance_to_obstacle_costmap = obstacleDistancesCostmapSmallMargin.value();
 
        // filter points that are too close to obstacles
        std::vector<armarx::LaserScanStep> filteredSteps;
        for (size_t i = 0; i < laserScanPositionsGlobal.size(); ++i)
        {
            const auto& pt = laserScanPositionsGlobal[i];
            const auto vertex = distance_to_obstacle_costmap.toVertex(pt);
            if (not distance_to_obstacle_costmap.isValid(vertex.index))
            {
                continue;
            }
            const auto& distance = distance_to_obstacle_costmap.value(vertex.index);
 
            if (distance.has_value() && *distance > properties.filtering.distance)
            {
                filteredSteps.push_back(laserScan.data[i]);
            }
        }
 
        laserScan.data = filteredSteps;
        ARMARX_VERBOSE << "Filtered points: " << size_before_filtering << " -> "
                       << laserScan.data.size();
 
        return true;
    }
 
    void
    LaserScannerFeatureExtraction::mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
                                                 const armem::Time& timestamp)
    {
        // convert features to global coordinate system
        std::vector<Features> converted;
        for (const auto& features : framedFeatures)
        {
            converted.reserve(converted.size() + features.features.size());
            Eigen::Isometry3f global_T_frame(robot->getRobotNode(features.frame)->getGlobalPose());
 
            for (const auto& singleFeature : features.features)
            {
                converted.push_back(conversions::transformFeature(singleFeature, global_T_frame));
            }
        }
 
        // merge overlapping features
        FeatureMerger merger{std::move(converted), properties.chainApproximationParams};
        std::vector<Features> merged = merger.merge();
 
        // save merged features in another entity with name 'global'
        std::string frame = "global";
        const Eigen::Isometry3f global_T_global = Eigen::Isometry3f::Identity();
 
        const auto armemFeatures = toArmemFeatures(merged, global_T_global, frame);
 
        laserScannerFeatureWriterPlugin->get().store(armemFeatures, getName(), timestamp);
 
        setDebugObserverDatafield("numMergedClusters", merged.size());
 
        if (properties.arviz.visualizeMergedFeatures)
        {
            // visualize the features above the floor
            auto global_T_vis = global_T_global;
            global_T_vis.translation() = Eigen::Vector3f{0, 0, 20};
            drawFeatures(merged, nullptr, frame, timestamp, global_T_vis);
        }
    }
 
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(LaserScannerFeatureExtraction,
                                         LaserScannerFeatureExtraction::GetDefaultName());
 
} // namespace armarx::navigation::components::laser_scanner_feature_extraction