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 <iterator>
#include <utility>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <IceUtil/Time.h>
 
#include <SimoxUtility/color/Color.h>
#include <VirtualRobot/BoundingBox.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include "RobotAPI/libraries/core/remoterobot/RemoteRobot.h"
 
#include <armarx/navigation/components/laser_scanner_feature_extraction/geometry.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/FeatureMerger.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/conversions/features.h>
 
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
#include "conversions/eigen.h"
#include "conversions/pcl_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());
 
        // Publish to a topic (passing the TopicListenerPrx).
        def->topic(featuresTopic);
 
        // // Add an optionalproperty.
        def->optional(
            properties.taskPeriodMs, "p.taskPeriodMs", "Update rate of the running task.");
 
        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.");
 
        return def;
    }
 
    LaserScannerFeatureExtraction::LaserScannerFeatureExtraction()
    {
        addPlugin(laserScannerReaderPlugin);
        addPlugin(laserScannerFeatureWriterPlugin);
    }
 
    void
    LaserScannerFeatureExtraction::onInitComponent()
    {
        // Topics and properties defined above are automagically registered.
 
        // Keep debug observer data until calling `sendDebugObserverBatch()`.
        // (Requies the armarx::DebugObserverComponentPluginUser.)
        // setDebugObserverBatchModeEnabled(true);
    }
 
    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());
 
        featureExtractor = std::make_unique<FeatureExtractor>(properties.scanClusteringParams,
                                                              properties.chainApproximationParams);
 
        ARMARX_INFO << "Connected";
 
        robot = RemoteRobot::createLocalClone(getRobotStateComponent());
        RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
        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;
            }
        }
 
 
        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;
    }
 
    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;
    }
 
    std::vector<Features>
    LaserScannerFeatureExtraction::onFeatures(const armem::laser_scans::LaserScanStamped& data,
                                              const std::vector<Features>& featuresFromExtractor)
    {
        ARMARX_DEBUG << "Publishing data";
 
        // obtain sensor pose
        RemoteRobot::synchronizeLocalCloneToTimestamp(
            robot, getRobotStateComponent(), data.header.timestamp.toMicroSecondsSinceEpoch());
        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
        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 = {}};
 
        const armem::laser_scans::client::Reader::Result laserScanResult =
            laserScannerReaderPlugin->get().queryData(laserScanQuery);
        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 (const auto& scan : laserScanResult.laserScans)
        {
            mostRecentTimestamp = std::max(mostRecentTimestamp, scan.header.timestamp);
 
            // 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());
    }
 
    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