Skip to content

Commit

Permalink
IMU sensor API to get world ref frame and heading offset (#186)
Browse files Browse the repository at this point in the history
* Parse localization tag taking into account world frame orientation and heading offset.

Signed-off-by: Aditya <[email protected]>
  • Loading branch information
adityapande-1995 authored Mar 4, 2022
1 parent 2614d1d commit d606f8a
Show file tree
Hide file tree
Showing 3 changed files with 439 additions and 20 deletions.
32 changes: 30 additions & 2 deletions include/ignition/sensors/ImuSensor.hh
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,30 @@ namespace ignition
{
namespace sensors
{
// Inline bracket to help doxygen filtering.
/// Inline bracket to help doxygen filtering.
inline namespace IGNITION_SENSORS_VERSION_NAMESPACE {
//

/// \brief Reference frames enum
enum class IGNITION_SENSORS_VISIBLE WorldFrameEnumType
{
/// \brief NONE : To be used only when <localization>
/// reference orientation tag is empty.
NONE = 0,

/// \brief ENU (East-North-Up): x - East, y - North, z - Up.
ENU = 1,

/// \brief NED (North-East-Down): x - North, y - East, z - Down.
NED = 2,

/// \brief NWU (North-West-Up): x - North, y - West, z - Up.
NWU = 3,

/// \brief CUSTOM : The frame is described using custom_rpy tag.
CUSTOM = 4
};

///
/// \brief forward declarations
class ImuSensorPrivate;

Expand Down Expand Up @@ -136,6 +157,13 @@ namespace ignition
/// \return Gravity vectory in meters per second squared.
public: math::Vector3d Gravity() const;

/// \brief Specify the rotation offset of the coordinates of the World
/// frame relative to a geo-referenced frame
/// \param[in] _rot rotation offset
/// \param[in] _relativeTo world frame orientation, ENU by default
public: void SetWorldFrameOrientation(
const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo);

IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING
/// \brief Data pointer for private data
/// \internal
Expand Down
124 changes: 106 additions & 18 deletions src/ImuSensor.cc
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,23 @@ class ignition::sensors::ImuSensorPrivate
/// \brief Flag for if time has been initialized
public: bool timeInitialized = false;

/// \brief Orientation of world frame relative to a specified frame
public: ignition::math::Quaterniond worldRelativeOrientation;

/// \brief Frame relative-to which the worldRelativeOrientation
// is defined
public: WorldFrameEnumType worldFrameRelativeTo = WorldFrameEnumType::ENU;

/// \brief Frame relative-to which the sensor orientation is reported
public: WorldFrameEnumType sensorOrientationRelativeTo
= WorldFrameEnumType::NONE;

/// \brief Frame realtive to which custom_rpy is specified
public: std::string customRpyParentFrame;

/// \brief Quaternion for to store custom_rpy
public: ignition::math::Quaterniond customRpyQuaternion;

/// \brief Previous update time step.
public: std::chrono::steady_clock::duration prevStep
{std::chrono::steady_clock::duration::zero()};
Expand Down Expand Up @@ -116,24 +133,6 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
return false;
}

// Set orientation reference frame
// TODO(adityapande-1995) : Add support for named frames like
// ENU using ign-gazebo
if (_sdf.ImuSensor()->Localization() == "CUSTOM")
{
if (_sdf.ImuSensor()->CustomRpyParentFrame() == "")
{
this->SetOrientationReference(ignition::math::Quaterniond(
_sdf.ImuSensor()->CustomRpy()));
}
else
{
ignwarn << "custom_rpy parent frame must be set to empty "
"string. Setting it to any other frame is not "
"supported yet." << std::endl;
}
}

if (this->Topic().empty())
this->SetTopic("/imu");

Expand Down Expand Up @@ -166,6 +165,26 @@ bool ImuSensor::Load(const sdf::Sensor &_sdf)
}
}

std::string localization = _sdf.ImuSensor()->Localization();

if (localization == "ENU")
{
this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::ENU;
} else if (localization == "NED") {
this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NED;
} else if (localization == "NWU") {
this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NWU;
} else if (localization == "CUSTOM") {
this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::CUSTOM;
} else {
this->dataPtr->sensorOrientationRelativeTo = WorldFrameEnumType::NONE;
}

this->dataPtr->customRpyParentFrame =
_sdf.ImuSensor()->CustomRpyParentFrame();
this->dataPtr->customRpyQuaternion = ignition::math::Quaterniond(
_sdf.ImuSensor()->CustomRpy());

this->dataPtr->initialized = true;
return true;
}
Expand Down Expand Up @@ -292,6 +311,75 @@ math::Pose3d ImuSensor::WorldPose() const
return this->dataPtr->worldPose;
}

//////////////////////////////////////////////////
void ImuSensor::SetWorldFrameOrientation(
const math::Quaterniond &_rot, WorldFrameEnumType _relativeTo)
{
this->dataPtr->worldRelativeOrientation = _rot;
this->dataPtr->worldFrameRelativeTo = _relativeTo;

// Set orientation reference frame if custom_rpy was supplied
if (this->dataPtr->sensorOrientationRelativeTo == WorldFrameEnumType::CUSTOM)
{
if (this->dataPtr->customRpyParentFrame == "")
{
this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
this->dataPtr->customRpyQuaternion);
}
else
{
ignwarn << "custom_rpy parent frame must be set to empty "
"string. Setting it to any other frame is not "
"supported yet." << std::endl;
}
return;
}

// Table to hold frame transformations
static const std::map<WorldFrameEnumType,
std::map<WorldFrameEnumType, ignition::math::Quaterniond>>
transformTable =
{
{WorldFrameEnumType::ENU,
{
{WorldFrameEnumType::ENU, ignition::math::Quaterniond(0, 0, 0)},
{WorldFrameEnumType::NED, ignition::math::Quaterniond(
IGN_PI, 0, IGN_PI/2)},
{WorldFrameEnumType::NWU, ignition::math::Quaterniond(
0, 0, IGN_PI/2)},
}
},
{WorldFrameEnumType::NED,
{
{WorldFrameEnumType::ENU, ignition::math::Quaterniond(
IGN_PI, 0, IGN_PI/2).Inverse()},
{WorldFrameEnumType::NED, ignition::math::Quaterniond(0, 0, 0)},
{WorldFrameEnumType::NWU, ignition::math::Quaterniond(
-IGN_PI, 0, 0)},
}
},
{WorldFrameEnumType::NWU,
{
{WorldFrameEnumType::ENU, ignition::math::Quaterniond(
0, 0, -IGN_PI/2)},
{WorldFrameEnumType::NED, ignition::math::Quaterniond(IGN_PI, 0, 0)},
{WorldFrameEnumType::NWU, ignition::math::Quaterniond(0, 0, 0)},
}
}
};

if (this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::NONE &&
this->dataPtr->sensorOrientationRelativeTo != WorldFrameEnumType::CUSTOM)
{
// A valid named localization tag is supplied in the sdf
auto tranformation =
transformTable.at(this->dataPtr->worldFrameRelativeTo).at
(this->dataPtr->sensorOrientationRelativeTo);
this->SetOrientationReference(this->dataPtr->worldRelativeOrientation *
tranformation);
}
}

//////////////////////////////////////////////////
void ImuSensor::SetOrientationReference(const math::Quaterniond &_orient)
{
Expand Down
Loading

0 comments on commit d606f8a

Please sign in to comment.