IMUOutput.h

Go to the documentation of this file.

/*
Copyright 2007-2025. Algoryx Simulation AB.
 
All AGX source code, intellectual property, documentation, sample code,
tutorials, scene files and technical white papers, are copyrighted, proprietary
and confidential material of Algoryx Simulation AB. You may not download, read,
store, distribute, publish, copy or otherwise disseminate, use or expose this
material unless having a written signed agreement with Algoryx Simulation AB, or having been
advised so by Algoryx Simulation AB for a time limited evaluation, or having purchased a
valid commercial license from Algoryx Simulation AB.
 
Algoryx Simulation AB disclaims all responsibilities for loss or damage caused
from using this software, unless otherwise stated in written agreements with
Algoryx Simulation AB.
*/
#pragma once
#include <agxSensor/SensorOutput.h>
#include <agxSensor/SystemNode.h>
 
#include <agx/Referenced.h>
 
#include <limits>
 
namespace agxSensor
{
  AGX_DECLARE_POINTER_TYPES(IMUOutput);
 
  class AGXSENSOR_EXPORT IMUOutput
    : public SystemNode
    , public ISensorOutput
  {
    public:
      enum Field : int32_t
      {
        ACCELEROMETER_X_F64,
        ACCELEROMETER_Y_F64,
        ACCELEROMETER_Z_F64,
        GYROSCOPE_X_F64,
        GYROSCOPE_Y_F64,
        GYROSCOPE_Z_F64,
        MAGNETOMETER_X_F64,
        MAGNETOMETER_Y_F64,
        MAGNETOMETER_Z_F64,
        SENSOR_0_X_F64,
        SENSOR_0_Y_F64,
        SENSOR_0_Z_F64,
        SENSOR_1_X_F64,
        SENSOR_1_Y_F64,
        SENSOR_1_Z_F64,
        SENSOR_2_X_F64,
        SENSOR_2_Y_F64,
        SENSOR_2_Z_F64,
        SENSOR_3_X_F64,
        SENSOR_3_Y_F64,
        SENSOR_3_Z_F64,
        SENSOR_4_X_F64,
        SENSOR_4_Y_F64,
        SENSOR_4_Z_F64,
        SENSOR_5_X_F64,
        SENSOR_5_Y_F64,
        SENSOR_5_Z_F64,
        SENSOR_6_X_F64,
        SENSOR_6_Y_F64,
        SENSOR_6_Z_F64,
        SENSOR_7_X_F64,
        SENSOR_7_Y_F64,
        SENSOR_7_Z_F64,
        SENSOR_8_X_F64,
        SENSOR_8_Y_F64,
        SENSOR_8_Z_F64,
        SENSOR_9_X_F64,
        SENSOR_9_Y_F64,
        SENSOR_9_Z_F64,
        SENSOR_MAX_X_F64 = (((1 << 30) - 3) / 3) * 3,
        SENSOR_MAX_Y_F64 = SENSOR_MAX_X_F64 + 1,
        SENSOR_MAX_Z_F64 = SENSOR_MAX_X_F64 + 2,
        NUM_FIELDS
      };
 
      using FieldVector = std::vector<Field>;
 
    public:
      static size_t sizeOf(Field field);
 
      static const char* getFieldName(Field field);
 
    public:
      IMUOutput() = default;
 
      template <Field... field>
      void build();
 
      IMUOutput& add(Field field);
 
      void setPaddingValue(const agx::Real& value);
 
      virtual size_t getElementSize() const override;
 
      virtual const BinaryOutputBuffer& getData() override;
 
      virtual bool hasUnreadData(bool markAsRead = true) override;
 
      bool contains(Field field) const;
 
      size_t getOffset(Field field) const;
 
      const FieldVector& getFields() const;
 
      agx::Vec3 computeAccelerometerCovariance() const;
 
      agx::Vec3 computeGyroscopeCovariance() const;
 
      agx::Vec3 computeMagnetometerCovariance() const;
 
      DOXYGEN_START_INTERNAL_BLOCK()
 
    public:
      virtual ~IMUOutput() = default;
 
      virtual void result(const CallbackData& data) override;
      virtual void cleanup() override;
 
      AGXSTREAM_DECLARE_SERIALIZABLE(agxSensor::IMUOutput);
 
      DOXYGEN_END_INTERNAL_BLOCK()
 
    private:
      FieldVector m_fields {};
      size_t m_elementSize {0u};
      agx::Real m_paddingValue {std::numeric_limits<agx::Real>::signaling_NaN()};
      bool m_hasUnreadData {false};
      BinaryOutputBuffer m_output {};
  };
 
  template <IMUOutput::Field... field>
  void IMUOutput::build()
  {
    m_fields.clear();
    m_elementSize = 0u;
    (this->add(field), ...);
  }
 
  DOXYGEN_START_INTERNAL_BLOCK()  // swig
 
#pragma pack(push, 1)
  struct SixDoFValue
  {
      agx::Real x0, y0, z0;
      agx::Real x1, y1, z1;
 
      inline SixDoFValue() {}
      inline SixDoFValue(const agx::Vec3& a, const agx::Vec3& b)
        : x0(a.x())
        , y0(a.y())
        , z0(a.z())
        , x1(b.x())
        , y1(b.y())
        , z1(b.z())
      {
      }
      inline SixDoFValue(const SixDoFValue&) = default;
      inline SixDoFValue& operator=(const SixDoFValue&) = default;
 
      inline agx::Vec3 getTriplet(const size_t i) const noexcept
      {
        if (i == 0)
          return agx::Vec3(x0, y0, z0);
        else if (i == 1)
          return agx::Vec3(x1, y1, z1);
        else
          return agx::Vec3();
      }
  };
 
  struct NineDoFValue
  {
      agx::Real x0, y0, z0;
      agx::Real x1, y1, z1;
      agx::Real x2, y2, z2;
 
      inline NineDoFValue() {}
      inline NineDoFValue(const agx::Vec3& a, const agx::Vec3& b, const agx::Vec3& c)
        : x0(a.x())
        , y0(a.y())
        , z0(a.z())
        , x1(b.x())
        , y1(b.y())
        , z1(b.z())
        , x2(c.x())
        , y2(c.y())
        , z2(c.z())
      {
      }
      inline NineDoFValue(const NineDoFValue&) = default;
      inline NineDoFValue& operator=(const NineDoFValue&) = default;
 
      inline agx::Vec3 getTriplet(const size_t i) const noexcept
      {
        if (i == 0)
          return agx::Vec3(x0, y0, z0);
        else if (i == 1)
          return agx::Vec3(x1, y1, z1);
        else if (i == 2)
          return agx::Vec3(x2, y2, z2);
        else
          return agx::Vec3();
      }
  };
#pragma pack(pop)
 
  class AGXSENSOR_EXPORT IMUOutputSixDoF : public IMUOutput
  {
    public:
      IMUOutputSixDoF();
  };
 
  class AGXSENSOR_EXPORT IMUOutputNineDoF : public IMUOutput
  {
    public:
      IMUOutputNineDoF();
  };
 
  DOXYGEN_END_INTERNAL_BLOCK()
}