LaserScannerSelfLocalisation.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::LaserScannerSelfLocalisation
 * @author     Fabian Paus ( fabian dot paus at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <atomic>
#include <optional>
 
#include <Eigen/Eigen>
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/util/IceReportSkipper.h>
#include <ArmarXCore/interface/components/EmergencyStopInterface.h>
 
#include <RobotAPI/interface/units/LaserScannerUnit.h>
 
#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
 
#include <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/KalmanFilter.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/PlatformKalmanFilter.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/SimpleEntity.h>
 
namespace armarx
{
    struct LineSegment2Df
    {
        Eigen::Vector2f start;
        Eigen::Vector2f end;
    };
 
    struct ExtractedEdge
    {
        LineSegment2Df segment;
        Eigen::Vector2f* pointsBegin;
        Eigen::Vector2f* pointsEnd;
    };
 
    struct LaserScanData
    {
        Eigen::Matrix4f pose;
        LaserScannerInfo info;
        std::shared_ptr<std::mutex> mutex;
        LaserScan scan;
        std::vector<Eigen::Vector2f> points;
        std::vector<ExtractedEdge> edges;
        IceUtil::Time measurementTime;
    };
 
    struct ReportedVelocity
    {
        float dt;
        float x;
        float y;
        float theta;
    };
 
    /**
     * @class LaserScannerSelfLocalisationPropertyDefinitions
     * @brief
     */
    class LaserScannerSelfLocalisationPropertyDefinitions :
        public armarx::ComponentPropertyDefinitions
    {
    public:
        LaserScannerSelfLocalisationPropertyDefinitions(std::string prefix) :
            armarx::ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("ReportTopicName",
                                                "LaserScannerSelfLocalisationTopic",
                                                "The name of the report topic.");
            defineOptionalProperty<std::string>("RobotStateComponentName",
                                                "RobotStateComponent",
                                                "The name of the RobotStateComponent. Used to get "
                                                "local transformation of laser scanners");
            defineOptionalProperty<std::string>("PlatformName",
                                                "Platform",
                                                "Name of the platform to use. This property is "
                                                "used to listen to the platform topic");
            defineOptionalProperty<std::string>("LaserScannerUnitName",
                                                "LaserScannerSimulation",
                                                "Name of the laser scanner unit to use.");
            defineOptionalProperty<std::string>("WorkingMemoryName",
                                                "WorkingMemory",
                                                "Name of the WorkingMemory that should be used");
            defineOptionalProperty<std::string>(
                "LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
            defineOptionalProperty<bool>(
                "UpdateWorkingMemory",
                true,
                "Update the working memory with the corrected position (disable in simulation)");
            defineOptionalProperty<std::string>("MapFilename",
                                                "RobotComponents/maps/building-5020-kitchen.json",
                                                "Floor map (2D) used for global localisation");
            defineOptionalProperty<std::string>("AgentName",
                                                "",
                                                "Name of the agent instance. If empty, the robot "
                                                "name of the RobotStateComponent will be used");
            defineOptionalProperty<std::string>(
                "EmergencyStopMasterName",
                "EmergencyStopMaster",
                "The name used to register as an EmergencyStopMaster");
            defineOptionalProperty<std::string>("DebugObserverName",
                                                "DebugObserver",
                                                "Name of the topic the DebugObserver listens on");
 
            defineOptionalProperty<int>(
                "UpdatePeriodInMs", 5, "Update period used for the map localisation");
            defineOptionalProperty<int>("UpdatePeriodWorkingMemoryInMs",
                                        30,
                                        "Update period used for updating the working memory");
            defineOptionalProperty<int>("UpdatePeriodLongtermMemoryInMs",
                                        30,
                                        "Update period used for updating the longterm memory");
            defineOptionalProperty<float>("RobotPositionZ",
                                          0.0f,
                                          "The z-coordinate of the reported postion. Laser "
                                          "scanners can only self localize in x,y.");
 
            defineOptionalProperty<float>("MaximalLaserScannerDelay",
                                          0.1,
                                          "If no new sensor values have been reported for this "
                                          "amound of seconds, we emit a soft emergency stop");
 
            defineOptionalProperty<int>("SmoothFrameSize",
                                        7,
                                        "Frame size used for smoothing of laser scanner input",
                                        PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>(
                "SmoothMergeDistance",
                160,
                "Distance in mm up to which laser scanner points are merged",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "MatchingMaxDistance",
                300.0f,
                "Maximal distance in mm up to which points are matched against edges of the map",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "MatchingMinPoints",
                0.01f,
                "Minimum percentage of points which need to be matched (range [0, 1]). If less "
                "points are matched no global correction is applied.",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "MatchingCorrectionFactor",
                0.01f,
                "This factor is used to apply the calculated map correction (range [0, 1])",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("EdgeMaxDistance",
                                          600.0f,
                                          "Maximum distance between adjacent points up to which "
                                          "they are merged into one edge [mm]",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>(
                "EdgeMaxDeltaAngle",
                0.10472f,
                "Maximum angle delta up to which adjacent points are merged into one edge [rad]",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("EdgePointAddingThreshold",
                                          10.0f,
                                          "Maximum least square error up to which points are added "
                                          "to an edge (extension at the front and back)",
                                          PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>("EdgeEpsilon",
                                        4,
                                        "Half frame size for line regression (angle calculation)",
                                        PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<int>(
                "EdgeMinPoints",
                30,
                "Minimum number of points per edge (no edges with less points will be extracted)",
                PropertyDefinitionBase::eModifiable);
 
            defineOptionalProperty<bool>("UseOdometry",
                                         true,
                                         "Enable or disable odometry for pose estimation",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("UseMapCorrection",
                                         true,
                                         "Enable or disable map localisation for pose correction",
                                         PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>(
                "ReportPoints",
                false,
                "Enable or disable the reports of (post-processed) laser scan points",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>("ReportEdges",
                                         true,
                                         "Enable or disable the reports of extracted edges",
                                         PropertyDefinitionBase::eModifiable);
 
            defineOptionalProperty<float>(
                "SensorStdDev",
                100,
                "Standard deviation in position of the localization result",
                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<float>("VelSensorStdDev",
                                          0.1f,
                                          "Standard deviation of the reported velocity",
                                          PropertyDefinitionBase::eModifiable);
 
            defineOptionalProperty<std::string>("LoggingFilePath",
                                                "",
                                                "Path to sensor data log file",
                                                PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<bool>(
                "RecordData",
                false,
                "If true, data is logged. Can be changed online. When set to false online, data is "
                "written to file. Otherwise on disconnect.",
                PropertyDefinitionBase::eModifiable);
 
            defineOptionalProperty<std::string>(
                "PlatformRectangleMin",
                "0, 0",
                "Ignores points within the platform rectangle (this is the min x/y point)");
            defineOptionalProperty<std::string>(
                "PlatformRectangleMax",
                "0, 0",
                "Ignores points within the platform rectangle (this is the max x/y point)");
        }
    };
 
    /**
     * @defgroup Component-LaserScannerSelfLocalisation LaserScannerSelfLocalisation
     * @ingroup RobotComponents-Components
     * The component LaserScannerSelfLocalisation is responsible for self localisation of a
     * platform in a predefined two dimensional map.
     *
     * The odometry is used as a primary source for determining the current position of the
     * platform. Since this method leads to unbounded errors, we also use the laser scanners
     * to correct the estimated position. This is done by matching the laser scan input to
     * edges of a predefined map.
     *
     * This method requires a good estimate of the initial position at startup because the
     * correction by the laser scans can only correct small errors.
     *
     *
     * @class LaserScannerSelfLocalisation
     * @ingroup Component-LaserScannerSelfLocalisation
     * @brief The class LaserScannerSelfLocalisation implements a self localisation strategy.
     */
    class LaserScannerSelfLocalisation :
        virtual public armarx::Component,
        virtual public armarx::LaserScannerSelfLocalisationInterface
    {
    public:
        LaserScannerSelfLocalisation();
 
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string
        getDefaultName() const override
        {
            return "LaserScannerSelfLocalisation";
        }
 
    protected:
        /**
         * @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;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
 
        // LaserScannerSelfLocalisationInterface interface
        std::string getReportTopicName(const Ice::Current&) override;
        void
        setAbsolutePose(Ice::Float x, Ice::Float y, Ice::Float theta, const Ice::Current&) override;
        LineSegment2DSeq getMap(const Ice::Current&) override;
 
        // LaserScannerUnitListener interface
        void reportSensorValues(const std::string& device,
                                const std::string& name,
                                const LaserScan& scan,
                                const TimestampBasePtr& timestamp,
                                const Ice::Current&) override;
 
        // PlatformUnitListener interface
        void reportPlatformVelocity(Ice::Float velX,
                                    Ice::Float velY,
                                    Ice::Float velTheta,
                                    const Ice::Current&) override;
        void reportPlatformOdometryPose(Ice::Float,
                                        Ice::Float,
                                        Ice::Float,
                                        const Ice::Current&) override;
 
        // Component interface
        void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
 
    private:
        void updateLocalisation();
 
        void updateProperties(bool initial = false);
 
        void resetKalmanFilter(const Eigen::Vector3f& pose);
        Eigen::Vector3f filterPose(const Eigen::Vector3f& pose);
        void writeLogFile();
 
    private:
        std::string reportTopicName;
        std::string robotStateComponentName;
        std::string platformName;
        std::string laserScannerUnitName;
        std::string workingMemoryName;
        std::string longtermMemoryName;
        std::string mapFilename;
        std::string agentName;
        std::string robotRootFrame;
 
        std::string emergencyStopMasterName;
        std::string debugObserverName;
        float workingMemoryUpdateFrequency;
        float longtermMemoryUpdateFrequency;
        float robotPositionZ = 0.0f;
        float maximalLaserScannerDelay = 0.1f;
 
        std::mutex propertyMutex;
        int propSmoothFrameSize;
        int propSmoothMergeDistance;
        float propMatchingMaxDistance;
        float propMatchingMinPoints;
        float propMatchingCorrectionFactor;
        float propEdgeMaxDistance;
        float propEdgeMaxDeltaAngle;
        float propEdgePointAddingThreshold;
        int propEdgeEpsilon;
        int propEdgeMinPoints;
        bool propReportPoints;
        bool propReportEdges;
        bool propUseOdometry;
        bool propUseMapCorrection;
        float propSensorStdDev;
        float propVelSensorStdDev;
        std::string propLoggingFilePath;
        bool propRecordData = false;
 
        std::atomic<bool> useOdometry;
 
        std::vector<LineSegment2Df> map;
 
        RobotStateComponentInterfacePrx robotState;
        LaserScannerUnitInterfacePrx laserScannerUnit;
        LaserScannerSelfLocalisationListenerPrx reportTopic;
        PlatformUnitListenerPrx platformUnitTopic;
 
        GlobalRobotPoseLocalizationListenerPrx globalRobotPoseTopic;
 
        DebugObserverInterfacePrx debugObserver;
        memoryx::WorkingMemoryInterfacePrx workingMemory;
        memoryx::AgentInstancesSegmentBasePrx agentsMemory;
        memoryx::AgentInstancePtr agentInstance;
        bool updateWorkingMemory = false;
        memoryx::LongtermMemoryInterfacePrx longtermMemory;
        memoryx::PersistentEntitySegmentBasePrx selfLocalisationSegment;
        std::string poseEntityId;
 
        std::vector<LaserScanData> scanData;
 
        std::mutex setPoseMutex;
        std::optional<Eigen::Vector3f> setPose;
 
        std::mutex odometryMutex;
        std::vector<ReportedVelocity> reportedVelocities;
        Eigen::Vector3f lastVelocity;
        IceUtil::Time lastVelocityUpdate;
 
        Eigen::Vector3f estimatedPose;
 
        Eigen::Vector2f platformRectMin{0, 0};
        Eigen::Vector2f platformRectMax{0, 0};
 
        PeriodicTask<LaserScannerSelfLocalisation>::pointer_type task;
        IceReportSkipper s;
 
        memoryx::PlatformKalmanFilterPtr filter;
        std::mutex kalmanMutex;
 
        // Filelogging
        struct LaserScannerFileLoggingData
        {
            std::mutex loggingMutex;
            std::string filePath;
 
            std::map<IceUtil::Time, std::vector<LaserScanData>> scanDataHistory;
        };
 
        std::unique_ptr<LaserScannerFileLoggingData> laserScannerFileLoggingData;
    };
} // namespace armarx