SelfLocalization.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 <optional>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "ArmarXCore/core/time/DateTime.h"
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
#include <ArmarXCore/core/util/IceReportSkipper.h>
 
#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h"
#include "RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h"
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/libraries/armem/client/plugins/ListeningPluginUser.h>
#include <RobotAPI/libraries/armem_robot_state/types.h>
 
#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
 
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
 
namespace armarx::armem::client::robot_state::localization
{
    class TransformWriterInterface;
    class TransformReaderInterface;
} // namespace armarx::armem::client::robot_state::localization
 
namespace armarx
{
 
    struct PoseStamped
    {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        Eigen::Affine3f pose = Eigen::Affine3f::Identity(); // [mm]
        armarx::DateTime timestamp;
    };
 
    /**
    * @brief The SelfLocalization class.
    *
    *   This class serves as a base class for all self-localization implementations.
    *   The global pose is sent to a multitude of topics, written to ArMem and to MemoryX short and longterm memory,
    *   which is all done by this class.
    *
    *   All you have to do, is to
    *   - implement worldToMapTransform()
    *   - call publishSelfLocalization(...) whenever needed.
    *
    *
    * Hint:
    *   If your component which is derived from SelfLocalization implements its own interface,
    *   make sure that it also extends the MemoryListenerInterface
    *   (RobotAPI/interface/armem/client/MemoryListenerInterface.ice).
    */
    class
    [[deprecated("This class was moved to armarx::localization_and_mapping. It is kept for ArmarXSimulation but should not be removed after CMake modernization of ArmarXSimulation.")]]
    SelfLocalization :
        virtual public armarx::Component,
        virtual public armem::ListeningClientPluginUser
    {
 
    public:
        SelfLocalization();
        virtual ~SelfLocalization();
 
        PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        void publishSelfLocalization(const PoseStamped& map_T_robot);
 
        [[nodiscard]] std::optional<Eigen::Affine3f> globalPoseFromLongTermMemory() const;
 
        /// @see armarx::ManagedIceObject::onInitComponent()
        void onInitComponent() override;
 
        /// @see armarx::ManagedIceObject::onConnectComponent()
        void onConnectComponent() override;
 
        void onReconnectComponent();
 
        /// @see armarx::ManagedIceObject::onDisconnectComponent()
        void onDisconnectComponent() override;
 
        /// @see armarx::ManagedIceObject::onExitComponent()
        void
        onExitComponent() override
        {
        }
 
 
    protected:
        /// static transformation from world to map
        virtual Eigen::Affine3f worldToMapTransform() const = 0;
 
        const std::string& getAgentName() const noexcept;
        const std::string& getRobotRootFrame() const noexcept;
        const std::shared_ptr<VirtualRobot::Robot>& getRobot() const noexcept;
 
        armem::client::robot_state::localization::TransformWriterInterface&
        getTransformWriter() const;
        const armem::client::robot_state::localization::TransformReaderInterface&
        getTransformReader() const;
 
    private:
        void publishSelfLocalizationOnTopic(const PoseStamped& map_T_robot);
 
        void publishSelfLocalizationToWorkingMemory(const PoseStamped& map_T_robot);
        void publishSelfLocalizationToLongtermMemory(const PoseStamped& map_T_robot);
        void publishSelfLocalizationToArMem(const PoseStamped& map_T_robot);
 
        // setup
        void setupArMem();
        void setupWorkingMemory();
        void setupLongTermMemory();
 
        // helpers
        TransformStamped toTransformStamped(const PoseStamped& poseStamped);
        armem::robot_state::Transform toTransform(const PoseStamped& poseStamped);
 
        // publisher
        LaserScannerSelfLocalisationListenerPrx reportTopic;
        GlobalRobotPoseLocalizationListenerPrx globalRobotPoseTopic;
        GlobalRobotPoseLocalizationCorrectionListenerPrx gobalRobotPoseCorrectionTopic;
 
        // long-term memory
        memoryx::LongtermMemoryInterfacePrx longtermMemory;
        memoryx::PersistentEntitySegmentBasePrx ltmSelfLocalisationSegment;
        std::optional<std::string> ltmPoseEntityId;
 
        // working memory
        memoryx::WorkingMemoryInterfacePrx workingMemory;
        memoryx::AgentInstancesSegmentBasePrx wmAgentsMemory;
        memoryx::AgentInstancePtr wmAgentInstance;
 
        // proxies
        RobotStateComponentInterfacePrx robotStateComponent;
        LocalizationUnitInterfacePrx localizationUnitPrx;
 
        // utils
        IceReportSkipper skipper{0.};
 
        struct Properties
        {
            // old working memory
            bool updateWorkingMemory = true;
            float workingMemoryUpdateFrequency = 50.F;
 
            // old long term memory
            std::string longtermMemoryName = "LongtermMemory";
            bool updateLongTermMemory = true;
            float longtermMemoryUpdateFrequency = 1.F;
            bool initialPoseFromLTM = true;
 
            // armem
            bool updateArMem = true;
            float armMemUpdateFrequency = 1000.F;
 
            // general
            bool useLocalizationUnit = true;
            std::string localizationUnitName = ""; // will be set in c'tor
        } properties;
 
        std::string agentName;
        std::string robotRootFrame;
 
        std::shared_ptr<VirtualRobot::Robot> robot;
 
        armem::client::plugins::ReaderWriterPlugin<armem::client::robot_state::localization::TransformWriter>* transformWriterPlugin = nullptr;
        armem::client::plugins::ReaderWriterPlugin<armem::client::robot_state::localization::TransformReader>* transformReaderPlugin = nullptr;
 
        Eigen::Affine3f global_T_map;
 
        std::atomic_bool connected{false};
 
        bool onConnectCalled{false};
    };
 
} // namespace armarx