From bced67030f647b1c3518622179c8784b346be02f Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 22 Nov 2023 09:45:19 +0200 Subject: [PATCH 1/8] Copies of AirSpeedSensor renamed to AirFlowSensor Signed-off-by: henrykotze --- include/gz/sensors/AirFlowSensor.hh | 96 ++++++++++++ src/AirFlowSensor.cc | 217 ++++++++++++++++++++++++++++ 2 files changed, 313 insertions(+) create mode 100644 include/gz/sensors/AirFlowSensor.hh create mode 100644 src/AirFlowSensor.cc diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh new file mode 100644 index 00000000..a27fcb83 --- /dev/null +++ b/include/gz/sensors/AirFlowSensor.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/src/AirFlowSensor.cc b/src/AirFlowSensor.cc new file mode 100644 index 00000000..741274ff --- /dev/null +++ b/src/AirFlowSensor.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(); +} From 8541dcc8871b6cd56d7a781e226c0052758179ae Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 22 Nov 2023 10:00:35 +0200 Subject: [PATCH 2/8] find/replaced AirSpeed -> AirFlow and equivalents Signed-off-by: henrykotze --- include/gz/sensors/AirFlowSensor.hh | 18 +++++----- src/AirFlowSensor.cc | 54 ++++++++++++++--------------- 2 files changed, 36 insertions(+), 36 deletions(-) diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh index a27fcb83..02c62b06 100644 --- a/include/gz/sensors/AirFlowSensor.hh +++ b/include/gz/sensors/AirFlowSensor.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef GZ_SENSORS_AIRSPEEDSENSOR_HH_ -#define GZ_SENSORS_AIRSPEEDSENSOR_HH_ +#ifndef GZ_SENSORS_AirFlowSensor_HH_ +#define GZ_SENSORS_AirFlowSensor_HH_ #include @@ -24,7 +24,7 @@ #include #include -#include +#include #include "gz/sensors/Sensor.hh" @@ -36,19 +36,19 @@ namespace gz inline namespace GZ_SENSORS_VERSION_NAMESPACE { // /// \brief forward declarations - class AirSpeedSensorPrivate; + class AirFlowSensorPrivate; - /// \brief AirSpeed Sensor Class + /// \brief AirFlow Sensor Class /// /// A sensor that reports air speed through differential pressure readings. - class GZ_SENSORS_AIR_SPEED_VISIBLE AirSpeedSensor : + class GZ_SENSORS_air_flow_VISIBLE AirFlowSensor : public Sensor { /// \brief constructor - public: AirSpeedSensor(); + public: AirFlowSensor(); /// \brief destructor - public: virtual ~AirSpeedSensor(); + public: virtual ~AirFlowSensor(); /// \brief Load the sensor based on data from an sdf::Sensor object. /// \param[in] _sdf SDF Sensor parameters. @@ -86,7 +86,7 @@ namespace gz GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING }; } diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc index 741274ff..26db3db4 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -20,7 +20,7 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif @@ -33,7 +33,7 @@ #include "gz/sensors/Noise.hh" #include "gz/sensors/SensorTypes.hh" #include "gz/sensors/SensorFactory.hh" -#include "gz/sensors/AirSpeedSensor.hh" +#include "gz/sensors/AirFlowSensor.hh" using namespace gz; using namespace sensors; @@ -50,8 +50,8 @@ 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 Private data for AirFlowSensor +class gz::sensors::AirFlowSensorPrivate { /// \brief node to create publisher public: transport::Node node; @@ -73,47 +73,47 @@ class gz::sensors::AirSpeedSensorPrivate }; ////////////////////////////////////////////////// -AirSpeedSensor::AirSpeedSensor() - : dataPtr(new AirSpeedSensorPrivate()) +AirFlowSensor::AirFlowSensor() + : dataPtr(new AirFlowSensorPrivate()) { } ////////////////////////////////////////////////// -AirSpeedSensor::~AirSpeedSensor() +AirFlowSensor::~AirFlowSensor() { } ////////////////////////////////////////////////// -bool AirSpeedSensor::Init() +bool AirFlowSensor::Init() { return this->Sensor::Init(); } ////////////////////////////////////////////////// -bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) +bool AirFlowSensor::Load(const sdf::Sensor &_sdf) { if (!Sensor::Load(_sdf)) return false; - if (_sdf.Type() != sdf::SensorType::AIR_SPEED) + if (_sdf.Type() != sdf::SensorType::air_flow) { - gzerr << "Attempting to a load an AirSpeed sensor, but received " + gzerr << "Attempting to a load an AirFlow sensor, but received " << "a " << _sdf.TypeStr() << std::endl; return false; } - if (_sdf.AirSpeedSensor() == nullptr) + if (_sdf.AirFlowSensor() == nullptr) { - gzerr << "Attempting to a load an AirSpeed sensor, but received " + gzerr << "Attempting to a load an AirFlow sensor, but received " << "a null sensor." << std::endl; return false; } if (this->Topic().empty()) - this->SetTopic("/air_speed"); + this->SetTopic("/air_flow"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) @@ -126,10 +126,10 @@ bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) << this->Topic() << "]" << std::endl; // Load the noise parameters - if (_sdf.AirSpeedSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) + if (_sdf.AirFlowSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) { - this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS] = - NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->PressureNoise()); + this->dataPtr->noises[air_flow_NOISE_PASCALS] = + NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->PressureNoise()); } this->dataPtr->initialized = true; @@ -137,7 +137,7 @@ bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) } ////////////////////////////////////////////////// -bool AirSpeedSensor::Load(sdf::ElementPtr _sdf) +bool AirFlowSensor::Load(sdf::ElementPtr _sdf) { sdf::Sensor sdfSensor; sdfSensor.Load(_sdf); @@ -145,17 +145,17 @@ bool AirSpeedSensor::Load(sdf::ElementPtr _sdf) } ////////////////////////////////////////////////// -bool AirSpeedSensor::Update( +bool AirFlowSensor::Update( const std::chrono::steady_clock::duration &_now) { - GZ_PROFILE("AirSpeedSensor::Update"); + GZ_PROFILE("AirFlowSensor::Update"); if (!this->dataPtr->initialized) { gzerr << "Not initialized, update ignored.\n"; return false; } - msgs::AirSpeedSensor msg; + msgs::AirFlowSensor msg; *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); @@ -179,11 +179,11 @@ bool AirSpeedSensor::Update( * air_vel_in_body_.X() * air_vel_in_body_.X(); // Apply pressure noise - if (this->dataPtr->noises.find(AIR_SPEED_NOISE_PASCALS) != + if (this->dataPtr->noises.find(air_flow_NOISE_PASCALS) != this->dataPtr->noises.end()) { diff_pressure = - this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( + this->dataPtr->noises[air_flow_NOISE_PASCALS]->Apply( diff_pressure); msg.mutable_pressure_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } @@ -199,19 +199,19 @@ bool AirSpeedSensor::Update( } ////////////////////////////////////////////////// -gz::math::Vector3d AirSpeedSensor::Velocity() const +gz::math::Vector3d AirFlowSensor::Velocity() const { return this->dataPtr->vel; } ////////////////////////////////////////////////// -void AirSpeedSensor::SetVelocity(const gz::math::Vector3d &_vel) +void AirFlowSensor::SetVelocity(const gz::math::Vector3d &_vel) { this->dataPtr->vel = _vel; } ////////////////////////////////////////////////// -bool AirSpeedSensor::HasConnections() const +bool AirFlowSensor::HasConnections() const { return this->dataPtr->pub && this->dataPtr->pub.HasConnections(); } From 25ed4681f345a7704cf2be95368e7bf600484c4f Mon Sep 17 00:00:00 2001 From: henrykotze Date: Fri, 24 Nov 2023 12:43:56 +0200 Subject: [PATCH 3/8] added noise model to measurements - Still need to add resolution to measurement and add this to the sdf format Signed-off-by: henrykotze --- include/gz/sensors/SensorTypes.hh | 8 +++ src/AirFlowSensor.cc | 90 ++++++++++++++++++------------- 2 files changed, 61 insertions(+), 37 deletions(-) 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 index 26db3db4..997dc3d6 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -38,18 +38,6 @@ 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 AirFlowSensor class gz::sensors::AirFlowSensorPrivate { @@ -65,11 +53,20 @@ class gz::sensors::AirFlowSensorPrivate /// \brief Pressure in pascals. public: double pressure = 0.0; + /// \brief Resolution of: [deg] + public: double direction_resolution = 0.0; + + /// \brief Pressure in pascals. + public: double speed_resolution = 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; + /// \brief Noise added to speed measurement + public: std::map speed_noises; + + /// \brief Noise added to directional measurement + public: std::map dir_noises; }; ////////////////////////////////////////////////// @@ -122,14 +119,20 @@ bool AirFlowSensor::Load(const sdf::Sensor &_sdf) return false; } - gzdbg << "Air speed for [" << this->Name() << "] advertised on [" + gzdbg << "Airflow for [" << this->Name() << "] advertised on [" << this->Topic() << "]" << std::endl; // Load the noise parameters - if (_sdf.AirFlowSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) + if (_sdf.AirFlowSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE) { - this->dataPtr->noises[air_flow_NOISE_PASCALS] = - NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->PressureNoise()); + this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE_PASCALS] = + NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->SpeedNoise()); + } + + if (_sdf.AirFlowSensor()->DirectionNoise().Type() != sdf::NoiseType::NONE) + { + this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE_PASCALS] = + NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->DirectionNoise()); } this->dataPtr->initialized = true; @@ -161,35 +164,48 @@ bool AirFlowSensor::Update( 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_flow_NOISE_PASCALS) != - this->dataPtr->noises.end()) + double airflow_direction_in_xy_plane = atan2f(air_vel_in_body_.Y(), + air_vel_in_body_.X()); + + // Apply noise + if (this->dataPtr->dir_noises.find(air_flow_dir_NOISE_PASCALS) != + this->dataPtr->dir_noises.end()) { - diff_pressure = - this->dataPtr->noises[air_flow_NOISE_PASCALS]->Apply( - diff_pressure); - msg.mutable_pressure_noise()->set_type(msgs::SensorNoise::GAUSSIAN); + airflow_direction_in_xy_plane = + this->dataPtr->dir_noises[air_flow_dir_NOISE_PASCALS]->Apply( + airflow_direction_in_xy_plane); + msg.mutable_direction_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } - msg.set_diff_pressure(diff_pressure * 100.0f); - msg.set_temperature(temperature_local); + // apply resolution to sensor measurement + + + 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_PASCALS) != + this->dataPtr->speed_noises.end()) + { + airflow_speed = + this->dataPtr->speed_noises[air_flow_speed_NOISE_PASCALS]->Apply( + airflow_direction_in_xy_plane); + msg.mutable_speed_noise()->set_type(msgs::SensorNoise::GAUSSIAN); + } + + // apply resolution to sensor measurement + + + + msg.set_xy_speed(airflow_direction_in_xy_plane); + msg.set_xy_direction(airflow_speed); // publish this->AddSequence(msg.mutable_header()); From e692763999ee503ca0754e57486b49d015b1228a Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sun, 26 Nov 2023 08:54:27 +0200 Subject: [PATCH 4/8] Add Wind to influence measurement Signed-off-by: henrykotze --- include/gz/sensors/AirFlowSensor.hh | 3 +++ src/AirFlowSensor.cc | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh index 02c62b06..a7967460 100644 --- a/include/gz/sensors/AirFlowSensor.hh +++ b/include/gz/sensors/AirFlowSensor.hh @@ -71,6 +71,9 @@ namespace gz /// \brief Update the velocity of the sensor public: void SetVelocity(const gz::math::Vector3d &_vel); + /// \brief Update the wind velocity in which the sensor is + public: void SetWindVelocity(const gz::math::Vector3d &_vel); + using Sensor::Update; /// \brief Update the sensor and generate data diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc index 997dc3d6..d57246d7 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -62,6 +62,9 @@ class gz::sensors::AirFlowSensorPrivate /// \brief Velocity of the air coming from 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; @@ -164,7 +167,7 @@ bool AirFlowSensor::Update( frame->set_key("frame_id"); frame->add_value(this->FrameId()); - math::Vector3d wind_vel_{0, 0, 0}; + math::Vector3d wind_vel_ = this-dataPtr->wind_vel; math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); // calculate differential pressure + noise in hPa @@ -226,6 +229,11 @@ 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 { From 92209b621c07921b46f79feb18b189bb764f0966 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sun, 26 Nov 2023 11:12:45 +0200 Subject: [PATCH 5/8] Compiling succesfully and typos fixes Signed-off-by: henrykotze --- include/gz/sensors/AirFlowSensor.hh | 6 +++--- src/AirFlowSensor.cc | 18 +++++++++--------- src/CMakeLists.txt | 3 +++ 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh index a7967460..f3a5ec44 100644 --- a/include/gz/sensors/AirFlowSensor.hh +++ b/include/gz/sensors/AirFlowSensor.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef GZ_SENSORS_AirFlowSensor_HH_ -#define GZ_SENSORS_AirFlowSensor_HH_ +#ifndef GZ_SENSORS_AIRFLOWSENSOR_HH_ +#define GZ_SENSORS_AIRFLOWSENSOR_HH_ #include @@ -41,7 +41,7 @@ namespace gz /// \brief AirFlow Sensor Class /// /// A sensor that reports air speed through differential pressure readings. - class GZ_SENSORS_air_flow_VISIBLE AirFlowSensor : + class GZ_SENSORS_AIR_FLOW_VISIBLE AirFlowSensor : public Sensor { /// \brief constructor diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc index d57246d7..c2ea757b 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -95,7 +95,7 @@ bool AirFlowSensor::Load(const sdf::Sensor &_sdf) if (!Sensor::Load(_sdf)) return false; - if (_sdf.Type() != sdf::SensorType::air_flow) + if (_sdf.Type() != sdf::SensorType::AIR_FLOW) { gzerr << "Attempting to a load an AirFlow sensor, but received " << "a " << _sdf.TypeStr() << std::endl; @@ -128,13 +128,13 @@ bool AirFlowSensor::Load(const sdf::Sensor &_sdf) // Load the noise parameters if (_sdf.AirFlowSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE) { - this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE_PASCALS] = + 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_PASCALS] = + this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE] = NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->DirectionNoise()); } @@ -167,7 +167,7 @@ bool AirFlowSensor::Update( frame->set_key("frame_id"); frame->add_value(this->FrameId()); - math::Vector3d wind_vel_ = this-dataPtr->wind_vel; + math::Vector3d wind_vel_ = this->dataPtr->wind_vel; math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); // calculate differential pressure + noise in hPa @@ -178,11 +178,11 @@ bool AirFlowSensor::Update( air_vel_in_body_.X()); // Apply noise - if (this->dataPtr->dir_noises.find(air_flow_dir_NOISE_PASCALS) != + 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_PASCALS]->Apply( + this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE]->Apply( airflow_direction_in_xy_plane); msg.mutable_direction_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } @@ -191,14 +191,14 @@ bool AirFlowSensor::Update( air_vel_in_body_.Z() = 0; - double airflow_speed = air_vel_in_body.Length(); + double airflow_speed = air_vel_in_body_.Length(); // Apply noise - if (this->dataPtr->speed_noises.find(air_flow_speed_NOISE_PASCALS) != + 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_PASCALS]->Apply( + this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE]->Apply( airflow_direction_in_xy_plane); msg.mutable_speed_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } 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) From f2216010b65490905c9f5f65c9114d12176bbef2 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sun, 26 Nov 2023 12:28:08 +0200 Subject: [PATCH 6/8] Integration testing of airflow sensor Signed-off-by: henrykotze --- test/integration/CMakeLists.txt | 2 + test/integration/air_flow.cc | 265 ++++++++++++++++++++++++++++++++ 2 files changed, 267 insertions(+) create mode 100644 test/integration/air_flow.cc 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..c2ce0351 --- /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); + } +} From a4c1a648c7706a80956c81f8a3d9462965df7692 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Tue, 28 Nov 2023 13:59:30 +0200 Subject: [PATCH 7/8] Update to datum position Signed-off-by: henrykotze --- src/AirFlowSensor.cc | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc index c2ea757b..04dea1d9 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -174,8 +174,10 @@ bool AirFlowSensor::Update( 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()); + air_vel_in_body_.X()) - M_PI_2; // Apply noise if (this->dataPtr->dir_noises.find(AIR_FLOW_DIR_NOISE) != @@ -187,9 +189,6 @@ bool AirFlowSensor::Update( msg.mutable_direction_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } - // apply resolution to sensor measurement - - air_vel_in_body_.Z() = 0; double airflow_speed = air_vel_in_body_.Length(); @@ -199,16 +198,12 @@ bool AirFlowSensor::Update( { airflow_speed = this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE]->Apply( - airflow_direction_in_xy_plane); + airflow_speed); msg.mutable_speed_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } - // apply resolution to sensor measurement - - - - msg.set_xy_speed(airflow_direction_in_xy_plane); - msg.set_xy_direction(airflow_speed); + msg.set_xy_direction(airflow_direction_in_xy_plane); + msg.set_xy_speed(airflow_speed); // publish this->AddSequence(msg.mutable_header()); From e067253e7a406b284b2faaa552bf7447ac3d670f Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 29 Nov 2023 13:59:04 +0200 Subject: [PATCH 8/8] Formatting updates Signed-off-by: henrykotze --- include/gz/sensors/AirFlowSensor.hh | 4 +++- src/AirFlowSensor.cc | 13 ++----------- test/integration/air_flow.cc | 4 ++-- 3 files changed, 7 insertions(+), 14 deletions(-) diff --git a/include/gz/sensors/AirFlowSensor.hh b/include/gz/sensors/AirFlowSensor.hh index f3a5ec44..960de5e3 100644 --- a/include/gz/sensors/AirFlowSensor.hh +++ b/include/gz/sensors/AirFlowSensor.hh @@ -69,9 +69,11 @@ namespace gz 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 in which the sensor is + /// \brief Update the wind velocity + /// \param[in] _vel The wind velocity [m/s] public: void SetWindVelocity(const gz::math::Vector3d &_vel); using Sensor::Update; diff --git a/src/AirFlowSensor.cc b/src/AirFlowSensor.cc index 04dea1d9..e68699a3 100644 --- a/src/AirFlowSensor.cc +++ b/src/AirFlowSensor.cc @@ -50,16 +50,7 @@ class gz::sensors::AirFlowSensorPrivate /// \brief true if Load() has been called and was successful public: bool initialized = false; - /// \brief Pressure in pascals. - public: double pressure = 0.0; - - /// \brief Resolution of: [deg] - public: double direction_resolution = 0.0; - - /// \brief Pressure in pascals. - public: double speed_resolution = 0.0; - - /// \brief Velocity of the air coming from the sensor + /// \brief Velocity of the sensor public: gz::math::Vector3d vel; /// \brief Velocity of the wind @@ -67,7 +58,7 @@ class gz::sensors::AirFlowSensorPrivate /// \brief Noise added to speed measurement public: std::map speed_noises; - + /// \brief Noise added to directional measurement public: std::map dir_noises; }; diff --git a/test/integration/air_flow.cc b/test/integration/air_flow.cc index c2ce0351..2dac6830 100644 --- a/test/integration/air_flow.cc +++ b/test/integration/air_flow.cc @@ -35,7 +35,7 @@ sdf::ElementPtr AirFlowToSdf(const std::string &_name, std::ostringstream stream; stream << "" - << "" + << "" << " " << " " << " " @@ -67,7 +67,7 @@ sdf::ElementPtr AirFlowToSdfWithNoise(const std::string &_name, std::ostringstream stream; stream << "" - << "" + << "" << " " << " " << " "