diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh new file mode 100644 index 00000000..960de5e3 --- /dev/null +++ b/include/gz/sensors/AirFlowSensor.hh @@ -0,0 +1,101 @@ +/* + * 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_AIRFLOWSENSOR_HH_ +#define GZ_SENSORS_AIRFLOWSENSOR_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 AirFlowSensorPrivate; + + /// \brief AirFlow Sensor Class + /// + /// A sensor that reports air speed through differential pressure readings. + class GZ_SENSORS_AIR_FLOW_VISIBLE AirFlowSensor : + public Sensor + { + /// \brief constructor + public: AirFlowSensor(); + + /// \brief destructor + public: virtual ~AirFlowSensor(); + + /// \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 + /// \param[in] _vel The velocity of the sensor [m/s] + public: void SetVelocity(const gz::math::Vector3d &_vel); + + /// \brief Update the wind velocity + /// \param[in] _vel The wind velocity [m/s] + public: void SetWindVelocity(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 81607c9b..fe03d7b1 100644 --- a/include/gz/sensors/SensorTypes.hh +++ b/include/gz/sensors/SensorTypes.hh @@ -209,6 +209,14 @@ namespace gz /// \sa AirSpeedSensor AIR_SPEED_NOISE_PASCALS = 25, + /// \brief Air flow noise streams for the air flow sensor + /// \sa AirFlowSensor + AIR_FLOW_SPEED_NOISE = 26, + + /// \brief Air flow noise streams for the air flow sensor + /// \sa AirFlowSensor + AIR_FLOW_DIR_NOISE = 27, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc new file mode 100644 index 00000000..e68699a3 --- /dev/null +++ b/src/AirFlowSensor.cc @@ -0,0 +1,227 @@ +/* + * 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/AirFlowSensor.hh" + +using namespace gz; +using namespace sensors; + +/// \brief Private data for AirFlowSensor +class gz::sensors::AirFlowSensorPrivate +{ + /// \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 Velocity of the sensor + public: gz::math::Vector3d vel; + + /// \brief Velocity of the wind + public: gz::math::Vector3d wind_vel; + + /// \brief Noise added to speed measurement + public: std::map speed_noises; + + /// \brief Noise added to directional measurement + public: std::map dir_noises; +}; + +////////////////////////////////////////////////// +AirFlowSensor::AirFlowSensor() + : dataPtr(new AirFlowSensorPrivate()) +{ +} + +////////////////////////////////////////////////// +AirFlowSensor::~AirFlowSensor() +{ +} + +////////////////////////////////////////////////// +bool AirFlowSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool AirFlowSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::AIR_FLOW) + { + gzerr << "Attempting to a load an AirFlow sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.AirFlowSensor() == nullptr) + { + gzerr << "Attempting to a load an AirFlow sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/air_flow"); + + 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 << "Airflow for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + + // Load the noise parameters + if (_sdf.AirFlowSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE) + { + this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE] = + NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->SpeedNoise()); + } + + if (_sdf.AirFlowSensor()->DirectionNoise().Type() != sdf::NoiseType::NONE) + { + this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE] = + NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->DirectionNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool AirFlowSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool AirFlowSensor::Update( + const std::chrono::steady_clock::duration &_now) +{ + GZ_PROFILE("AirFlowSensor::Update"); + if (!this->dataPtr->initialized) + { + gzerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::AirFlowSensor 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()); + + math::Vector3d wind_vel_ = this->dataPtr->wind_vel; + 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_); + + // airflow flowing in to the sensor is measured as 0. Hense the substraction + // of M_2_PI. + double airflow_direction_in_xy_plane = atan2f(air_vel_in_body_.Y(), + air_vel_in_body_.X()) - M_PI_2; + + // Apply noise + if (this->dataPtr->dir_noises.find(AIR_FLOW_DIR_NOISE) != + this->dataPtr->dir_noises.end()) + { + airflow_direction_in_xy_plane = + this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE]->Apply( + airflow_direction_in_xy_plane); + msg.mutable_direction_noise()->set_type(msgs::SensorNoise::GAUSSIAN); + } + + air_vel_in_body_.Z() = 0; + double airflow_speed = air_vel_in_body_.Length(); + + // Apply noise + if (this->dataPtr->speed_noises.find(AIR_FLOW_SPEED_NOISE) != + this->dataPtr->speed_noises.end()) + { + airflow_speed = + this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE]->Apply( + airflow_speed); + msg.mutable_speed_noise()->set_type(msgs::SensorNoise::GAUSSIAN); + } + + msg.set_xy_direction(airflow_direction_in_xy_plane); + msg.set_xy_speed(airflow_speed); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +gz::math::Vector3d AirFlowSensor::Velocity() const +{ + return this->dataPtr->vel; +} + +////////////////////////////////////////////////// +void AirFlowSensor::SetVelocity(const gz::math::Vector3d &_vel) +{ + this->dataPtr->vel = _vel; +} + +void AirFlowSensor::SetWindVelocity(const gz::math::Vector3d &_vel) +{ + this->dataPtr->wind_vel = _vel; +} + +////////////////////////////////////////////////// +bool AirFlowSensor::HasConnections() const +{ + return this->dataPtr->pub && this->dataPtr->pub.HasConnections(); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7bd02827..b20ad0ce 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -135,6 +135,9 @@ gz_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) set(altimeter_sources AltimeterSensor.cc) gz_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) +set(air_flow_sources AirFlowSensor.cc) +gz_add_component(air_flow SOURCES ${air_flow_sources} GET_TARGET_NAME air_flow_target) + set(air_pressure_sources AirPressureSensor.cc) gz_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ba4a1575..a34a8e21 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(dri_tests ) set(tests + air_flow.cc air_pressure.cc air_speed.cc altimeter.cc @@ -58,6 +59,7 @@ gz_build_tests(TYPE INTEGRATION SOURCES ${tests} LIB_DEPS + ${PROJECT_LIBRARY_TARGET_NAME}-air_flow ${PROJECT_LIBRARY_TARGET_NAME}-air_pressure ${PROJECT_LIBRARY_TARGET_NAME}-air_speed ${PROJECT_LIBRARY_TARGET_NAME}-altimeter diff --git a/test/integration/air_flow.cc b/test/integration/air_flow.cc new file mode 100644 index 00000000..2dac6830 --- /dev/null +++ b/test/integration/air_flow.cc @@ -0,0 +1,265 @@ +/* + * 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 flow sdf element +sdf::ElementPtr AirFlowToSdf(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 flow sdf element with noise +sdf::ElementPtr AirFlowToSdfWithNoise(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 << "" + << " " + << " " + << " " + << " " + << " " << _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 flow sensor +class AirFlowSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(AirFlowSensorTest, CreateAirFlow) +{ + // Create SDF describing an air_pressure sensor + const std::string name = "TestAirFlow"; + const std::string topic = "/gz/sensors/test/air_flow"; + const std::string topicNoise = "/gz/sensors/test/air_flow_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 AirFlowSdf = AirFlowToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr AirFlowSdfNoise = AirFlowToSdfWithNoise(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(AirFlowSdf); + 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( + AirFlowSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + EXPECT_EQ(name, sensorNoise->Name()); + EXPECT_EQ(topicNoise, sensorNoise->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(AirFlowSensorTest, SensorReadings) +{ + // Create SDF describing an air_flow sensor + const std::string name = "TestAirFlow"; + const std::string topic = "/gz/sensors/test/air_flow"; + const std::string topicNoise = "/gz/sensors/test/air_flow_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 AirFlowSdf = AirFlowToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr AirFlowSdfNoise = AirFlowToSdfWithNoise(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( + AirFlowSdf); + ASSERT_NE(nullptr, sensor); + EXPECT_FALSE(sensor->HasConnections()); + + auto sensorNoise = sf.CreateSensor( + AirFlowSdfNoise); + 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.xy_speed()); + EXPECT_DOUBLE_EQ(0.0, msg.xy_direction()); + + // 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.xy_speed())); + EXPECT_FALSE(gz::math::equal(0.0, msgNoise.xy_direction())); +} + +///////////////////////////////////////////////// +TEST_F(AirFlowSensorTest, Topic) +{ + const std::string name = "TestAirFlow"; + 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 AirFlowSdf = AirFlowToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto AirFlow = factory.CreateSensor< + gz::sensors::AirFlowSensor>(AirFlowSdf); + ASSERT_NE(nullptr, AirFlow); + + EXPECT_EQ("/air_flow", AirFlow->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto AirFlowSdf = AirFlowToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto AirFlow = factory.CreateSensor< + gz::sensors::AirFlowSensor>(AirFlowSdf); + ASSERT_NE(nullptr, AirFlow); + + EXPECT_EQ("/topic_with_spaces/characters", AirFlow->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto AirFlowSdf = AirFlowToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor< + gz::sensors::AirFlowSensor>(AirFlowSdf); + ASSERT_EQ(nullptr, sensor); + } +}