diff --git a/src/ImuSensor.cc b/src/ImuSensor.cc index 25498cce..a88ed2be 100644 --- a/src/ImuSensor.cc +++ b/src/ImuSensor.cc @@ -116,6 +116,24 @@ 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"); diff --git a/src/ImuSensor_TEST.cc b/src/ImuSensor_TEST.cc index d6b4f7c8..0f4aa16d 100644 --- a/src/ImuSensor_TEST.cc +++ b/src/ImuSensor_TEST.cc @@ -28,6 +28,7 @@ #pragma warning(pop) #endif +#include #include #include #include @@ -187,7 +188,7 @@ class ImuSensor_TEST : public ::testing::Test // Documentation inherited protected: void SetUp() override { - ignition::common::Console::SetVerbosity(4); + ignition::common::Console::SetVerbosity(3); } }; @@ -390,6 +391,126 @@ TEST(ImuSensor_TEST, Orientation) } +////////////////////////////////////////////////// +TEST(ImuSensor_TEST, OrientationReference) +{ + // Create a sensor manager + ignition::sensors::Manager mgr; + + sdf::ElementPtr imuSDF; + + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " CUSTOM" + << " 1.570795 0 0" + << " " + << " " + << " 1" + << " true" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + { + imuSDF = sdf::ElementPtr(); + } + else + { + imuSDF = sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); + } + + // Create an ImuSensor + auto sensor = mgr.CreateSensor( + imuSDF); + + // Make sure the above dynamic cast worked. + ASSERT_NE(nullptr, sensor); + + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + + math::Quaterniond orientValue(math::Vector3d(-1.570795, 0, 0)); + EXPECT_EQ(orientValue, sensor->Orientation()); + +} + +////////////////////////////////////////////////// +TEST(ImuSensor_TEST, CustomRpyParentFrame) +{ + // Create a sensor manager + ignition::sensors::Manager mgr; + + sdf::ElementPtr imuSDF; + + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " imu_test" + << " 1" + << " " + << " " + << " CUSTOM" + << " " + << " 1.570795 0 0" + << " " + << " " + << " " + << " 1" + << " true" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + { + imuSDF = sdf::ElementPtr(); + } + else + { + imuSDF = sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); + } + + // Create an ImuSensor + auto sensor = mgr.CreateSensor( + imuSDF); + + // Make sure the above dynamic cast worked. + ASSERT_NE(nullptr, sensor); + + sensor->Update(std::chrono::steady_clock::duration( + std::chrono::nanoseconds(10000000))); + + // Since parent_frame is not set to empty in the sdf, + // custom_rpy will be rejected and reference orientation will + // not be set. + math::Quaterniond orientValue(math::Vector3d(-1.570795, 0, 0)); + math::Quaterniond defultOrientValue(math::Vector3d(0, 0, 0)); + ASSERT_NE(orientValue, sensor->Orientation()); + EXPECT_EQ(defultOrientValue, sensor->Orientation()); +} + ////////////////////////////////////////////////// int main(int argc, char **argv) {