diff --git a/include/ignition/sensors/GpsSensor.hh b/include/ignition/sensors/GpsSensor.hh deleted file mode 100644 index be685f3b..00000000 --- a/include/ignition/sensors/GpsSensor.hh +++ /dev/null @@ -1,135 +0,0 @@ -/* - * 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. - * 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() const; - - - /// \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 diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh new file mode 100644 index 00000000..8f296341 --- /dev/null +++ b/include/ignition/sensors/NavSatSensor.hh @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2021 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_NAVSAT_HH_ +#define IGNITION_SENSORS_NAVSAT_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 NavSatPrivate; + + /// \brief NavSat Sensor Class + /// + /// A sensor that reports position and velocity readings over + /// Ignition Transport using spherical coordinates (latitude / longitude). + /// + /// By default, it publishes `ignition::msgs::NavSat` messages on the + /// `/.../navsat` topic. + /// + /// This sensor assumes the world is using the East-North-Up (ENU) frame. + class IGNITION_SENSORS_NAVSAT_VISIBLE NavSatSensor : public Sensor + { + /// \brief Constructor + public: NavSatSensor(); + + /// \brief Destructor + public: virtual ~NavSatSensor() = default; + + /// \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 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 NavSat + /// \param[in] _latitude Latitude of NavSat + public: void SetLatitude(const math::Angle &_latitude); + + /// \brief Get the latitude of the NavSat, wraped between +/- 180 degrees. + /// \return Latitude angle. + public: const math::Angle &Latitude() const; + + /// \brief Set the longitude of the NavSat + /// \param[in] _longitude Longitude of NavSat + public: void SetLongitude(const math::Angle &_longitude); + + /// \brief Get the longitude of the NavSat, wraped between +/- 180 degrees. + /// \return Longitude angle. + public: const math::Angle &Longitude() const; + + /// \brief Set the altitude of the NavSat + /// \param[in] _altitude altitude of NavSat in meters + public: void SetAltitude(double _altitude); + + /// \brief NavSat altitude above sea level + /// \return Altitude in meters + public: double Altitude() const; + + /// \brief Set the velocity of the NavSat in ENU world frame. + /// \param[in] _vel NavSat in meters per second. + public: void SetVelocity(const math::Vector3d &_vel); + + /// \brief Get the velocity of the NavSat sensor in the ENU world frame. + /// \return Velocity in meters per second + public: const math::Vector3d &Velocity() const; + + /// \brief Easy short hand for setting the position of the sensor. + /// \param[in] _latitude Latitude angle. + /// \param[in] _longitude Longitude angle. + /// \param[in] _altitude Altitude in meters, defaults to zero. + public: void SetPosition(const math::Angle &_latitude, + const math::Angle &_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 diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 2736eb70..8b451958 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,21 +149,21 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, - /// \brief Noise streams for the Gps position sensor - /// \sa Gps - GPS_HORIZONTAL_POSITION_NOISE = 15, + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_POSITION_NOISE = 15, - /// \brief Noise streams for the Gps position sensor - /// \sa Gps - GPS_VERTICAL_POSITION_NOISE = 16, + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_VERTICAL_POSITION_NOISE = 16, - /// \brief Noise streams for the Gps velocity sensor - /// \sa Gps - GPS_HORIZONTAL_VELOCITY_NOISE = 17, + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_VELOCITY_NOISE = 17, - /// \brief Noise streams for the Gps velocity sensor - /// \sa Gps - GPS_VERTICAL_VELOCITY_NOISE = 18, + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_VERTICAL_VELOCITY_NOISE = 18, /// \internal /// \brief Indicator used to create an iterator over the enum. Do not diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 68b66f99..447f8955 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -115,8 +115,8 @@ 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(navsat_sources NavSatSensor.cc) +ign_add_component(navsat SOURCES ${navsat_sources} GET_TARGET_NAME navsat_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 deleted file mode 100644 index 8220b4b6..00000000 --- a/src/GpsSensor.cc +++ /dev/null @@ -1,275 +0,0 @@ -/* - * 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. - * 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()->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_HORIZONTAL_VELOCITY_NOISE] = - NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->HorizontalVelocityNoise()); - } - if (_sdf.GpsSensor()->VerticalVelocityNoise().Type() - != sdf::NoiseType::NONE) - { - this->dataPtr->noises[GPS_VERTICAL_VELOCITY_NOISE] = - NoiseFactory::NewNoiseModel( - _sdf.GpsSensor()->VerticalVelocityNoise()); - } - - 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 horizontal position noise - if (this->dataPtr->noises.find(GPS_HORIZONTAL_POSITION_NOISE) != - this->dataPtr->noises.end()) - { - this->SetLatitude( - this->dataPtr->noises[GPS_HORIZONTAL_POSITION_NOISE]->Apply( - this->Latitude())); - - this->SetLongitude( - 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_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()){ - value = this->dataPtr->noises[noiseType]->Apply(value); - } - }; - - 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 - 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::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); - this->SetAltitude(_altitude); -} - -IGN_SENSORS_REGISTER_SENSOR(GpsSensor) diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc new file mode 100644 index 00000000..d5b0dfa9 --- /dev/null +++ b/src/NavSatSensor.cc @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2021 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/NavSatSensor.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 NavSat +class ignition::sensors::NavSatPrivate +{ + /// \brief Node to create publisher + public: transport::Node node; + + /// \brief To publish NavSat messages. + public: transport::Node::Publisher pub; + + /// \brief True if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Latitude angle + public: math::Angle latitude; + + /// \brief Longitude angle + public: math::Angle longitude; + + /// \brief Altitude + public: double altitude = 0.0; + + /// \brief Velocity in ENU frame. + public: math::Vector3d velocity; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +NavSatSensor::NavSatSensor() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +bool NavSatSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool NavSatSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::NAVSAT) + { + ignerr << "Attempting to a load an NAVSAT sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.NavSatSensor() == nullptr) + { + ignerr << "Attempting to a load an NAVSAT sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/navsat"); + + 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.NavSatSensor()->HorizontalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_HORIZONTAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->HorizontalPositionNoise()); + } + if (_sdf.NavSatSensor()->VerticalPositionNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_VERTICAL_POSITION_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->VerticalPositionNoise()); + } + if (_sdf.NavSatSensor()->HorizontalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_HORIZONTAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->HorizontalVelocityNoise()); + } + if (_sdf.NavSatSensor()->VerticalVelocityNoise().Type() + != sdf::NoiseType::NONE) + { + this->dataPtr->noises[NAVSAT_VERTICAL_VELOCITY_NOISE] = + NoiseFactory::NewNoiseModel( + _sdf.NavSatSensor()->VerticalVelocityNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool NavSatSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool NavSatSensor::Update(const common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("NavSatSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::NavSat msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + msg.set_frame_id(this->Name()); + + // Apply horizontal position noise + if (this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE) != + this->dataPtr->noises.end()) + { + this->SetLatitude(IGN_DTOR( + this->dataPtr->noises[NAVSAT_HORIZONTAL_POSITION_NOISE]->Apply( + this->Latitude().Degree()))); + + this->SetLongitude(IGN_DTOR( + this->dataPtr->noises[NAVSAT_HORIZONTAL_POSITION_NOISE]->Apply( + this->Longitude().Degree()))); + } + + if (this->dataPtr->noises.find(NAVSAT_VERTICAL_POSITION_NOISE) != + this->dataPtr->noises.end()) + { + this->SetAltitude( + this->dataPtr->noises[NAVSAT_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()){ + value = this->dataPtr->noises[noiseType]->Apply(value); + } + }; + + applyNoise(NAVSAT_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.X()); + applyNoise(NAVSAT_HORIZONTAL_VELOCITY_NOISE, this->dataPtr->velocity.Y()); + applyNoise(NAVSAT_VERTICAL_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 NavSatSensor::SetLatitude(const math::Angle &_latitude) +{ + this->dataPtr->latitude = _latitude; +} + +////////////////////////////////////////////////// +const math::Angle &NavSatSensor::Latitude() const +{ + return this->dataPtr->latitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetAltitude(double _altitude) +{ + this->dataPtr->altitude = _altitude; +} + +////////////////////////////////////////////////// +double NavSatSensor::Altitude() const +{ + return this->dataPtr->altitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetLongitude(const math::Angle &_longitude) +{ + this->dataPtr->longitude = _longitude; +} + +////////////////////////////////////////////////// +const math::Angle &NavSatSensor::Longitude() const +{ + return this->dataPtr->longitude; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetVelocity(const math::Vector3d &_vel) +{ + this->dataPtr->velocity = _vel; +} + +////////////////////////////////////////////////// +const math::Vector3d &NavSatSensor::Velocity() const +{ + return this->dataPtr->velocity; +} + +////////////////////////////////////////////////// +void NavSatSensor::SetPosition(const math::Angle &_latitude, + const math::Angle &_longitude, double _altitude) +{ + this->SetLongitude(_longitude); + this->SetLatitude(_latitude); + this->SetAltitude(_altitude); +} + +IGN_SENSORS_REGISTER_SENSOR(NavSatSensor)