From a7270941f8da8b2117b1f38381090be919e2bd4a Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 15:40:28 +1100 Subject: [PATCH 1/4] added GPS Sensor Signed-off-by: Dre Westcook --- .gitignore | 1 + include/ignition/sensors/GpsSensor.hh | 134 +++++++++++++ include/ignition/sensors/SensorTypes.hh | 8 + src/CMakeLists.txt | 3 + src/GpsSensor.cc | 238 ++++++++++++++++++++++++ 5 files changed, 384 insertions(+) create mode 100644 include/ignition/sensors/GpsSensor.hh create mode 100644 src/GpsSensor.cc diff --git a/.gitignore b/.gitignore index 25867e4f..5e9f4b02 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ build # Version control generated files *.orig +.vscode \ No newline at end of file diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh new file mode 100644 index 00000000..f952fd98 --- /dev/null +++ b/include/ignition/sensors/GpsSensor.hh @@ -0,0 +1,134 @@ +/* + * 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. + * +*/ +#ifndef IGNITION_SENSORS_GPS_HH_ +#define IGNITION_SENSORS_GPS_HH_ + +#include + +#include + +#include +#include + +#include +#include + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class GpsPrivate; + + /// \brief Gps Sensor Class + /// + /// An gps sensor that reports vertical position and velocity + /// readings over ign transport + class IGNITION_SENSORS_GPS_VISIBLE GpsSensor : public Sensor + { + /// \brief constructor + public: GpsSensor(); + + /// \brief destructor + public: virtual ~GpsSensor(); + + /// \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 Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \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 Set the latitude of the GPS + /// \param[in] _latitude Latitude of GPS in degrees + public: void SetLatitude(double _latitude); + + /// \brief Get the latitude of the GPS, wraped between +/- 180 + /// \return Latitude in degrees. + public: double Latitude() const; + + + /// \brief Set the longitude of the GPS + /// \param[in] _longitude Longitude of GPS in degrees + public: void SetLongitude(double _longitude); + + /// \brief Get the longitude of the GPS, wraped between +/- 180 + /// \return Longitude in degrees. + public: double Longitude() const; + + + /// \brief Set the altitude of the GPS + /// \param[in] _altitude altitude of GPS in meters + public: void SetAltitude(double _altitude); + + /// \brief GPS altitude is the GEOMETRIC altitude above sea level + /// \return Altitude in meters + public: double Altitude() const; + + + /// \brief Set the velocity of the GPS + /// \param[in] _vel vector3 of GPS in meters per second. + public: void SetVelocity(ignition::math::Vector3d _vel); + + /// \brief Get the velocity of the GPS sensor + /// \return velocity vector3 in meters per second + public: ignition::math::Vector3d Velocity(); + + + /// \brief Easy short hand for setting the lat/long of the gps. + /// \param[in] _latitude in degrees + /// \param[in] _longitude in degrees + public: void SetPosition(double _latitude, double _longitude, double _altitude = 0.0); + + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif \ No newline at end of file diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 479e671b..de3cb6a0 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -148,6 +148,14 @@ namespace ignition /// \brief Noise streams for the Lidar sensor /// \sa Lidar LIDAR_NOISE = 14, + + /// \brief Noise streams for the Lidar sensor + /// \sa Lidar + GPS_POSITION_NOISE = 15, + + /// \brief Noise streams for the Lidar sensor + /// \sa Lidar + GPS_VELOCITY_NOISE = 16, /// \internal /// \brief Indicator used to create an iterator over the enum. Do not diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 97a38024..77306c6f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -114,6 +114,9 @@ ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) +set(gps_sources GpsSensor.cc) +ign_add_component(gps SOURCES ${gps_sources} GET_TARGET_NAME gps_target) + set(air_pressure_sources AirPressureSensor.cc) ign_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) diff --git a/src/GpsSensor.cc b/src/GpsSensor.cc new file mode 100644 index 00000000..5a4de0c4 --- /dev/null +++ b/src/GpsSensor.cc @@ -0,0 +1,238 @@ +/* + * 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 +#include +#include + +#include "ignition/sensors/GpsSensor.hh" +#include "ignition/sensors/Noise.hh" +#include "ignition/sensors/SensorFactory.hh" +#include "ignition/sensors/SensorTypes.hh" + +using namespace ignition; +using namespace sensors; + +/// \brief Private data for Gps +class ignition::sensors::GpsPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish gps messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief latitude angle + public: ignition::math::Angle latitude; + + /// \brief longitude angle + public: ignition::math::Angle longitude; + + /// \brief altitude + public: double altitude = 0.0; + + /// \brief velocity + public: ignition::math::Vector3d velocity; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +GpsSensor::GpsSensor() + : dataPtr(new GpsPrivate()) +{ +} + +////////////////////////////////////////////////// +GpsSensor::~GpsSensor() +{ +} + +////////////////////////////////////////////////// +bool GpsSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool GpsSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::GPS) + { + ignerr << "Attempting to a load an GPS sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.GpsSensor() == nullptr) + { + ignerr << "Attempting to a load an GPS sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/gps"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise(this->Topic()); + + if (!this->dataPtr->pub) + { + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + // Load the noise parameters + if (_sdf.GpsSensor()->PositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->PositionNoise()); + } + if (_sdf.GpsSensor()->VelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->VelocityNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool GpsSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool GpsSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool GpsSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("GpsSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::GPS 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->Name()); + + // Apply gps position noise + if (this->dataPtr->noises.find(GPS_POSITION_NOISE) != + this->dataPtr->noises.end()) + { + + // Apply in degrees. + this->SetLatitude( + this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Latitude()) + ); + this->SetLongitude( + this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Longitude()) + ); + this->SetAltitude( + this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Altitude()) + ); + } + + // taken from ImuSensor.cc - convenience method + auto applyNoise = [&](SensorNoiseType noiseType, double &value) + { + if(this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()){ + value = this->dataPtr->noises[noiseType]->Apply(value); + } + }; + + applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.X()); + applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.Y()); + applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.Z()); + + //normalise so that it is within +/- 180 + this->dataPtr->latitude.Normalize(); + this->dataPtr->longitude.Normalize(); + + + msg.set_latitude_deg(this->dataPtr->latitude.Degree()); + msg.set_longitude_deg(this->dataPtr->longitude.Degree()); + msg.set_altitude(this->dataPtr->altitude); + msg.set_velocity_east(this->dataPtr->velocity.X()); + msg.set_velocity_north(this->dataPtr->velocity.X()); + msg.set_velocity_up(this->dataPtr->velocity.X()); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +void GpsSensor::SetLatitude(double _latitude) { + this->dataPtr->latitude.Degree(_latitude); +} + +double GpsSensor::Latitude() const { + return this->dataPtr->latitude.Degree(); +} + +void GpsSensor::SetAltitude(double _altitude) { + this->dataPtr->altitude = _altitude; +} + +double GpsSensor::Altitude() const { + return this->dataPtr->altitude; +} + +void GpsSensor::SetLongitude(double _longitude) { + this->dataPtr->longitude.Degree(_longitude); +} + +double GpsSensor::Longitude() const { + return this->dataPtr->longitude.Degree(); +} + +void GpsSensor::SetPosition(double _latitude, double _longitude, double _altitude) { + this->SetLongitude(_longitude); + this->SetLatitude(_latitude); + this->SetAltitude(_altitude); +} + +IGN_SENSORS_REGISTER_SENSOR(GpsSensor) \ No newline at end of file From e2becff75d80a2f0b4c4d1d8e8580de9f411314b Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 18:35:22 +1100 Subject: [PATCH 2/4] added velocity components Signed-off-by: Dre Westcook --- include/ignition/sensors/GpsSensor.hh | 4 ++-- src/GpsSensor.cc | 8 ++++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh index f952fd98..74292b54 100644 --- a/include/ignition/sensors/GpsSensor.hh +++ b/include/ignition/sensors/GpsSensor.hh @@ -108,11 +108,11 @@ namespace ignition /// \brief Set the velocity of the GPS /// \param[in] _vel vector3 of GPS in meters per second. - public: void SetVelocity(ignition::math::Vector3d _vel); + public: void SetVelocity(ignition::math::Vector3d &_vel); /// \brief Get the velocity of the GPS sensor /// \return velocity vector3 in meters per second - public: ignition::math::Vector3d Velocity(); + public: ignition::math::Vector3d Velocity() const; /// \brief Easy short hand for setting the lat/long of the gps. diff --git a/src/GpsSensor.cc b/src/GpsSensor.cc index 5a4de0c4..00c54c3f 100644 --- a/src/GpsSensor.cc +++ b/src/GpsSensor.cc @@ -229,6 +229,14 @@ double GpsSensor::Longitude() const { return this->dataPtr->longitude.Degree(); } +void GpsSensor::SetVelocity(ignition::math::Vector3d &_vel) { + this->dataPtr->velocity = _vel; +} + +ignition::math::Vector3d GpsSensor::Velocity() const { + return this->dataPtr->velocity; +} + void GpsSensor::SetPosition(double _latitude, double _longitude, double _altitude) { this->SetLongitude(_longitude); this->SetLatitude(_latitude); From 129096a815aa6b88b3065fb89c5038ce657bc7da Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 13:51:55 +1100 Subject: [PATCH 3/4] fixed lint errors Signed-off-by: Dre Westcook --- include/ignition/sensors/GpsSensor.hh | 5 +- include/ignition/sensors/SensorTypes.hh | 20 ++++--- src/GpsSensor.cc | 70 +++++++++++++++---------- 3 files changed, 59 insertions(+), 36 deletions(-) diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh index 74292b54..ac3c3487 100644 --- a/include/ignition/sensors/GpsSensor.hh +++ b/include/ignition/sensors/GpsSensor.hh @@ -118,7 +118,8 @@ namespace ignition /// \brief Easy short hand for setting the lat/long of the gps. /// \param[in] _latitude in degrees /// \param[in] _longitude in degrees - public: void SetPosition(double _latitude, double _longitude, double _altitude = 0.0); + public: void SetPosition(double _latitude, double _longitude, + double _altitude = 0.0); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING @@ -131,4 +132,4 @@ namespace ignition } } -#endif \ No newline at end of file +#endif diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index de3cb6a0..ab533d7a 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,13 +149,21 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, - /// \brief Noise streams for the Lidar sensor - /// \sa Lidar - GPS_POSITION_NOISE = 15, + /// \brief Noise streams for the Gps position sensor + /// \sa Gps + GPS_HORIZONTAL_POSITION_NOISE = 15, - /// \brief Noise streams for the Lidar sensor - /// \sa Lidar - GPS_VELOCITY_NOISE = 16, + /// \brief Noise streams for the Gps position sensor + /// \sa Gps + GPS_VERTICAL_POSITION_NOISE = 16, + + /// \brief Noise streams for the Gps velocity sensor + /// \sa Gps + GPS_HORIZONTAL_VELOCITY_NOISE = 17, + + /// \brief Noise streams for the Gps velocity sensor + /// \sa Gps + GPS_VERTICAL_VELOCITY_NOISE = 18, /// \internal /// \brief Indicator used to create an iterator over the enum. Do not diff --git a/src/GpsSensor.cc b/src/GpsSensor.cc index 00c54c3f..8118ece0 100644 --- a/src/GpsSensor.cc +++ b/src/GpsSensor.cc @@ -50,7 +50,7 @@ class ignition::sensors::GpsPrivate public: double altitude = 0.0; /// \brief velocity - public: ignition::math::Vector3d velocity; + public: ignition::math::Vector3d velocity; /// \brief Noise added to sensor data public: std::map noises; @@ -111,14 +111,14 @@ bool GpsSensor::Load(const sdf::Sensor &_sdf) { this->dataPtr->noises[GPS_POSITION_NOISE] = NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->PositionNoise()); + _sdf.GpsSensor()->PositionNoise()); } if (_sdf.GpsSensor()->VelocityNoise().Type() != sdf::NoiseType::NONE) { this->dataPtr->noises[GPS_VELOCITY_NOISE] = NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->VelocityNoise()); + _sdf.GpsSensor()->VelocityNoise()); } this->dataPtr->initialized = true; @@ -157,36 +157,40 @@ bool GpsSensor::Update(const std::chrono::steady_clock::duration &_now) frame->set_key("frame_id"); frame->add_value(this->Name()); - // Apply gps position noise - if (this->dataPtr->noises.find(GPS_POSITION_NOISE) != + // Apply gps horizontal position noise + if (this->dataPtr->noises.find(GPS_HORIZONTAL_POSITION_NOISE) != this->dataPtr->noises.end()) { - - // Apply in degrees. this->SetLatitude( - this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Latitude()) - ); + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE]->Apply( + this->Latitude())); + this->SetLongitude( - this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Longitude()) - ); + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE]->Apply( + this->Longitude())); + } + + if (this->dataPtr->noises.find(GPS_VERTICAL_POSITION_NOISE) != + this->dataPtr->noises.end()) + { this->SetAltitude( - this->dataPtr->noises[GPS_POSITION_NOISE]->Apply(this->Altitude()) - ); + this->dataPtr->noises[GPS_VERTICAL_POSITION_NOISE]->Apply( + this->Altitude())); } // taken from ImuSensor.cc - convenience method auto applyNoise = [&](SensorNoiseType noiseType, double &value) { - if(this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()){ + if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()){ value = this->dataPtr->noises[noiseType]->Apply(value); } }; - applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.X()); - applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.Y()); - applyNoise(GPS_VELOCITY_NOISE, this->dataPtr->velocity.Z()); + applyNoise(GPS_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.X()); + applyNoise(GPS_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.Y()); + applyNoise(GPS_VERTICAL_VELOCITY_NOISE, this->dataPtr->velocity.Z()); - //normalise so that it is within +/- 180 + // normalise so that it is within +/- 180 this->dataPtr->latitude.Normalize(); this->dataPtr->longitude.Normalize(); @@ -205,42 +209,52 @@ bool GpsSensor::Update(const std::chrono::steady_clock::duration &_now) return true; } -void GpsSensor::SetLatitude(double _latitude) { +void GpsSensor::SetLatitude(double _latitude) +{ this->dataPtr->latitude.Degree(_latitude); } -double GpsSensor::Latitude() const { +double GpsSensor::Latitude() const +{ return this->dataPtr->latitude.Degree(); } -void GpsSensor::SetAltitude(double _altitude) { +void GpsSensor::SetAltitude(double _altitude) +{ this->dataPtr->altitude = _altitude; } -double GpsSensor::Altitude() const { +double GpsSensor::Altitude() const +{ return this->dataPtr->altitude; } -void GpsSensor::SetLongitude(double _longitude) { +void GpsSensor::SetLongitude(double _longitude) +{ this->dataPtr->longitude.Degree(_longitude); } -double GpsSensor::Longitude() const { +double GpsSensor::Longitude() const +{ return this->dataPtr->longitude.Degree(); } -void GpsSensor::SetVelocity(ignition::math::Vector3d &_vel) { +void GpsSensor::SetVelocity(ignition::math::Vector3d &_vel) +{ this->dataPtr->velocity = _vel; } -ignition::math::Vector3d GpsSensor::Velocity() const { +ignition::math::Vector3d GpsSensor::Velocity() const +{ return this->dataPtr->velocity; } -void GpsSensor::SetPosition(double _latitude, double _longitude, double _altitude) { +void GpsSensor::SetPosition(double _latitude, double _longitude, + double _altitude) +{ this->SetLongitude(_longitude); this->SetLatitude(_latitude); this->SetAltitude(_altitude); } -IGN_SENSORS_REGISTER_SENSOR(GpsSensor) \ No newline at end of file +IGN_SENSORS_REGISTER_SENSOR(GpsSensor) From ecb588d7042628f49ac2a3a4bc004d4f06899d6d Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 14:17:11 +1100 Subject: [PATCH 4/4] added test Signed-off-by: Dre Westcook --- include/ignition/sensors/GpsSensor.hh | 2 +- include/ignition/sensors/SensorTypes.hh | 4 ++-- src/GpsSensor.cc | 29 +++++++++++++++++++------ 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh index ac3c3487..be685f3b 100644 --- a/include/ignition/sensors/GpsSensor.hh +++ b/include/ignition/sensors/GpsSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2020 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. diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index ab533d7a..2736eb70 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -148,7 +148,7 @@ namespace ignition /// \brief Noise streams for the Lidar sensor /// \sa Lidar LIDAR_NOISE = 14, - + /// \brief Noise streams for the Gps position sensor /// \sa Gps GPS_HORIZONTAL_POSITION_NOISE = 15, @@ -160,7 +160,7 @@ namespace ignition /// \brief Noise streams for the Gps velocity sensor /// \sa Gps GPS_HORIZONTAL_VELOCITY_NOISE = 17, - + /// \brief Noise streams for the Gps velocity sensor /// \sa Gps GPS_VERTICAL_VELOCITY_NOISE = 18, diff --git a/src/GpsSensor.cc b/src/GpsSensor.cc index 8118ece0..8220b4b6 100644 --- a/src/GpsSensor.cc +++ b/src/GpsSensor.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2020 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. @@ -106,19 +106,34 @@ bool GpsSensor::Load(const sdf::Sensor &_sdf) } // Load the noise parameters - if (_sdf.GpsSensor()->PositionNoise().Type() + + if (_sdf.GpsSensor()->HorizontalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->HorizontalPositionNoise()); + } + if (_sdf.GpsSensor()->VerticalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[GPS_VERTICAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.GpsSensor()->VerticalPositionNoise()); + } + if (_sdf.GpsSensor()->HorizontalVelocityNoise().Type() != sdf::NoiseType::NONE) { - this->dataPtr->noises[GPS_POSITION_NOISE] = + this->dataPtr->noises[GPS_HORIZONTAL_VELOCITY_NOISE] = NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->PositionNoise()); + _sdf.GpsSensor()->HorizontalVelocityNoise()); } - if (_sdf.GpsSensor()->VelocityNoise().Type() + if (_sdf.GpsSensor()->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) { - this->dataPtr->noises[GPS_VELOCITY_NOISE] = + this->dataPtr->noises[GPS_VERTICAL_VELOCITY_NOISE] = NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->VelocityNoise()); + _sdf.GpsSensor()->VerticalVelocityNoise()); } this->dataPtr->initialized = true;