diff --git a/CMakeLists.txt b/CMakeLists.txt index af7e1963..ff5634de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,7 @@ endif() #-------------------------------------- # Find gz-msgs -gz_find_package(gz-msgs9 REQUIRED) +gz_find_package(gz-msgs9 REQUIRED VERSION 9.3) set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) #-------------------------------------- diff --git a/include/gz/sensors/AirSpeedSensor.hh b/include/gz/sensors/AirSpeedSensor.hh new file mode 100644 index 00000000..a27fcb83 --- /dev/null +++ b/include/gz/sensors/AirSpeedSensor.hh @@ -0,0 +1,96 @@ +/* + * 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 GZ_SENSORS_AIRSPEEDSENSOR_HH_ +#define GZ_SENSORS_AIRSPEEDSENSOR_HH_ + +#include + +#include + +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace gz +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class AirSpeedSensorPrivate; + + /// \brief AirSpeed Sensor Class + /// + /// A sensor that reports air speed through differential pressure readings. + class GZ_SENSORS_AIR_SPEED_VISIBLE AirSpeedSensor : + public Sensor + { + /// \brief constructor + public: AirSpeedSensor(); + + /// \brief destructor + public: virtual ~AirSpeedSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Get the current velocity. + /// \return Current velocity of the sensor. + public: gz::math::Vector3d Velocity() const; + + /// \brief Update the velocity of the sensor + public: void SetVelocity(const gz::math::Vector3d &_vel); + + using Sensor::Update; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + public: virtual bool HasConnections() const override; + + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/SensorTypes.hh b/include/gz/sensors/SensorTypes.hh index b8d203df..81607c9b 100644 --- a/include/gz/sensors/SensorTypes.hh +++ b/include/gz/sensors/SensorTypes.hh @@ -205,6 +205,10 @@ namespace gz /// \sa NavSat NAVSAT_VERTICAL_VELOCITY_NOISE = 24, + /// \brief Air speed noise streams for the air speed sensor + /// \sa AirSpeedSensor + AIR_SPEED_NOISE_PASCALS = 25, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc new file mode 100644 index 00000000..741274ff --- /dev/null +++ b/src/AirSpeedSensor.cc @@ -0,0 +1,217 @@ +/* + * 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. + * +*/ + +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif +#include + +#include +#include + +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorTypes.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/AirSpeedSensor.hh" + +using namespace gz; +using namespace sensors; + +// altitude AMSL see level [m] +static constexpr auto kDefaultHomeAltAmsl = 0.0f; +// international standard atmosphere (troposphere model - valid up to 11km). +// temperature at MSL [K] (15 [C]) +static constexpr auto kTemperaturMsl = 288.15f; +// pressure at MSL [Pa] +static constexpr auto kPressureMsl = 101325.0f; +// reduction in temperature with altitude for troposphere [K/m] +static constexpr auto kLapseRate = 0.0065f; +// air density at MSL [kg/m^3] +static constexpr auto kAirDensityMsl = 1.225f; + +/// \brief Private data for AirSpeedSensor +class gz::sensors::AirSpeedSensorPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish air speed messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Pressure in pascals. + public: double pressure = 0.0; + + /// \brief Velocity of the air coming from the sensor + public: gz::math::Vector3d vel; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +AirSpeedSensor::AirSpeedSensor() + : dataPtr(new AirSpeedSensorPrivate()) +{ +} + +////////////////////////////////////////////////// +AirSpeedSensor::~AirSpeedSensor() +{ +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::AIR_SPEED) + { + gzerr << "Attempting to a load an AirSpeed sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.AirSpeedSensor() == nullptr) + { + gzerr << "Attempting to a load an AirSpeed sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/air_speed"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise( + this->Topic()); + + if (!this->dataPtr->pub) + { + gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + gzdbg << "Air speed for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + + // Load the noise parameters + if (_sdf.AirSpeedSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) + { + this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS] = + NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->PressureNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Update( + const std::chrono::steady_clock::duration &_now) +{ + GZ_PROFILE("AirSpeedSensor::Update"); + if (!this->dataPtr->initialized) + { + gzerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::AirSpeedSensor msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->FrameId()); + + // compute the air density at the local altitude / temperature + // Z-component from ENU + const float alt_rel = static_cast(this->Pose().Pos().Z()); + const float alt_amsl = kDefaultHomeAltAmsl + alt_rel; + const float temperature_local = kTemperaturMsl - kLapseRate * alt_amsl; + const float density_ratio = powf(kTemperaturMsl / temperature_local , 4.256f); + const float air_density = kAirDensityMsl / density_ratio; + + math::Vector3d wind_vel_{0, 0, 0}; + math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); + + // calculate differential pressure + noise in hPa + math::Vector3d air_vel_in_body_ = this->dataPtr->vel - + veh_q_world_to_body.RotateVectorReverse(wind_vel_); + float diff_pressure = math::sgn(air_vel_in_body_.X()) * 0.005f * air_density + * air_vel_in_body_.X() * air_vel_in_body_.X(); + + // Apply pressure noise + if (this->dataPtr->noises.find(AIR_SPEED_NOISE_PASCALS) != + this->dataPtr->noises.end()) + { + diff_pressure = + this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( + diff_pressure); + msg.mutable_pressure_noise()->set_type(msgs::SensorNoise::GAUSSIAN); + } + + msg.set_diff_pressure(diff_pressure * 100.0f); + msg.set_temperature(temperature_local); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +gz::math::Vector3d AirSpeedSensor::Velocity() const +{ + return this->dataPtr->vel; +} + +////////////////////////////////////////////////// +void AirSpeedSensor::SetVelocity(const gz::math::Vector3d &_vel) +{ + this->dataPtr->vel = _vel; +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::HasConnections() const +{ + return this->dataPtr->pub && this->dataPtr->pub.HasConnections(); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e7d771f6..7bd02827 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -137,6 +137,10 @@ gz_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimete set(air_pressure_sources AirPressureSensor.cc) gz_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) + +set(air_speed_sources AirSpeedSensor.cc) +gz_add_component(air_speed SOURCES ${air_speed_sources} GET_TARGET_NAME air_speed_target) + set(force_torque_sources ForceTorqueSensor.cc) gz_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME force_torque_target) @@ -205,4 +209,3 @@ gz_build_tests(TYPE UNIT SOURCES ${gtest_sources} LIB_DEPS ${rendering_target}) gz_build_tests(TYPE UNIT SOURCES Lidar_TEST.cc LIB_DEPS ${lidar_target}) gz_build_tests(TYPE UNIT SOURCES Camera_TEST.cc LIB_DEPS ${camera_target}) gz_build_tests(TYPE UNIT SOURCES ImuSensor_TEST.cc LIB_DEPS ${imu_target}) - diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 7702174b..aaa5b908 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(dri_tests set(tests air_pressure.cc + air_speed.cc altimeter.cc force_torque.cc imu.cc @@ -57,6 +58,7 @@ gz_build_tests(TYPE INTEGRATION ${tests} LIB_DEPS ${PROJECT_LIBRARY_TARGET_NAME}-air_pressure + ${PROJECT_LIBRARY_TARGET_NAME}-air_speed ${PROJECT_LIBRARY_TARGET_NAME}-altimeter ${PROJECT_LIBRARY_TARGET_NAME}-force_torque ${PROJECT_LIBRARY_TARGET_NAME}-imu @@ -64,4 +66,3 @@ gz_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-navsat ) - diff --git a/test/integration/air_speed.cc b/test/integration/air_speed.cc new file mode 100644 index 00000000..59b0d630 --- /dev/null +++ b/test/integration/air_speed.cc @@ -0,0 +1,258 @@ +/* + * 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 + +#include +#include +#include + +#include "test_config.hh" // NOLINT(build/include) +#include "TransportTestTools.hh" + +/// \brief Helper function to create an air speed sdf element +sdf::ElementPtr AirSpeedToSdf(const std::string &_name, + const gz::math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Helper function to create an air speed sdf element with noise +sdf::ElementPtr AirSpeedToSdfWithNoise(const std::string &_name, + const gz::math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize, double _mean, double _stddev, double _bias) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Test air speed sensor +class AirSpeedSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, CreateAirSpeed) +{ + // Create SDF describing an air_pressure sensor + const std::string name = "TestAirSpeed"; + const std::string topic = "/gz/sensors/test/air_speed"; + const std::string topicNoise = "/gz/sensors/test/air_speed_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); + sdf::ElementPtr airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr airSpeedSdfNoise = AirSpeedToSdfWithNoise(name, + sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(airSpeedSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); + + std::unique_ptr sensorNoise = + sf.CreateSensor( + airSpeedSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + EXPECT_EQ(name, sensorNoise->Name()); + EXPECT_EQ(topicNoise, sensorNoise->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, SensorReadings) +{ + // Create SDF describing an air_pressure sensor + const std::string name = "TestAirSpeed"; + const std::string topic = "/gz/sensors/test/air_pressure"; + const std::string topicNoise = "/gz/sensors/test/air_pressure_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); + sdf::ElementPtr airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr airSpeedSdfNoise = AirSpeedToSdfWithNoise(name, + sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + gz::sensors::SensorFactory sf; + auto sensor = sf.CreateSensor( + airSpeedSdf); + ASSERT_NE(nullptr, sensor); + EXPECT_FALSE(sensor->HasConnections()); + + auto sensorNoise = sf.CreateSensor( + airSpeedSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + sensor->SetPose( + gz::math::Pose3d(0, 0, 1.5, 0, 0, 0) * sensorNoise->Pose()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + EXPECT_TRUE(sensor->HasConnections()); + sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + EXPECT_DOUBLE_EQ(0.0, msg.diff_pressure()); + EXPECT_DOUBLE_EQ(288.1369934082031, msg.temperature()); + + // verify msg with noise received on the topic + WaitForMessageTestHelper + msgHelperNoise(topicNoise); + sensorNoise->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1)), false); + EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; + auto msgNoise = msgHelperNoise.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + EXPECT_FALSE(gz::math::equal(0.0, msgNoise.diff_pressure())); + EXPECT_DOUBLE_EQ(288.14675903320312, msgNoise.temperature()); +} + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, Topic) +{ + const std::string name = "TestAirSpeed"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = gz::math::Pose3d(); + + // Factory + gz::sensors::SensorFactory factory; + + // Default topic + { + const std::string topic; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto airSpeed = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_NE(nullptr, airSpeed); + + EXPECT_EQ("/air_speed", airSpeed->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto airSpeed = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_NE(nullptr, airSpeed); + + EXPECT_EQ("/topic_with_spaces/characters", airSpeed->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_EQ(nullptr, sensor); + } +}