Component.h

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
 */
 
#pragma once
 
#include <math.h>
 
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <variant>
#include <vector>
 
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/util/Throttler.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/libraries/logging/FrequencyReporter.h>
 
#include "RobotAPI/libraries/armem/client/forward_declarations.h"
#include "RobotAPI/libraries/armem/client/plugins/PluginUser.h"
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.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 <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
 
#include <armarx/navigation/algorithms/Costmap.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/ChainApproximation.h>
#include <armarx/navigation/components/laser_scanner_feature_extraction/ScanClustering.h>
#include <armarx/navigation/memory/client/costmap/Reader.h>
#include <armarx/navigation/memory/client/laser_scanner_features/Writer.h>
#include <armarx/navigation/memory/types.h>
 
#include "ArVizDrawer.h"
#include "FeatureExtractor.h"
 
namespace armarx::navigation::components::laser_scanner_feature_extraction
{
    using LineSegment2DChain = std::vector<Eigen::Vector2f>;
    using LineSegment2DChainSeq = std::vector<LineSegment2DChain>;
 
    // class FeatureExtractor;
    // class ArVizDrawer;
 
    /**
     * @defgroup Component-LaserScannerFeatureExtraction LaserScannerFeatureExtraction
     * @ingroup RobotComponents-Components
     * A description of the component LaserScannerFeatureExtraction.
     *
     * @class LaserScannerFeatureExtraction
     * @ingroup Component-LaserScannerFeatureExtraction
     * @brief Brief description of class LaserScannerFeatureExtraction.
     *
     * Detailed description of class LaserScannerFeatureExtraction.
     */
    class LaserScannerFeatureExtraction :
        virtual public armarx::Component,
        virtual public armarx::DebugObserverComponentPluginUser,
        virtual public armarx::ArVizComponentPluginUser,
        virtual public armem::ClientPluginUser
    {
    public:
        LaserScannerFeatureExtraction();
 
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
 
        /// Get the component's default name.
        static std::string GetDefaultName();
 
    protected:
        /// @see PropertyUser::createPropertyDefinitions()
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onExitComponent()
        void onExitComponent() override;
 
        /* (Requires armarx::LightweightRemoteGuiComponentPluginUser.)
        /// This function should be called once in onConnect() or when you
        /// need to re-create the Remote GUI tab.
        void createRemoteGuiTab();
 
        /// After calling `RemoteGui_startRunningTask`, this function is
        /// called periodically in a separate thread. If you update variables,
        /// make sure to synchronize access to them.
        void RemoteGui_update() override;
        */
 
    private:
        struct FramedFeatures
        {
            std::vector<Features> features;
            std::string frame;
        };
 
        void runPeriodically();
 
        bool filterLaserScannerData(armem::laser_scans::LaserScanStamped& laserScan);
 
        navigation::algorithms::Costmap fetchDistanceToObstacleCostmapSmallMargin(
            navigation::memory::client::costmap::Reader& costmapReader);
 
 
        std::vector<Features> onFeatures(const armem::laser_scans::LaserScanStamped& data,
                                         const std::vector<Features>& features);
 
        Throttler& getOrCreateThrottler(std::string frame);
        void drawRobot(const armem::Time& timestamp);
        void 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);
 
        void mergeFeatures(const std::vector<FramedFeatures>& framedFeatures,
                           const armem::Time& timestamp);
 
        void publishFeatures(const memory::LaserScannerFeatures& features,
                             const armem::Time& timestamp);
 
        // Private methods go here.
 
        // Forward declare `Properties` if you used it before its defined.
        // struct Properties;
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
        /// Draw some boxes in ArViz.
        void drawBoxes(const Properties& p, viz::Client& arviz);
        */
 
        // Private member variables go here.
        static const std::string defaultName;
 
        /// Properties shown in the Scenario GUI.
        struct Properties
        {
            int taskPeriodMs = 100;
 
            //
 
            std::string robotName;
 
            struct RobotHull
            {
                enum Shape
                {
                    RECTANGLE,
                    CIRCLE
                };
 
                Shape shape = RECTANGLE;
                // for shape circle
                float radius = 500.F; // [mm]
                // for shape rectangle
                float robotConvexHullMargin{50.F};
            } robotHull;
 
            //
            struct CableFix
            {
                bool enabled = {true};
                float cableAreaWidth{400};
                float maxAreaTh{50 * 50};
            } cableFix;
 
            ScanClustering::Params scanClusteringParams{
                .distanceThreshold = 30.F, // [mm]
                // we know the scan resolution which produces a bit more than 1000 points
                .angleThreshold = 2 * M_PIf32 / 1000.F * 5.F, // [rad]
                .maxDistance = 3'000.F // [mm]
            };
 
            ChainApproximation::Params chainApproximationParams{
                .distanceTh = 40.F // [mm]
            };
 
            struct ArViz
            {
                bool drawRawPoints = {true};
                bool visualizeSeparateFeatures = {false};
                bool visualizeMergedFeatures = {true};
 
                ArVizDrawer::Parameters arvizDrawer;
            } arviz;
 
            struct Filtering
            {
                bool filterLaserScansWithCostmap = false;
                float distance = 100.0;
 
                std::string costmapProviderName = "distance_to_obstacle_costmap_provider";
                std::string costmapName = "distance_to_obstacles_small_margin";
            } filtering;
        };
 
        Properties properties;
        /* Use a mutex if you access variables from different threads
         * (e.g. ice functions and RemoteGui_update()).
        std::mutex propertiesMutex;
        */
 
        /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
        /// Tab shown in the Remote GUI.
        struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
        {
            armarx::RemoteGui::Client::LineEdit boxLayerName;
            armarx::RemoteGui::Client::IntSpinBox numBoxes;
 
            armarx::RemoteGui::Client::Button drawBoxes;
        };
        RemoteGuiTab tab;
        */
 
        /* (Requires the armarx::ArVizComponentPluginUser.)
         * When used from different threads, an ArViz client needs to be synchronized.
        /// Protects the arviz client inherited from the ArViz plugin.
        std::mutex arvizMutex;
        */
 
        PeriodicTask<LaserScannerFeatureExtraction>::pointer_type task;
 
        std::unique_ptr<FeatureExtractor> featureExtractor;
        std::unique_ptr<ArVizDrawer> arVizDrawer;
 
        std::unique_ptr<FrequencyReporter> frequencyReporterPublish;
        std::unique_ptr<FrequencyReporter> frequencyReporterRun;
 
        std::optional<armarx::navigation::algorithms::Costmap> obstacleDistancesCostmapSmallMargin;
 
        std::unordered_map<std::string, Throttler> throttlers;
 
        VirtualRobot::RobotPtr robot;
 
        std::variant<std::monostate, VirtualRobot::MathTools::ConvexHull2DPtr, Eigen::Vector2f>
            robotHull;
        VirtualRobot::MathTools::ConvexHull2DPtr cableArea;
 
 
        armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>*
            virtualRobotReaderPlugin = nullptr;
 
        armem::client::plugins::ReaderWriterPlugin<armem::laser_scans::client::Reader>*
            laserScannerReaderPlugin = nullptr;
 
        armem::client::plugins::ReaderWriterPlugin<memory::client::laser_scanner_features::Writer>*
            laserScannerFeatureWriterPlugin = nullptr;
 
        armem::client::plugins::ReaderWriterPlugin<navigation::memory::client::costmap::Reader>*
            costmapReaderPlugin = nullptr;
    };
} // namespace armarx::navigation::components::laser_scanner_feature_extraction