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/>.
 *
 * @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 <atomic>
#include <cstdint>
#include <experimental/memory>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <Ice/Current.h>
 
#include "ArmarXCore/libraries/logging/FrequencyReporter.h"
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/core/util/Throttler.h>
#include <ArmarXCore/interface/core/PackagePath.h>
#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/interface/observers/Timestamp.h>
 
#include <ArmarXGui/interface/RemoteGuiInterface.h>
 
#include "RobotAPI/libraries/armem_robot_state/client/common/RobotReader.h"
#include <RobotAPI/interface/units/LaserScannerUnit.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
#include <RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h>
 
#include <armarx/localization_and_mapping/cartographer_adapter/CartographerAdapter.h>
#include <armarx/localization_and_mapping/cartographer_adapter/MessageQueue.h>
#include <armarx/localization_and_mapping/cartographer_adapter/interfaces.h>
#include <armarx/localization_and_mapping/cartographer_adapter/types.h>
#include <armarx/localization_and_mapping/components/cartographer_mapping_and_localization/ComponentInterface.h>
#include <armarx/localization_and_mapping/components/cartographer_mapping_and_localization/LocalizationRemoteGui.h>
#include <armarx/localization_and_mapping/components/cartographer_mapping_and_localization/MappingRemoteGui.h>
#include <armarx/localization_and_mapping/self_localization/SelfLocalization.h>
#include <armarx/navigation/memory/client/costmap/Writer.h>
 
// forward decl
namespace cartographer::mapping
{
    class MapBuilderInterface;
} // namespace cartographer::mapping
 
namespace armarx
{
    class TransformWriterInterface;
    class TransformReaderInterface;
    class FrequencyReporter;
} // namespace armarx
 
namespace armarx::localization_and_mapping::cartographer_adapter
{
    // forward decl
    class ArVizDrawer;
    class ArVizDrawerMapBuilder;
} // namespace armarx::localization_and_mapping::cartographer_adapter
 
namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping
{
 
 
    /**
     * @defgroup Component-CartographerMappingAndLocalization CartographerMappingAndLocalization
     * @ingroup RobotComponents-Components
     * A description of the component CartographerMappingAndLocalization.
     *
     * @class CartographerMappingAndLocalization
     * @ingroup Component-CartographerMappingAndLocalization
     * @brief Brief description of class CartographerMappingAndLocalization.
     *
     * Detailed description of class CartographerMappingAndLocalization.
     */
    class Component :
        virtual public CartographerMappingAndLocalizationInterface,
        virtual public ArVizComponentPluginUser,
        virtual public cartographer_adapter::SlamDataCallable,
        virtual public RemoteGuiCallee,
        virtual public LocalizationRemoteGuiCallee,
        virtual public self_localization::SelfLocalization
    {
    public:
        Component();
        virtual ~Component();
 
        /// @see armarx::ManagedIceObject::getDefaultName()
        std::string getDefaultName() const override;
        static std::string GetDefaultName();
 
        // LaserScannerUnitListener interface
        void reportSensorValues(const ::std::string&,
                                const ::std::string&,
                                const ::armarx::LaserScan&,
                                const ::armarx::TimestampBasePtr&,
                                const ::Ice::Current&) override;
 
        // PlatformUnitListener interface
        // TODO: use streaming (RobotUnit)
        //        void reportOdometryPose(const TransformStamped& odometryPose, const Ice::Current&) override;
        //        void reportOdometryVelocity(const TwistStamped& velocity, const Ice::Current&) override;
 
        // SlamDataCallable interface
        void onLocalSlamData(const cartographer_adapter::LocalSlamData& slamData) override;
        // void onGraphOptimized(const OptimizedGraphData& graphData) override;
        // void onGridMap(const SubMapDataVector& submapData) override;
        void onLaserSensorData(const cartographer_adapter::LaserScannerData& laserData) override;
 
        // RemoteGuiCallee
        void onCreateMapButtonClicked(const RemoteGuiCallee::ButtonClickContext& ctx) override;
 
        enum class Mode
        {
            Mapping,
            Localization
        };
 
        void onWorldToMapTransformUpdate(const Eigen::Isometry3f& world_T_map) override;
 
        enum class ConfigProfile
        {
            Default,
            RobotSpecific
        };
 
    protected:
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        void onReconnectComponent();
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onExitComponent() override;
 
        /// @see PropertyUser::createPropertyDefinitions()
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        Eigen::Isometry3f worldToMapTransform() const override;
 
    private:
        void setupMappingAdapter();
 
        //! Recompute the occupancy grid and costmaps from the current map and world_T_map, and
        //! store them in memory. Called once on startup, and again whenever the world-to-map
        //! registration is updated.
        void storeOccupancyGridAndCostmapInMemory();
 
        //! adapter to cartographer
        std::unique_ptr<cartographer_adapter::CartographerAdapter> mappingAdapter;
        cartographer_adapter::CartographerAdapter::Config adapterConfig;
 
        //! ArViz plotter
        std::unique_ptr<cartographer_adapter::ArVizDrawer> arvizDrawer;
 
        //! Remote GUI to trigger map creation
        std::unique_ptr<MappingRemoteGui> mappingRemoteGui;
        std::unique_ptr<LocalizationRemoteGui> localizationRemoteGui;
 
        // queues to handle incoming messages
        cartographer_adapter::MessageQueue<cartographer_adapter::LaserScannerMessage>
            laserMessageQueue;
        cartographer_adapter::MessageQueue<cartographer_adapter::CartographerAdapter::OdomData>
            odomMessageQueue;
 
        //! helper flag to prevent creation of multiple maps
        bool mapCreated{false};
 
        // Eigen::Isometry3f globalPoseFromLongTermMemory(const std::string& agentName, const auto& robot);
 
        // FIXME remove
        // std::atomic<int64_t> lastTimestamp{0};
 
        // topic publisher
        RemoteGuiInterfacePrx remoteGui;
        DebugObserverInterfacePrx debugObserver;
 
        // others
        // armem::vision::laser_scans::client::Writer laserScansMemoryWriter;
        // armem::vision::occupancy_grid::client::Writer occupancyGridMemoryWriter;
        // armem::robot_state::RobotReader robotReader;
 
        //! properties
        struct Properties
        {
            armarx::data::PackagePath mapPath{.package = "PriorKnowledgeData",
                                              .path = "maps/cartographer"};
 
            std::string mapStorageSubDir;
 
            armarx::data::PackagePath mapToLoad{.package = "PriorKnowledgeData",
                                                .path = "maps/cartographer/..."};
 
            armarx::data::PackagePath cartographerConfigPath{.package =
                                                                 "armarx_localization_and_mapping",
                                                             .path = "cartographer/config/"};
 
            Mode mode = Mode::Mapping;
            bool enableVisualization = true;
            bool enableMappingVisualization = true;
 
            ConfigProfile profile = ConfigProfile::Default;
 
            int odomQueryPeriodMs = 20;
            int occupancyGridMemoryUpdatePeriodMs = 1000;
 
            bool useNativeOdometryTimestamps = true;
            bool useNativeLaserscannerTimestamps = true;
 
            std::string laserScanners;
 
            struct
            {
                float global_localization_min_score = 0.5;
            } cartographer;
 
            float obstacleRadius = 500; // [mm]
 
            float occupiedThreshold = 0.55;
 
        } properties;
 
        Throttler throttlerLaserScansMemoryWriter{10.F};
        Throttler throttlerOccupancyGridMemoryWriter{10.F};
 
        Throttler throttlerArviz{10.F};
 
        std::unique_ptr<cartographer_adapter::ArVizDrawerMapBuilder> arVizDrawerMapBuilder;
 
        std::optional<FrequencyReporter> laserDataReporter;
        std::optional<FrequencyReporter> odomDataReporter;
        std::optional<FrequencyReporter> slamResultReporter;
 
        // std::unique_ptr<components::cartographer_localization_and_mapping::RobotUnitReader> robotUnitReader;
 
        SimplePeriodicTask<>::pointer_type occupancyGridTask;
        SimplePeriodicTask<>::pointer_type odometryQueryTask;
 
        std::experimental::observer_ptr<armem::client::plugins::ReaderWriterPlugin<
            armem::vision::occupancy_grid::client::Writer>>
            occupancyGridWriterPlugin;
 
        std::experimental::observer_ptr<
            armem::client::plugins::ReaderWriterPlugin<navigation::memory::client::costmap::Writer>>
            costmapWriterPlugin;
 
        std::experimental::observer_ptr<
            armem::client::plugins::ReaderWriterPlugin<armem::robot_state::RobotReader>>
            robotReaderPlugin;
 
        Eigen::Isometry3f world_T_map = Eigen::Isometry3f::Identity();
        std::optional<Eigen::Isometry3f> map_T_robot = std::nullopt;
 
        std::atomic_bool receivingData{false};
 
 
        std::optional<Eigen::Isometry3f> globalPose() const;
 
        // armarx::plugins::RobotUnitComponentPlugin* robotUnit;
 
 
        std::mutex inputMtx;
 
 
        // used within the odometry task
        int64_t lastTimestamp = 0;
        std::atomic_bool robotIsMoving = false;
 
        plugins::HeartbeatComponentPlugin* heartbeatPlugin = nullptr;
    };
} // namespace armarx::localization_and_mapping::components::cartographer_localization_and_mapping