GlobalRobotPoseSensorDevice.cpp

Go to the documentation of this file.

#include "GlobalRobotPoseSensorDevice.h"
 
#include <SimoxUtility/math/convert/pos_rpy_to_mat4f.h>
 
#include "ArmarXCore/core/logging/Logging.h"
 
namespace armarx
{
    SensorValueBase::SensorValueInfo<SensorValueGlobalPoseCorrection>
    SensorValueGlobalPoseCorrection::GetClassMemberInfo()
    {
        SensorValueInfo<SensorValueGlobalPoseCorrection> svi;
        svi.addMemberVariable(&SensorValueGlobalPoseCorrection::global_T_odom, "global_T_odom")
            .setVariantReportFrame("", GlobalFrame);
        return svi;
    }
 
    bool
    SensorValueGlobalPoseCorrection::isAvailable() const
    {
        return not global_T_odom.isIdentity();
    }
 
    SensorValueBase::SensorValueInfo<SensorValueGlobalRobotPose>
    SensorValueGlobalRobotPose::GetClassMemberInfo()
    {
        SensorValueInfo<SensorValueGlobalRobotPose> svi;
        svi.addMemberVariable(&SensorValueGlobalRobotPose::global_T_root, "global_T_root")
            .setVariantReportFrame("", GlobalFrame);
        return svi;
    }
 
    bool
    SensorValueGlobalRobotPose::isAvailable() const
    {
        return not global_T_root.isIdentity();
    }
 
    std::string
    GlobalRobotPoseCorrectionSensorDevice::DeviceName()
    {
        return "GlobalRobotPoseCorrectionSensorDevice";
    }
 
    GlobalRobotPoseCorrectionSensorDevice::GlobalRobotPoseCorrectionSensorDevice() :
        DeviceBase(DeviceName()), SensorDevice(DeviceName())
    {
        transformationBuffer.reinitAllBuffers(SensorValueType::Transformation::Identity());
    }
 
    const SensorValueBase*
    GlobalRobotPoseCorrectionSensorDevice::getSensorValue() const
    {
        return &sensor;
    }
 
    std::string
    GlobalRobotPoseCorrectionSensorDevice::getReportingFrame() const
    {
        return GlobalFrame;
    }
 
    void
    GlobalRobotPoseCorrectionSensorDevice::rtReadSensorValues(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        sensor.global_T_odom = transformationBuffer.getUpToDateReadBuffer();
    }
 
    void
    GlobalRobotPoseCorrectionSensorDevice::updateGlobalPositionCorrection(
        const SensorValueType::Transformation& global_T_odom)
    {
        transformationBuffer.getWriteBuffer() = global_T_odom;
        transformationBuffer.commitWrite();
    }
 
    std::string
    GlobalRobotPoseSensorDevice::DeviceName()
    {
        return "GlobalRobotPoseSensorDevice";
    }
 
    GlobalRobotPoseSensorDevice::GlobalRobotPoseSensorDevice() :
        DeviceBase(DeviceName()), SensorDevice(DeviceName())
    {
    }
 
    const SensorValueBase*
    GlobalRobotPoseSensorDevice::getSensorValue() const
    {
        return &sensor;
    }
 
    std::string
    GlobalRobotPoseSensorDevice::getReportingFrame() const
    {
        return GlobalFrame;
    }
 
    void
    GlobalRobotPoseSensorDevice::rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp,
                                                    const IceUtil::Time& timeSinceLastIteration)
    {
    }
 
    std::string
    GlobalRobotLocalizationSensorDevice::DeviceName()
    {
        return "GlobalRobotLocalizationSensorDevice";
    }
 
    GlobalRobotLocalizationSensorDevice::GlobalRobotLocalizationSensorDevice() :
        DeviceBase(DeviceName()), SensorDevice(DeviceName())
    {
    }
 
    void
    GlobalRobotLocalizationSensorDevice::rtReadSensorValues(
        const IceUtil::Time& sensorValuesTimestamp,
        const IceUtil::Time& timeSinceLastIteration)
    {
        if (sensorGlobalPositionCorrection == nullptr)
        {
            ARMARX_ERROR << "The global position correction sensor is not available."
                         << deactivateSpam(1);
            return;
        }
        if (sensorRelativePosition == nullptr)
        {
            ARMARX_ERROR << "The relative position sensor is not available." << deactivateSpam(1);
            return;
        }
 
        const auto global_T_relative = sensorGlobalPositionCorrection->global_T_odom;
        const auto relative_T_robot =
            simox::math::pos_rpy_to_mat4f(sensorRelativePosition->relativePositionX,
                                          sensorRelativePosition->relativePositionY,
                                          0,
                                          0,
                                          0,
                                          sensorRelativePosition->relativePositionRotation);
        const auto global_T_robot = global_T_relative * relative_T_robot;
 
        sensor.global_T_root = global_T_robot;
    }
} // namespace armarx