From 9e8da4bef01026fde8a9620454f24c8e54a9a6b7 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Fri, 24 Nov 2023 12:55:52 +0200 Subject: [PATCH 1/5] Airflow Sensor Copied Airspeed Sensor and renamed to Airflow - Also already added airflow code where airspeed is found made corresponding changes to fit airflow new index number for airflow sensor Added AirPressure header back Testing for airflow sensor implemented Add noise testing for airflow sensor Python implementation for Airflow Sensor Signed-off-by: henrykotze --- include/sdf/AirFlow.hh | 93 ++++++++++++++ include/sdf/Sensor.hh | 22 ++++ python/CMakeLists.txt | 1 + python/src/sdf/_gz_sdformat_pybind11.cc | 2 + python/src/sdf/pyAirFlow.cc | 61 ++++++++++ python/src/sdf/pyAirFlow.hh | 41 +++++++ python/test/pyAirFlow_TEST.py | 67 +++++++++++ sdf/1.10/CMakeLists.txt | 1 + sdf/1.10/air_flow.sdf | 18 +++ sdf/1.10/sensor.sdf | 2 + src/AirFlow.cc | 138 +++++++++++++++++++++ src/Airflow_TEST.cc | 154 ++++++++++++++++++++++++ src/Sensor.cc | 42 ++++++- test/integration/link_dom.cc | 19 ++- test/integration/sdf_dom_conversion.cc | 24 ++++ test/sdf/sensors.sdf | 21 ++++ 16 files changed, 704 insertions(+), 2 deletions(-) create mode 100644 include/sdf/AirFlow.hh create mode 100644 python/src/sdf/pyAirFlow.cc create mode 100644 python/src/sdf/pyAirFlow.hh create mode 100644 python/test/pyAirFlow_TEST.py create mode 100644 sdf/1.10/air_flow.sdf create mode 100644 src/AirFlow.cc create mode 100644 src/Airflow_TEST.cc diff --git a/include/sdf/AirFlow.hh b/include/sdf/AirFlow.hh new file mode 100644 index 000000000..880ecd53d --- /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 diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 0b998c1f1..37a98cfc3 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 speed sensor, or nullptr if this sensor type + /// is not an AirSpeed sensor. + /// \return Pointer to the AirSpeed sensor, or nullptr if this + /// Sensor is not a AirSpeed sensor. + /// \sa SensorType Type() const + public: const AirFlow *AirFlowSensor() const; + + /// \brief Get a mutable air speed sensor, or nullptr if this sensor type + /// is not an AirSpeed sensor. + /// \return Pointer to the AirSpeed sensor, or nullptr if this + /// Sensor is not a AirSpeed 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..4261f5cb4 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 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..df96697be --- /dev/null +++ b/python/src/sdf/pyAirFlow.cc @@ -0,0 +1,61 @@ +/* + * 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("dir_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..68c2d0c92 --- /dev/null +++ b/python/test/pyAirFlow_TEST.py @@ -0,0 +1,67 @@ +# 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) + 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..6034f806c --- /dev/null +++ b/src/AirFlow.cc @@ -0,0 +1,138 @@ +/* + * 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..6b622932e --- /dev/null +++ b/src/Airflow_TEST.cc @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2019 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" + +///////////////////////////////////////////////// +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 From eabab06f66f0c002c2fb7ca5f462cce70ec2dd11 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 29 Nov 2023 13:06:43 +0200 Subject: [PATCH 2/5] codecheck passed Signed-off-by: henrykotze --- src/AirFlow.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/AirFlow.cc b/src/AirFlow.cc index 6034f806c..c6dac70b9 100644 --- a/src/AirFlow.cc +++ b/src/AirFlow.cc @@ -90,8 +90,10 @@ bool AirFlow::operator!=(const AirFlow &_air) const ////////////////////////////////////////////////// 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; + 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; } From dba65dba5706c9ec8bb6cb6ada0ade0495bc1873 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 29 Nov 2023 14:34:17 +0200 Subject: [PATCH 3/5] Fix typos Signed-off-by: henrykotze --- include/sdf/Sensor.hh | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 37a98cfc3..a36d64697 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -345,17 +345,17 @@ namespace sdf /// \param[in] _air The air pressure sensor. public: void SetAirSpeedSensor(const AirSpeed &_air); - /// \brief Get the air speed sensor, or nullptr if this sensor type - /// is not an AirSpeed sensor. - /// \return Pointer to the AirSpeed sensor, or nullptr if this - /// Sensor is not a AirSpeed sensor. + /// \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 speed sensor, or nullptr if this sensor type - /// is not an AirSpeed sensor. - /// \return Pointer to the AirSpeed sensor, or nullptr if this - /// Sensor is not a AirSpeed sensor. + /// \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(); From 73a7ddae559a0e905b5f6a98370c5cb1f61c177e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henry=20Kotz=C3=A9?= Date: Fri, 1 Dec 2023 09:47:11 +0200 Subject: [PATCH 4/5] Apply suggestions from code review MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Henry Kotzé --- include/sdf/AirFlow.hh | 6 +++--- python/src/sdf/pyAirFlow.cc | 1 - src/Airflow_TEST.cc | 3 ++- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/sdf/AirFlow.hh b/include/sdf/AirFlow.hh index 880ecd53d..d8573bdc4 100644 --- a/include/sdf/AirFlow.hh +++ b/include/sdf/AirFlow.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef SDF_AirFlow_HH_ -#define SDF_AirFlow_HH_ +#ifndef SDF_AIRFLOW_HH_ +#define SDF_AIRFLOW_HH_ #include @@ -90,4 +90,4 @@ namespace sdf }; } } -#endif +#endif // SDF_AIRFLOW_HH_ diff --git a/python/src/sdf/pyAirFlow.cc b/python/src/sdf/pyAirFlow.cc index df96697be..88144c54a 100644 --- a/python/src/sdf/pyAirFlow.cc +++ b/python/src/sdf/pyAirFlow.cc @@ -21,7 +21,6 @@ #include "sdf/AirFlow.hh" - using namespace pybind11::literals; namespace sdf diff --git a/src/Airflow_TEST.cc b/src/Airflow_TEST.cc index 6b622932e..f604f2177 100644 --- a/src/Airflow_TEST.cc +++ b/src/Airflow_TEST.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -17,6 +17,7 @@ #include #include "sdf/AirFlow.hh" +#include "sdf/Noise.hh" ///////////////////////////////////////////////// TEST(DOMAirFlow, Construction) From 9560bfc6b412f8d5dbdce008ef22656a4d8dc6c2 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Fri, 1 Dec 2023 19:29:27 +0200 Subject: [PATCH 5/5] Add python Airflow Test Signed-off-by: henrykotze --- python/CMakeLists.txt | 1 + python/src/sdf/pyAirFlow.cc | 2 +- python/test/pyAirFlow_TEST.py | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 4261f5cb4..1134c1c8d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -124,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/pyAirFlow.cc b/python/src/sdf/pyAirFlow.cc index 88144c54a..9519a74a7 100644 --- a/python/src/sdf/pyAirFlow.cc +++ b/python/src/sdf/pyAirFlow.cc @@ -43,7 +43,7 @@ void defineAirFlow(pybind11::object module) .def("set_speed_noise", &sdf::AirFlow::SetSpeedNoise, "Set the noise values related to the speed data.") - .def("dir_noise", &sdf::AirFlow::DirectionNoise, + .def("direction_noise", &sdf::AirFlow::DirectionNoise, "Get the direction noise values.") .def("set_direction_noise", &sdf::AirFlow::SetDirectionNoise, diff --git a/python/test/pyAirFlow_TEST.py b/python/test/pyAirFlow_TEST.py index 68c2d0c92..12c70f2f9 100644 --- a/python/test/pyAirFlow_TEST.py +++ b/python/test/pyAirFlow_TEST.py @@ -48,6 +48,7 @@ def test_set(self): 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())