diff --git a/include/sdf/AirFlow.hh b/include/sdf/AirFlow.hh new file mode 100644 index 000000000..d8573bdc4 --- /dev/null +++ b/include/sdf/AirFlow.hh @@ -0,0 +1,93 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef SDF_AIRFLOW_HH_ +#define SDF_AIRFLOW_HH_ + +#include + +#include +#include +#include +#include + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief AirFlow contains information about a general + /// purpose air flow sensor. + /// This sensor can be attached to a link. + class SDFORMAT_VISIBLE AirFlow + { + /// \brief Default constructor + public: AirFlow(); + + /// \brief Load the air flow based on an element pointer. + /// This is *not* the usual entry point. Typical usage of the SDF DOM is + /// through the Root object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the noise values. + /// \return Noise values for differential pressure data. + public: const Noise &SpeedNoise() const; + + /// \brief Set the noise values related to the differential pressure data. + /// \param[in] _noise Noise values for the pressure data. + public: void SetSpeedNoise(const Noise &_noise);\ + + /// \brief Get the noise values. + /// \return Noise values for differential pressure data. + public: const Noise &DirectionNoise() const; + + /// \brief Set the noise values related to the differential pressure data. + /// \param[in] _noise Noise values for the pressure data. + public: void SetDirectionNoise(const Noise &_noise); + + /// \brief Return true if both AirFlow objects contain the + /// same values. + /// \param[_in] _air AirFlow value to compare. + /// \returen True if 'this' == _air. + public: bool operator==(const AirFlow &_air) const; + + /// \brief Return true this AirFlow object does not contain + /// the same values as the passed in parameter. + /// \param[_in] _air AirFlow value to compare. + /// \returen True if 'this' != _air. + public: bool operator!=(const AirFlow &_air) const; + + /// \brief Create and return an SDF element filled with data from this + /// air pressure sensor. + /// Note that parameter passing functionality is not captured with this + /// function. + /// \return SDF element pointer with updated sensor values. + public: sdf::ElementPtr ToElement() const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } +} +#endif // SDF_AIRFLOW_HH_ diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 0b998c1f1..a36d64697 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -35,6 +35,7 @@ namespace sdf // // Forward declarations. + class AirFlow; class AirPressure; class AirSpeed; class Altimeter; @@ -134,6 +135,9 @@ namespace sdf /// \brief An air speed sensor. AIR_SPEED = 26, + + /// \brief An airflow sensor. + AIR_FLOW = 27, }; /// \brief Information about an SDF sensor. @@ -341,6 +345,24 @@ namespace sdf /// \param[in] _air The air pressure sensor. public: void SetAirSpeedSensor(const AirSpeed &_air); + /// \brief Get the air flow sensor, or nullptr if this sensor type + /// is not an AirFlow sensor. + /// \return Pointer to the AirFlow sensor, or nullptr if this + /// Sensor is not a AirFlow sensor. + /// \sa SensorType Type() const + public: const AirFlow *AirFlowSensor() const; + + /// \brief Get a mutable air flow sensor, or nullptr if this sensor type + /// is not an AirFlow sensor. + /// \return Pointer to the AirFlow sensor, or nullptr if this + /// Sensor is not a AirFlow sensor. + /// \sa SensorType Type() const + public: AirFlow *AirFlowSensor(); + + /// \brief Set the air pressure sensor. + /// \param[in] _air The air pressure sensor. + public: void SetAirFlowSensor(const AirFlow &_air); + /// \brief Set the camera sensor. /// \param[in] _cam The camera sensor. public: void SetCameraSensor(const Camera &_cam); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index a84167f70..1134c1c8d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -44,6 +44,7 @@ endfunction() set(BINDINGS_MODULE_NAME "pysdformat${PROJECT_VERSION_MAJOR}") pybind11_add_module(${BINDINGS_MODULE_NAME} MODULE src/sdf/_gz_sdformat_pybind11.cc + src/sdf/pyAirFlow.cc src/sdf/pyAirPressure.cc src/sdf/pyAirSpeed.cc src/sdf/pyAltimeter.cc @@ -123,6 +124,7 @@ if (BUILD_TESTING AND NOT WIN32) ) set(python_tests + pyAirFlow_TEST pyAirPressure_TEST pyAirSpeed_TEST pyAltimeter_TEST diff --git a/python/src/sdf/_gz_sdformat_pybind11.cc b/python/src/sdf/_gz_sdformat_pybind11.cc index a6f587193..95c069281 100644 --- a/python/src/sdf/_gz_sdformat_pybind11.cc +++ b/python/src/sdf/_gz_sdformat_pybind11.cc @@ -18,6 +18,7 @@ #include #include +#include "pyAirFlow.hh" #include "pyAirPressure.hh" #include "pyAirSpeed.hh" #include "pyAltimeter.hh" @@ -74,6 +75,7 @@ PYBIND11_MODULE(BINDINGS_MODULE_NAME, m) { std::string("gz.math") + std::to_string(GZ_MATH_MAJOR_VERSION); pybind11::module::import(gzMathModule.c_str()); + sdf::python::defineAirFlow(m); sdf::python::defineAirPressure(m); sdf::python::defineAirSpeed(m); sdf::python::defineAltimeter(m); diff --git a/python/src/sdf/pyAirFlow.cc b/python/src/sdf/pyAirFlow.cc new file mode 100644 index 000000000..9519a74a7 --- /dev/null +++ b/python/src/sdf/pyAirFlow.cc @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "pyAirFlow.hh" + +#include +#include + +#include "sdf/AirFlow.hh" + +using namespace pybind11::literals; + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +///////////////////////////////////////////////// +void defineAirFlow(pybind11::object module) +{ + pybind11::class_ geometryModule(module, "AirFlow"); + geometryModule + .def(pybind11::init<>()) + .def(pybind11::init()) + .def(pybind11::self == pybind11::self) + .def(pybind11::self != pybind11::self) + .def("speed_noise", &sdf::AirFlow::SpeedNoise, + "Get the speed noise values.") + .def("set_speed_noise", + &sdf::AirFlow::SetSpeedNoise, + "Set the noise values related to the speed data.") + .def("direction_noise", &sdf::AirFlow::DirectionNoise, + "Get the direction noise values.") + .def("set_direction_noise", + &sdf::AirFlow::SetDirectionNoise, + "Set the noise values related to the direction data.") + .def("__copy__", [](const sdf::AirFlow &self) { + return sdf::AirFlow(self); + }) + .def("__deepcopy__", [](const sdf::AirFlow &self, pybind11::dict) { + return sdf::AirFlow(self); + }, "memo"_a); +} +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf diff --git a/python/src/sdf/pyAirFlow.hh b/python/src/sdf/pyAirFlow.hh new file mode 100644 index 000000000..796b3c09a --- /dev/null +++ b/python/src/sdf/pyAirFlow.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SDFORMAT_PYTHON_AIRFLOW_HH_ +#define SDFORMAT_PYTHON_AIRFLOW_HH_ + +#include + +#include "sdf/AirFlow.hh" + +#include "sdf/config.hh" + +namespace sdf +{ +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +namespace python +{ +/// Define a pybind11 wrapper for an sdf::AirFlow +/** + * \param[in] module a pybind11 module to add the definition to + */ +void defineAirFlow(pybind11::object module); +} // namespace python +} // namespace SDF_VERSION_NAMESPACE +} // namespace sdf + +#endif // SDFORMAT_PYTHON_AirFlow_HH_ diff --git a/python/test/pyAirFlow_TEST.py b/python/test/pyAirFlow_TEST.py new file mode 100644 index 000000000..12c70f2f9 --- /dev/null +++ b/python/test/pyAirFlow_TEST.py @@ -0,0 +1,68 @@ +# Copyright (C) 2023 Open Source Robotics Foundation + +# Licensed under the Apache License, Version 2.0 (the "License") +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at + +# http://www.apache.org/licenses/LICENSE-2.0 + +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from gz_test_deps.sdformat import AirFlow, Noise +import gz_test_deps.sdformat as sdf +import unittest + +class AirFlowTEST(unittest.TestCase): + + + def test_default_construction(self): + airflow = AirFlow() + defaultNoise = Noise() + self.assertEqual(defaultNoise, airflow.direction_noise()) + self.assertEqual(defaultNoise, airflow.speed_noise()) + + def test_set(self): + air = AirFlow() + defaultNoise = Noise() + dir_noise = Noise() + speed_noise = Noise() + self.assertEqual(defaultNoise, air.speed_noise()) + self.assertEqual(defaultNoise, air.direction_noise()) + + dir_noise.set_type(sdf.NoiseType.GAUSSIAN) + dir_noise.set_mean(1.2) + dir_noise.set_std_dev(2.3) + dir_noise.set_bias_mean(4.5) + dir_noise.set_bias_std_dev(6.7) + dir_noise.set_precision(8.9) + + speed_noise.set_type(sdf.NoiseType.GAUSSIAN) + speed_noise.set_mean(1.2) + speed_noise.set_std_dev(2.3) + speed_noise.set_bias_mean(4.5) + speed_noise.set_bias_std_dev(6.7) + speed_noise.set_precision(8.9) + + air.set_direction_noise(dir_noise) + air.set_speed_noise(speed_noise) + self.assertEqual(dir_noise, air.direction_noise()) + self.assertEqual(speed_noise, air.speed_noise()) + + # Copy Constructor + air2 = AirFlow(air) + self.assertEqual(air, air2) + + # Copy operator + air3 = air + self.assertEqual(air, air3) + + air4 = AirFlow() + self.assertNotEqual(air3, air4); + + +if __name__ == '__main__': + unittest.main() diff --git a/sdf/1.10/CMakeLists.txt b/sdf/1.10/CMakeLists.txt index 531202e38..1a1ebaca5 100644 --- a/sdf/1.10/CMakeLists.txt +++ b/sdf/1.10/CMakeLists.txt @@ -1,5 +1,6 @@ set (sdfs actor.sdf + air_flow.sdf air_pressure.sdf air_speed.sdf altimeter.sdf diff --git a/sdf/1.10/air_flow.sdf b/sdf/1.10/air_flow.sdf new file mode 100644 index 000000000..35dabb0e7 --- /dev/null +++ b/sdf/1.10/air_flow.sdf @@ -0,0 +1,18 @@ + + These elements are specific to an air flow sensor. This sensor emulates an ultrasonic airflow sensor which determines the airspeed and direction based on the doppler effect. + + + + Noise parameters for the speed data. + + + + + + + Noise parameters for the direction data. + + + + + diff --git a/sdf/1.10/sensor.sdf b/sdf/1.10/sensor.sdf index 78954738b..0effcb41a 100644 --- a/sdf/1.10/sensor.sdf +++ b/sdf/1.10/sensor.sdf @@ -8,6 +8,7 @@ The type name of the sensor. By default, SDFormat supports types + air_flow, air_pressure, air_speed, altimeter, @@ -61,6 +62,7 @@ + diff --git a/src/AirFlow.cc b/src/AirFlow.cc new file mode 100644 index 000000000..c6dac70b9 --- /dev/null +++ b/src/AirFlow.cc @@ -0,0 +1,140 @@ +/* + * Copyright 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "sdf/AirFlow.hh" +#include "sdf/parser.hh" + +using namespace sdf; + +/// \brief Private AirFlow data. +class sdf::AirFlow::Implementation +{ + /// \brief The speed noise. + public: Noise speed_noise; + + /// \brief The direction noise. + public: Noise direction_noise; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf; +}; + +////////////////////////////////////////////////// +AirFlow::AirFlow() + : dataPtr(gz::utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +Errors AirFlow::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "air_flow") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load an Air Pressure Sensor, but the provided SDF " + "element is not a ."}); + return errors; + } + + // Load the noise values. + if (_sdf->HasElement("speed")) + { + sdf::ElementPtr elem = _sdf->GetElement("speed"); + if (elem->HasElement("noise")) + this->dataPtr->speed_noise.Load(elem->GetElement("noise")); + } + + // Load the noise values. + if (_sdf->HasElement("direction")) + { + sdf::ElementPtr elem = _sdf->GetElement("direction"); + if (elem->HasElement("noise")) + this->dataPtr->direction_noise.Load(elem->GetElement("noise")); + } + + return errors; +} + +////////////////////////////////////////////////// +sdf::ElementPtr AirFlow::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +bool AirFlow::operator!=(const AirFlow &_air) const +{ + return !(*this == _air); +} + +////////////////////////////////////////////////// +bool AirFlow::operator==(const AirFlow &_air) const +{ + bool speed_noise = this->dataPtr->speed_noise == + _air.dataPtr->speed_noise; + bool dir_noise = this->dataPtr->direction_noise == + _air.dataPtr->direction_noise; + + return speed_noise && dir_noise; +} + +////////////////////////////////////////////////// +const Noise &AirFlow::SpeedNoise() const +{ + return this->dataPtr->speed_noise; +} + +////////////////////////////////////////////////// +void AirFlow::SetSpeedNoise(const Noise &_noise) +{ + this->dataPtr->speed_noise = _noise; +} + +////////////////////////////////////////////////// +const Noise &AirFlow::DirectionNoise() const +{ + return this->dataPtr->direction_noise; +} + +////////////////////////////////////////////////// +void AirFlow::SetDirectionNoise(const Noise &_noise) +{ + this->dataPtr->direction_noise = _noise; +} + +///////////////////////////////////////////////// +sdf::ElementPtr AirFlow::ToElement() const +{ + sdf::ElementPtr elem(new sdf::Element); + sdf::initFile("air_flow.sdf", elem); + + sdf::ElementPtr speedElem = elem->GetElement("speed"); + sdf::ElementPtr speedNoiseElem = speedElem->GetElement("noise"); + speedNoiseElem->Copy(this->dataPtr->speed_noise.ToElement()); + + sdf::ElementPtr directionElem = elem->GetElement("direction"); + sdf::ElementPtr directionNoiseElem = directionElem->GetElement("noise"); + directionNoiseElem->Copy(this->dataPtr->direction_noise.ToElement()); + + return elem; +} diff --git a/src/Airflow_TEST.cc b/src/Airflow_TEST.cc new file mode 100644 index 000000000..f604f2177 --- /dev/null +++ b/src/Airflow_TEST.cc @@ -0,0 +1,155 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include "sdf/AirFlow.hh" +#include "sdf/Noise.hh" + +///////////////////////////////////////////////// +TEST(DOMAirFlow, Construction) +{ + sdf::AirFlow alt; + sdf::Noise defaultNoise; + EXPECT_EQ(defaultNoise, alt.DirectionNoise()); +} + +///////////////////////////////////////////////// +TEST(DOMAirFlow, Set) +{ + sdf::AirFlow alt; + sdf::Noise defaultNoise, dir_noise, speed_noise; + EXPECT_EQ(defaultNoise, alt.DirectionNoise()); + EXPECT_EQ(defaultNoise, alt.SpeedNoise()); + + speed_noise.SetType(sdf::NoiseType::GAUSSIAN); + speed_noise.SetMean(1.2); + speed_noise.SetStdDev(2.3); + speed_noise.SetBiasMean(4.5); + speed_noise.SetBiasStdDev(6.7); + speed_noise.SetPrecision(8.9); + + dir_noise.SetType(sdf::NoiseType::GAUSSIAN); + dir_noise.SetMean(1.2); + dir_noise.SetStdDev(2.3); + dir_noise.SetBiasMean(4.5); + dir_noise.SetBiasStdDev(6.7); + dir_noise.SetPrecision(8.9); + + alt.SetDirectionNoise(dir_noise); + EXPECT_EQ(dir_noise, alt.DirectionNoise()); + + alt.SetSpeedNoise(speed_noise); + EXPECT_EQ(speed_noise, alt.SpeedNoise()); + + // Copy Constructor + sdf::AirFlow alt2(alt); + EXPECT_EQ(alt, alt2); + + // Copy operator + sdf::AirFlow alt3; + alt3 = alt; + EXPECT_EQ(alt, alt3); + + // Move Constructor + sdf::AirFlow alt4(std::move(alt)); + EXPECT_EQ(alt2, alt4); + + alt = alt4; + EXPECT_EQ(alt2, alt); + + // Move operator + sdf::AirFlow alt5; + alt5 = std::move(alt2); + EXPECT_EQ(alt3, alt5); + + alt2 = alt5; + EXPECT_EQ(alt3, alt2); + + // inequality + sdf::AirFlow alt6; + EXPECT_NE(alt3, alt6); + alt6.SetDirectionNoise(alt3.DirectionNoise()); + EXPECT_NE(alt3, alt6); + alt6.SetSpeedNoise(alt3.SpeedNoise()); + EXPECT_EQ(alt3, alt6); +} + +///////////////////////////////////////////////// +TEST(DOMAirFlow, Load) +{ + sdf::ElementPtr sdf(std::make_shared()); + + // No element + sdf::AirFlow alt; + sdf::Errors errors = alt.Load(sdf); + ASSERT_FALSE(errors.empty()); + EXPECT_TRUE(errors[0].Message().find("is not a ") + != std::string::npos) << errors[0].Message(); + + EXPECT_NE(nullptr, alt.Element()); + EXPECT_EQ(sdf.get(), alt.Element().get()); + + // The AirFlow::Load function is test more thouroughly in the + // link_dom.cc integration test. +} + +///////////////////////////////////////////////// +TEST(DOMAirFlow, ToElement) +{ + // test calling ToElement on a DOM object constructed without calling Load + sdf::AirFlow alt; + sdf::Noise defaultNoise, dir_noise, speed_noise; + EXPECT_EQ(defaultNoise, alt.DirectionNoise()); + + dir_noise.SetType(sdf::NoiseType::GAUSSIAN); + dir_noise.SetMean(1.2); + dir_noise.SetStdDev(2.3); + dir_noise.SetBiasMean(4.5); + dir_noise.SetBiasStdDev(6.7); + dir_noise.SetPrecision(8.9); + + speed_noise.SetType(sdf::NoiseType::GAUSSIAN); + speed_noise.SetMean(1.2); + speed_noise.SetStdDev(2.3); + speed_noise.SetBiasMean(4.5); + speed_noise.SetBiasStdDev(6.7); + speed_noise.SetPrecision(8.9); + + alt.SetDirectionNoise(dir_noise); + alt.SetSpeedNoise(speed_noise); + + sdf::ElementPtr altElem = alt.ToElement(); + EXPECT_NE(nullptr, altElem); + EXPECT_EQ(nullptr, alt.Element()); + + // verify values after loading the element back + sdf::AirFlow alt2; + alt2.Load(altElem); + + EXPECT_EQ(dir_noise, alt2.DirectionNoise()); + EXPECT_EQ(speed_noise, alt2.SpeedNoise()); + + // make changes to DOM and verify ToElement produces updated values + dir_noise.SetMean(2.3); + alt2.SetDirectionNoise(dir_noise); + sdf::ElementPtr alt2Elem = alt2.ToElement(); + EXPECT_NE(nullptr, alt2Elem); + sdf::AirFlow alt3; + alt3.Load(alt2Elem); + EXPECT_EQ(dir_noise, alt3.DirectionNoise()); + EXPECT_EQ(speed_noise, alt3.SpeedNoise()); +} diff --git a/src/Sensor.cc b/src/Sensor.cc index 6c6dde9d6..06a8a9257 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -19,6 +19,7 @@ #include #include #include +#include "sdf/AirFlow.hh" #include "sdf/AirPressure.hh" #include "sdf/AirSpeed.hh" #include "sdf/Altimeter.hh" @@ -68,7 +69,8 @@ const std::vector sensorTypeStrs = "boundingbox_camera", "custom", "wide_angle_camera", - "air_speed" + "air_speed", + "air_flow" }; class sdf::Sensor::Implementation @@ -115,6 +117,9 @@ class sdf::Sensor::Implementation /// \brief Optional air pressure sensor. public: std::optional airSpeed; + /// \brief Optional air flow sensor. + public: std::optional airFlow; + /// \brief Optional camera. public: std::optional camera; @@ -171,6 +176,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); case SensorType::AIR_SPEED: return *(this->dataPtr->airSpeed) == *(_sensor.dataPtr->airSpeed); + case SensorType::AIR_FLOW: + return *(this->dataPtr->airFlow) == *(_sensor.dataPtr->airFlow); case SensorType::FORCE_TORQUE: return *(this->dataPtr->forceTorque) == *(_sensor.dataPtr->forceTorque); case SensorType::IMU: @@ -258,6 +265,14 @@ Errors Sensor::Load(ElementPtr _sdf) _sdf->GetElement("air_pressure")); errors.insert(errors.end(), err.begin(), err.end()); } + else if (type == "air_flow") + { + this->dataPtr->type = SensorType::AIR_FLOW; + this->dataPtr->airFlow.emplace(); + Errors err = this->dataPtr->airFlow->Load( + _sdf->GetElement("air_flow")); + errors.insert(errors.end(), err.begin(), err.end()); + } else if (type == "air_speed") { this->dataPtr->type = SensorType::AIR_SPEED; @@ -593,6 +608,24 @@ void Sensor::SetAirPressureSensor(const AirPressure &_air) this->dataPtr->airPressure = _air; } +///////////////////////////////////////////////// +const AirFlow *Sensor::AirFlowSensor() const +{ + return optionalToPointer(this->dataPtr->airFlow); +} + +///////////////////////////////////////////////// +AirFlow *Sensor::AirFlowSensor() +{ + return optionalToPointer(this->dataPtr->airFlow); +} + +///////////////////////////////////////////////// +void Sensor::SetAirFlowSensor(const AirFlow &_air) +{ + this->dataPtr->airFlow = _air; +} + ///////////////////////////////////////////////// const AirSpeed *Sensor::AirSpeedSensor() const { @@ -758,6 +791,13 @@ sdf::ElementPtr Sensor::ToElement(sdf::Errors &_errors) const sdf::ElementPtr airPressureElem = elem->GetElement("air_pressure"); airPressureElem->Copy(this->dataPtr->airPressure->ToElement()); } + // air flow + else if (this->Type() == sdf::SensorType::AIR_FLOW && + this->dataPtr->airFlow) + { + sdf::ElementPtr airFlowElem = elem->GetElement("air_flow"); + airFlowElem->Copy(this->dataPtr->airFlow->ToElement()); + } // air speed else if (this->Type() == sdf::SensorType::AIR_SPEED && this->dataPtr->airSpeed) diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 9917067d5..08f5f5dc7 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -20,6 +20,7 @@ #include #include +#include "sdf/AirFlow.hh" #include "sdf/AirPressure.hh" #include "sdf/AirSpeed.hh" #include "sdf/Altimeter.hh" @@ -285,7 +286,7 @@ TEST(DOMLink, Sensors) const sdf::Link *link = model->LinkByIndex(0); ASSERT_NE(nullptr, link); EXPECT_EQ("link", link->Name()); - EXPECT_EQ(27u, link->SensorCount()); + EXPECT_EQ(28u, link->SensorCount()); // Get the altimeter sensor const sdf::Sensor *altimeterSensor = link->SensorByIndex(0); @@ -717,6 +718,22 @@ TEST(DOMLink, Sensors) EXPECT_DOUBLE_EQ(0.0, airSpeedSensorSDF->PressureNoise().Mean()); EXPECT_DOUBLE_EQ(0.01, airSpeedSensorSDF->PressureNoise().StdDev()); + // Get the air_flow sensor + const sdf::Sensor *airFlowSensor = link->SensorByName( + "air_flow_sensor"); + ASSERT_NE(nullptr, airFlowSensor); + EXPECT_EQ("air_flow_sensor", airFlowSensor->Name()); + EXPECT_EQ(sdf::SensorType::AIR_FLOW, airFlowSensor->Type()); + EXPECT_EQ(gz::math::Pose3d(2, 14, 96, 0, 0, 0), + airFlowSensor->RawPose()); + EXPECT_FALSE(airFlowSensor->EnableMetrics()); + const sdf::AirFlow *airFlowSensorSDF = airFlowSensor->AirFlowSensor(); + ASSERT_NE(nullptr, airFlowSensorSDF); + EXPECT_DOUBLE_EQ(0.0, airFlowSensorSDF->SpeedNoise().Mean()); + EXPECT_DOUBLE_EQ(0.01, airFlowSensorSDF->SpeedNoise().StdDev()); + EXPECT_DOUBLE_EQ(0.0, airFlowSensorSDF->DirectionNoise().Mean()); + EXPECT_DOUBLE_EQ(0.02, airFlowSensorSDF->DirectionNoise().StdDev()); + // Get the wide angle camera sensor EXPECT_TRUE(link->SensorNameExists("wide_angle_camera_sensor")); const sdf::Sensor *wideAngleCameraSensor = diff --git a/test/integration/sdf_dom_conversion.cc b/test/integration/sdf_dom_conversion.cc index 8a1fdb8ef..f4e15d39a 100644 --- a/test/integration/sdf_dom_conversion.cc +++ b/test/integration/sdf_dom_conversion.cc @@ -18,6 +18,7 @@ #include +#include "sdf/AirFlow.hh" #include "sdf/AirPressure.hh" #include "sdf/AirSpeed.hh" #include "sdf/Altimeter.hh" @@ -103,6 +104,29 @@ TEST(SDFDomConversion, Sensors) EXPECT_DOUBLE_EQ(0.01, airSpeedSensorSDF->PressureNoise().StdDev()); } + { + // Get the air_flow sensor + const sdf::Sensor *sensor = link->SensorByName("air_flow_sensor"); + // convert to sdf element and load it back + sdf::ElementPtr sensorElem = sensor->ToElement(); + auto airFlowSensor = std::make_unique(); + airFlowSensor->Load(sensorElem); + + ASSERT_NE(nullptr, airFlowSensor); + EXPECT_EQ("air_flow_sensor", airFlowSensor->Name()); + EXPECT_EQ(sdf::SensorType::AIR_FLOW, airFlowSensor->Type()); + EXPECT_EQ(gz::math::Pose3d(2, 14, 96, 0, 0, 0), + airFlowSensor->RawPose()); + EXPECT_FALSE(airFlowSensor->EnableMetrics()); + const sdf::AirFlow *airFlowSensorSDF = airFlowSensor->AirFlowSensor(); + ASSERT_NE(nullptr, airFlowSensorSDF); + EXPECT_DOUBLE_EQ(0.0, airFlowSensorSDF->SpeedNoise().Mean()); + EXPECT_DOUBLE_EQ(0.01, airFlowSensorSDF->SpeedNoise().StdDev()); + EXPECT_DOUBLE_EQ(0.0, airFlowSensorSDF->DirectionNoise().Mean()); + EXPECT_DOUBLE_EQ(0.02, airFlowSensorSDF->DirectionNoise().StdDev()); + + } + // camera { const sdf::Sensor *sensor = link->SensorByName("camera_sensor"); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index db67f3b69..d51805f90 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -661,6 +661,27 @@ + + 2 14 96 0 0 0 + 1 + 10.0 + false + + + + 0 + 0.01 + + + + + 0 + 0.02 + + + + + 20 30 40 0 0 0