GlobalRobotPoseSensorDevice.h

Go to the documentation of this file.

 
#pragma once
 
#include <Eigen/Core>
 
#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
 
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include <RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx
{
 
    /**
    * @brief The pose correction to obtain the robot's global pose.
    *
    * In conjunction with the SensorValueHolonomicPlatformRelativePosition
    * information, the global robot pose can be obtained which
    * - provides the robot's initial localization within the map
    * - compensates odometry drift over time.
    *
    */
    class SensorValueGlobalPoseCorrection : virtual public SensorValueBase
    {
    public:
        using Transformation = Eigen::Matrix4f;
        Transformation global_T_odom = Transformation::Identity();
 
        // TODO add timestamp
 
        static SensorValueInfo<SensorValueGlobalPoseCorrection> GetClassMemberInfo();
 
        bool isAvailable() const;
 
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    class SensorValueGlobalRobotPose : virtual public SensorValueBase
    {
    public:
        using Transformation = Eigen::Matrix4f;
        Transformation global_T_root = Transformation::Identity();
 
        static SensorValueInfo<SensorValueGlobalRobotPose> GetClassMemberInfo();
 
        bool isAvailable() const;
 
        DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
    };
 
    TYPEDEF_PTRS_SHARED(GlobalRobotPoseCorrectionSensorDevice);
 
    class GlobalRobotPoseCorrectionSensorDevice : virtual public SensorDevice
    {
    public:
        using SensorValueType = SensorValueGlobalPoseCorrection;
 
        static std::string DeviceName();
 
        GlobalRobotPoseCorrectionSensorDevice();
 
        const SensorValueBase* getSensorValue() const override;
 
        std::string getReportingFrame() const override;
 
        void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
                                const IceUtil::Time& timeSinceLastIteration) override;
 
        void updateGlobalPositionCorrection(const SensorValueType::Transformation& global_T_odom);
 
    private:
        SensorValueType sensor;
 
        TripleBuffer<SensorValueType::Transformation> transformationBuffer;
    };
 
    TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
 
    class GlobalRobotPoseSensorDevice : virtual public SensorDevice
    {
    public:
        using SensorValueType = SensorValueGlobalRobotPose;
 
        static std::string DeviceName();
 
        GlobalRobotPoseSensorDevice();
 
        const SensorValueBase* getSensorValue() const override;
 
        std::string getReportingFrame() const override;
 
        void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
                                const IceUtil::Time& timeSinceLastIteration) override;
 
    protected:
        SensorValueType sensor;
    };
 
    TYPEDEF_PTRS_SHARED(GlobalRobotPoseSensorDevice);
 
    class GlobalRobotLocalizationSensorDevice : virtual public GlobalRobotPoseSensorDevice
    {
    public:
        static std::string DeviceName();
 
        GlobalRobotLocalizationSensorDevice();
 
        void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
                                const IceUtil::Time& timeSinceLastIteration) override;
 
        const SensorValueGlobalPoseCorrection* sensorGlobalPositionCorrection;
        const SensorValueHolonomicPlatformRelativePosition* sensorRelativePosition;
    };
 
 
} // namespace armarx