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 <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 <MemoryX/interface/components/LongtermMemoryInterface.h>
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
#include <MemoryX/libraries/memorytypes/entity/SimpleEntity.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/KalmanFilter.h>
#include <MemoryX/libraries/helpers/EarlyVisionHelpers/PlatformKalmanFilter.h>
 
#include <RobotComponents/interface/components/LaserScannerSelfLocalisation.h>
 
#include <Eigen/Eigen>
#include <optional>
#include <atomic>
 
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;
 
    };
}