From a0482c1048d7b6232867ea351e6e659f9b2fb4a3 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 15:40:28 +1100 Subject: [PATCH 01/13] 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 875ccb07..468981a3 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, /// \brief Force body-frame X axis noise in N /// \sa ForceTorqueSensor diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e6408671..90fca9a9 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -117,6 +117,9 @@ ign_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME f 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 539ee00c7d48b7ac653eb8fcd7c9cee1256cb847 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Wed, 30 Dec 2020 18:35:22 +1100 Subject: [PATCH 02/13] 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 7e6290260644c5c80cb8d37e35752fe7808525eb Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 13:51:55 +1100 Subject: [PATCH 03/13] 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 468981a3..2f730690 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, /// \brief Force body-frame X axis noise in N /// \sa ForceTorqueSensor 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 2390e4b21262911e6623a51a50eac31c67a7fab0 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 14:17:11 +1100 Subject: [PATCH 04/13] 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 2f730690..39b4b057 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; From e448163fe68ea9d2c78d7447b6bc8e4205052a29 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 13 Dec 2021 15:39:21 -0800 Subject: [PATCH 05/13] GPS -> NavSat Signed-off-by: Louise Poubel --- include/ignition/sensors/GpsSensor.hh | 135 ----------- include/ignition/sensors/NavSatSensor.hh | 134 +++++++++++ include/ignition/sensors/SensorTypes.hh | 24 +- src/CMakeLists.txt | 4 +- src/GpsSensor.cc | 275 ----------------------- src/NavSatSensor.cc | 273 ++++++++++++++++++++++ 6 files changed, 421 insertions(+), 424 deletions(-) delete mode 100644 include/ignition/sensors/GpsSensor.hh create mode 100644 include/ignition/sensors/NavSatSensor.hh delete mode 100644 src/GpsSensor.cc create mode 100644 src/NavSatSensor.cc 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 39b4b057..35d3044b 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, /// \brief Force body-frame X axis noise in N /// \sa ForceTorqueSensor diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 90fca9a9..a57ca2ed 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -117,8 +117,8 @@ ign_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME f 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) From b464cf4ed5b9898c0198a3a9f57b8ccf1afa25f6 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 13 Dec 2021 16:30:56 -0800 Subject: [PATCH 06/13] remove deprecated functions Signed-off-by: Louise Poubel --- include/ignition/sensors/NavSatSensor.hh | 7 ------- src/NavSatSensor.cc | 6 ------ 2 files changed, 13 deletions(-) diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh index 8f296341..eff1605b 100644 --- a/include/ignition/sensors/NavSatSensor.hh +++ b/include/ignition/sensors/NavSatSensor.hh @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -70,12 +69,6 @@ namespace ignition /// \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 diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index d5b0dfa9..fea01dd6 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -142,12 +142,6 @@ bool NavSatSensor::Load(sdf::ElementPtr _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) { From 0c7974d031cb45a89f742deeb5245533c9804c3f Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 11:26:04 -0800 Subject: [PATCH 07/13] Compilation fixes Signed-off-by: Louise Poubel --- include/ignition/sensors/NavSatSensor.hh | 2 +- src/NavSatSensor.cc | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh index eff1605b..c6cb064f 100644 --- a/include/ignition/sensors/NavSatSensor.hh +++ b/include/ignition/sensors/NavSatSensor.hh @@ -53,7 +53,7 @@ namespace ignition public: NavSatSensor(); /// \brief Destructor - public: virtual ~NavSatSensor() = default; + public: virtual ~NavSatSensor(); /// \brief Load the sensor based on data from an sdf::Sensor object. /// \param[in] _sdf SDF Sensor parameters. diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index fea01dd6..08d68813 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -62,6 +62,9 @@ NavSatSensor::NavSatSensor() { } +////////////////////////////////////////////////// +NavSatSensor::~NavSatSensor() = default; + ////////////////////////////////////////////////// bool NavSatSensor::Init() { @@ -264,4 +267,3 @@ void NavSatSensor::SetPosition(const math::Angle &_latitude, this->SetAltitude(_altitude); } -IGN_SENSORS_REGISTER_SENSOR(NavSatSensor) From c5225895f99ca63c414681ead47785fe213cfa3e Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 13:13:55 -0800 Subject: [PATCH 08/13] fix vel Signed-off-by: Louise Poubel --- src/NavSatSensor.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index 08d68813..b7bda58d 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -200,8 +200,8 @@ bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) 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()); + msg.set_velocity_north(this->dataPtr->velocity.Y()); + msg.set_velocity_up(this->dataPtr->velocity.Z()); // publish this->AddSequence(msg.mutable_header()); From 0ab5007e82f3fe2357dc4cfdddcfa3428af97377 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 22 Dec 2021 18:23:19 -0800 Subject: [PATCH 09/13] test and other tweaks Signed-off-by: Louise Poubel --- include/ignition/sensors/SensorTypes.hh | 32 +-- src/CMakeLists.txt | 6 +- src/NavSatSensor.cc | 5 +- test/integration/CMakeLists.txt | 2 + test/integration/navsat.cc | 332 ++++++++++++++++++++++++ 5 files changed, 355 insertions(+), 22 deletions(-) create mode 100644 test/integration/navsat.cc diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 35d3044b..63345045 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,22 +149,6 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, - /// \brief Noise streams for the NavSat position sensor - /// \sa NavSat - NAVSAT_HORIZONTAL_POSITION_NOISE = 15, - - /// \brief Noise streams for the NavSat position sensor - /// \sa NavSat - NAVSAT_VERTICAL_POSITION_NOISE = 16, - - /// \brief Noise streams for the NavSat velocity sensor - /// \sa NavSat - NAVSAT_HORIZONTAL_VELOCITY_NOISE = 17, - - /// \brief Noise streams for the NavSat velocity sensor - /// \sa NavSat - NAVSAT_VERTICAL_VELOCITY_NOISE = 18, - /// \brief Force body-frame X axis noise in N /// \sa ForceTorqueSensor FORCE_X_NOISE_N = 15, @@ -189,6 +173,22 @@ namespace ignition /// \sa ForceTorqueSensor TORQUE_Z_NOISE_N_M = 20, + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_POSITION_NOISE = 21, + + /// \brief Noise streams for the NavSat position sensor + /// \sa NavSat + NAVSAT_VERTICAL_POSITION_NOISE = 22, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_HORIZONTAL_VELOCITY_NOISE = 23, + + /// \brief Noise streams for the NavSat velocity sensor + /// \sa NavSat + NAVSAT_VERTICAL_VELOCITY_NOISE = 24, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a57ca2ed..53c2782c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -117,12 +117,12 @@ ign_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME f set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_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) +set(navsat_sources NavSatSensor.cc) +ign_add_component(navsat SOURCES ${navsat_sources} GET_TARGET_NAME navsat_target) + set(rgbd_camera_sources RgbdCameraSensor.cc) ign_add_component(rgbd_camera SOURCES ${rgbd_camera_sources} GET_TARGET_NAME rgbd_camera_target) target_compile_definitions(${rgbd_camera_target} PUBLIC RgbdCameraSensor_EXPORTS) diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index b7bda58d..3ef39a70 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -159,7 +159,7 @@ bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); msg.set_frame_id(this->Name()); - // Apply horizontal position noise + // Apply noise if (this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE) != this->dataPtr->noises.end()) { @@ -180,7 +180,6 @@ bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) this->Altitude())); } - // taken from ImuSensor.cc - convenience method auto applyNoise = [&](SensorNoiseType noiseType, double &value) { if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()){ @@ -262,8 +261,8 @@ const math::Vector3d &NavSatSensor::Velocity() const void NavSatSensor::SetPosition(const math::Angle &_latitude, const math::Angle &_longitude, double _altitude) { - this->SetLongitude(_longitude); this->SetLatitude(_latitude); + this->SetLongitude(_longitude); this->SetAltitude(_altitude); } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 31e8acee..cfaeccc9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(tests force_torque.cc logical_camera.cc magnetometer.cc + navsat.cc imu.cc ) @@ -55,5 +56,6 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu ${PROJECT_LIBRARY_TARGET_NAME}-force_torque + ${PROJECT_LIBRARY_TARGET_NAME}-navsat ) diff --git a/test/integration/navsat.cc b/test/integration/navsat.cc new file mode 100644 index 00000000..86dc2109 --- /dev/null +++ b/test/integration/navsat.cc @@ -0,0 +1,332 @@ +/* + * 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 + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; +using namespace sensors; +using namespace std::chrono_literals; + +/// \brief Helper function to create an navsat sdf element +sdf::ElementPtr NavSatToSdf(const std::string &_name, + const 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 navsat sdf element with noise +sdf::ElementPtr NavSatToSdfWithNoise(const std::string &_name, + const 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 << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " << _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 navsat sensor +class NavSatSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, CreateNavSat) +{ + // Create SDF describing an navsat sensor + const std::string name = "TestNavSat"; + const std::string topic = "/ignition/sensors/test/navsat"; + const std::string topicNoise = "/ignition/sensors/test/navsat_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + math::Pose3d sensorPose(math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity); + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize); + + auto navsatSdfNoise = NavSatToSdfWithNoise(name, sensorPose, updateRate, + topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + SensorFactory sf; + auto sensor = sf.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); + + auto sensorNoise = sf.CreateSensor(navsatSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + EXPECT_EQ(name, sensorNoise->Name()); + EXPECT_EQ(topicNoise, sensorNoise->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, SensorReadings) +{ + // Create SDF describing an navsat sensor + const std::string name = "TestNavSat"; + const std::string topic = "/ignition/sensors/test/navsat"; + const std::string topicNoise = "/ignition/sensors/test/navsat_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + math::Pose3d sensorPose(math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity); + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsatSdfNoise = NavSatToSdfWithNoise(name, sensorPose, updateRate, + topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + SensorFactory sf; + auto sensor = sf.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, sensor); + + auto sensorNoise = sf.CreateSensor(navsatSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + // verify initial readings + EXPECT_DOUBLE_EQ(0.0, sensor->Latitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensor->Longitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensor->Altitude()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().X()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Y()); + EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Z()); + + // set state and verify readings + math::Angle lat{IGN_DTOR(20)}; + sensor->SetLatitude(lat); + sensorNoise->SetLatitude(lat); + EXPECT_EQ(lat, sensor->Latitude()); + EXPECT_EQ(lat, sensorNoise->Latitude()); + + math::Angle lon{IGN_DTOR(-20)}; + sensor->SetLongitude(lon); + sensorNoise->SetLongitude(lon); + EXPECT_EQ(lon, sensor->Longitude()); + EXPECT_EQ(lon, sensorNoise->Longitude()); + + double altitude{20.0}; + sensor->SetAltitude(altitude); + sensorNoise->SetAltitude(altitude); + EXPECT_DOUBLE_EQ(altitude, sensor->Altitude()); + EXPECT_DOUBLE_EQ(altitude, sensorNoise->Altitude()); + + lat += IGN_DTOR(20); + lon += IGN_DTOR(20); + altitude += 100; + sensor->SetPosition(lat, lon, altitude); + sensorNoise->SetPosition(lat, lon, altitude); + EXPECT_EQ(lat, sensor->Latitude()); + EXPECT_EQ(lat, sensorNoise->Latitude()); + EXPECT_EQ(lon, sensor->Longitude()); + EXPECT_EQ(lon, sensorNoise->Longitude()); + EXPECT_DOUBLE_EQ(altitude, sensor->Altitude()); + EXPECT_DOUBLE_EQ(altitude, sensorNoise->Altitude()); + + math::Vector3d velocity{1.0, 2.0, 3.0}; + sensor->SetVelocity(velocity); + sensorNoise->SetVelocity(velocity); + EXPECT_EQ(velocity, sensor->Velocity()); + EXPECT_EQ(velocity, sensorNoise->Velocity()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + sensor->Update(std::chrono::steady_clock::duration(1s)); + 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(lat.Degree(), msg.latitude_deg()); + EXPECT_DOUBLE_EQ(lon.Degree(), msg.longitude_deg()); + EXPECT_DOUBLE_EQ(altitude, msg.altitude()); + EXPECT_DOUBLE_EQ(velocity.X(), msg.velocity_east()); + EXPECT_DOUBLE_EQ(velocity.Y(), msg.velocity_north()); + EXPECT_DOUBLE_EQ(velocity.Z(), msg.velocity_up()); + + // verify msg with noise received on the topic + WaitForMessageTestHelper msgHelperNoise(topicNoise); + sensorNoise->Update(std::chrono::steady_clock::duration(1s)); + EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; + auto msgNoise = msgHelperNoise.Message(); + EXPECT_EQ(1, msgNoise.header().stamp().sec()); + EXPECT_EQ(0, msgNoise.header().stamp().nsec()); + EXPECT_FALSE(math::equal(lat.Degree(), msgNoise.latitude_deg())); + EXPECT_FALSE(math::equal(lon.Degree(), msgNoise.longitude_deg())); + EXPECT_FALSE(math::equal(altitude, msgNoise.altitude())); + EXPECT_FALSE(math::equal(velocity.X(), msgNoise.velocity_east())); + EXPECT_FALSE(math::equal(velocity.Y(), msgNoise.velocity_north())); + EXPECT_FALSE(math::equal(velocity.Z(), msgNoise.velocity_up())); +} + +///////////////////////////////////////////////// +TEST_F(NavSatSensorTest, Topic) +{ + const std::string name = "TestNavSat"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = math::Pose3d(); + + // Factory + SensorFactory factory; + + // Default topic + { + const std::string topic; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsat = factory.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, navsat); + + EXPECT_EQ("/navsat", navsat->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto navsat = factory.CreateSensor(navsatSdf); + ASSERT_NE(nullptr, navsat); + + EXPECT_EQ("/topic_with_spaces/characters", navsat->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto navsatSdf = NavSatToSdf(name, sensorPose, updateRate, topic, + alwaysOn, visualize); + + auto sensor = factory.CreateSensor< + NavSatSensor>(navsatSdf); + ASSERT_EQ(nullptr, sensor); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From cf197c2d477b73b6f79ca8b3b3fa17bc57b84e6c Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 23 Dec 2021 14:33:32 -0800 Subject: [PATCH 10/13] Apply suggestions from code review Signed-off-by: Louise Poubel Signed-off-by: Ashton Larkin Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- include/ignition/sensors/NavSatSensor.hh | 15 +++++++-------- src/NavSatSensor.cc | 2 +- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh index c6cb064f..61a9ee88 100644 --- a/include/ignition/sensors/NavSatSensor.hh +++ b/include/ignition/sensors/NavSatSensor.hh @@ -19,12 +19,11 @@ #include -#include - #include +#include -#include -#include +#include "ignition/sensors/config.hh" +#include "ignition/sensors/navsat/Export.hh" #include "ignition/sensors/Sensor.hh" @@ -79,7 +78,7 @@ namespace ignition /// \param[in] _latitude Latitude of NavSat public: void SetLatitude(const math::Angle &_latitude); - /// \brief Get the latitude of the NavSat, wraped between +/- 180 degrees. + /// \brief Get the latitude of the NavSat, wrapped between +/- 180 degrees. /// \return Latitude angle. public: const math::Angle &Latitude() const; @@ -87,7 +86,7 @@ namespace ignition /// \param[in] _longitude Longitude of NavSat public: void SetLongitude(const math::Angle &_longitude); - /// \brief Get the longitude of the NavSat, wraped between +/- 180 degrees. + /// \brief Get the longitude of the NavSat, wrapped between +/- 180 degrees. /// \return Longitude angle. public: const math::Angle &Longitude() const; @@ -95,7 +94,7 @@ namespace ignition /// \param[in] _altitude altitude of NavSat in meters public: void SetAltitude(double _altitude); - /// \brief NavSat altitude above sea level + /// \brief Get NavSat altitude above sea level /// \return Altitude in meters public: double Altitude() const; @@ -110,7 +109,7 @@ namespace ignition /// \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. + /// \param[in] _altitude Altitude in meters; defaults to zero. public: void SetPosition(const math::Angle &_latitude, const math::Angle &_longitude, double _altitude = 0.0); diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index 3ef39a70..b39336cd 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -16,9 +16,9 @@ */ #include -#include #include #include +#include #include "ignition/sensors/NavSatSensor.hh" #include "ignition/sensors/Noise.hh" From 49850c72809fc0b9ba8ee26ce7fddfbec35de51b Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 23 Dec 2021 16:59:20 -0800 Subject: [PATCH 11/13] PR feedbacj Signed-off-by: Louise Poubel --- .gitignore | 1 - src/NavSatSensor.cc | 51 ++++++++++++++++---------------------- test/integration/navsat.cc | 35 +++++++++++++++----------- 3 files changed, 43 insertions(+), 44 deletions(-) diff --git a/.gitignore b/.gitignore index 5e9f4b02..25867e4f 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,3 @@ build # Version control generated files *.orig -.vscode \ No newline at end of file diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index b39336cd..4eb275a4 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -38,7 +38,7 @@ class ignition::sensors::NavSatPrivate public: transport::Node::Publisher pub; /// \brief True if Load() has been called and was successful - public: bool initialized = false; + public: bool loaded = false; /// \brief Latitude angle public: math::Angle latitude; @@ -53,7 +53,7 @@ class ignition::sensors::NavSatPrivate public: math::Vector3d velocity; /// \brief Noise added to sensor data - public: std::map noises; + public: std::unordered_map noises; }; ////////////////////////////////////////////////// @@ -133,7 +133,7 @@ bool NavSatSensor::Load(const sdf::Sensor &_sdf) _sdf.NavSatSensor()->VerticalVelocityNoise()); } - this->dataPtr->initialized = true; + this->dataPtr->loaded = true; return true; } @@ -149,9 +149,9 @@ bool NavSatSensor::Load(sdf::ElementPtr _sdf) bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) { IGN_PROFILE("NavSatSensor::Update"); - if (!this->dataPtr->initialized) + if (!this->dataPtr->loaded) { - ignerr << "Not initialized, update ignored.\n"; + ignerr << "Not loaded, update ignored.\n"; return false; } @@ -160,36 +160,29 @@ bool NavSatSensor::Update(const std::chrono::steady_clock::duration &_now) msg.set_frame_id(this->Name()); // Apply noise - if (this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE) != - this->dataPtr->noises.end()) + auto iter = this->dataPtr->noises.find(NAVSAT_HORIZONTAL_POSITION_NOISE); + if (iter != 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->SetLatitude(IGN_DTOR(iter->second->Apply(this->Latitude().Degree()))); + this->SetLongitude(IGN_DTOR(iter->second->Apply( this->Longitude().Degree()))); } - - if (this->dataPtr->noises.find(NAVSAT_VERTICAL_POSITION_NOISE) != - this->dataPtr->noises.end()) + iter = this->dataPtr->noises.find(NAVSAT_VERTICAL_POSITION_NOISE); + if (iter != this->dataPtr->noises.end()) { - this->SetAltitude( - this->dataPtr->noises[NAVSAT_VERTICAL_POSITION_NOISE]->Apply( - this->Altitude())); + this->SetAltitude(iter->second->Apply(this->Altitude())); } - - auto applyNoise = [&](SensorNoiseType noiseType, double &value) + iter = this->dataPtr->noises.find(NAVSAT_HORIZONTAL_VELOCITY_NOISE); + if (iter != this->dataPtr->noises.end()) { - 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()); + this->dataPtr->velocity.X(iter->second->Apply(this->dataPtr->velocity.X())); + this->dataPtr->velocity.Y(iter->second->Apply(this->dataPtr->velocity.Y())); + } + iter = this->dataPtr->noises.find(NAVSAT_VERTICAL_VELOCITY_NOISE); + if (iter != this->dataPtr->noises.end()) + { + this->dataPtr->velocity.Z(iter->second->Apply(this->dataPtr->velocity.Z())); + } // normalise so that it is within +/- 180 this->dataPtr->latitude.Normalize(); diff --git a/test/integration/navsat.cc b/test/integration/navsat.cc index 86dc2109..46261bc4 100644 --- a/test/integration/navsat.cc +++ b/test/integration/navsat.cc @@ -30,7 +30,7 @@ using namespace ignition; using namespace sensors; using namespace std::chrono_literals; -/// \brief Helper function to create an navsat sdf element +/// \brief Helper function to create a navsat sdf element sdf::ElementPtr NavSatToSdf(const std::string &_name, const math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, @@ -62,7 +62,7 @@ sdf::ElementPtr NavSatToSdf(const std::string &_name, ->GetElement("sensor"); } -/// \brief Helper function to create an navsat sdf element with noise +/// \brief Helper function to create a navsat sdf element with noise sdf::ElementPtr NavSatToSdfWithNoise(const std::string &_name, const math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, @@ -141,7 +141,7 @@ class NavSatSensorTest: public testing::Test ///////////////////////////////////////////////// TEST_F(NavSatSensorTest, CreateNavSat) { - // Create SDF describing an navsat sensor + // Create SDF describing a navsat sensor const std::string name = "TestNavSat"; const std::string topic = "/ignition/sensors/test/navsat"; const std::string topicNoise = "/ignition/sensors/test/navsat_noise"; @@ -178,13 +178,13 @@ TEST_F(NavSatSensorTest, CreateNavSat) ///////////////////////////////////////////////// TEST_F(NavSatSensorTest, SensorReadings) { - // Create SDF describing an navsat sensor - const std::string name = "TestNavSat"; - const std::string topic = "/ignition/sensors/test/navsat"; - const std::string topicNoise = "/ignition/sensors/test/navsat_noise"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; + // Create SDF describing a navsat sensor + const std::string name{"TestNavSat"}; + const std::string topic{"/ignition/sensors/test/navsat"}; + const std::string topicNoise{"/ignition/sensors/test/navsat_noise"}; + const double updateRate{30.0}; + const bool alwaysOn{true}; + const bool visualize{true}; // Create sensor SDF math::Pose3d sensorPose(math::Vector3d(0.25, 0.0, 0.5), @@ -211,6 +211,13 @@ TEST_F(NavSatSensorTest, SensorReadings) EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Y()); EXPECT_DOUBLE_EQ(0.0, sensor->Velocity().Z()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Latitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Longitude().Degree()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Altitude()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().X()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().Y()); + EXPECT_DOUBLE_EQ(0.0, sensorNoise->Velocity().Z()); + // set state and verify readings math::Angle lat{IGN_DTOR(20)}; sensor->SetLatitude(lat); @@ -280,10 +287,10 @@ TEST_F(NavSatSensorTest, SensorReadings) ///////////////////////////////////////////////// TEST_F(NavSatSensorTest, Topic) { - const std::string name = "TestNavSat"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; + const std::string name{"TestNavSat"}; + const double updateRate{30.0}; + const bool alwaysOn{true}; + const bool visualize{true}; auto sensorPose = math::Pose3d(); // Factory From dd65d907f88bac22e219996d917d1a33eb0457dc Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 4 Jan 2022 15:57:32 -0800 Subject: [PATCH 12/13] Require msgs 8.2 Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e1e9c1b..e31c56fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ endif() #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs8 REQUIRED) +ign_find_package(ignition-msgs8 REQUIRED VERSION 8.2) set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- From 1e65194054209b1ebb2e434ad7c72e7114f8dc98 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 4 Jan 2022 16:16:47 -0800 Subject: [PATCH 13/13] codecheck Signed-off-by: Louise Poubel --- include/ignition/sensors/NavSatSensor.hh | 6 ++++-- src/NavSatSensor.cc | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/ignition/sensors/NavSatSensor.hh b/include/ignition/sensors/NavSatSensor.hh index 61a9ee88..0c6dfc9a 100644 --- a/include/ignition/sensors/NavSatSensor.hh +++ b/include/ignition/sensors/NavSatSensor.hh @@ -78,7 +78,8 @@ namespace ignition /// \param[in] _latitude Latitude of NavSat public: void SetLatitude(const math::Angle &_latitude); - /// \brief Get the latitude of the NavSat, wrapped between +/- 180 degrees. + /// \brief Get the latitude of the NavSat, wrapped between +/- 180 + /// degrees. /// \return Latitude angle. public: const math::Angle &Latitude() const; @@ -86,7 +87,8 @@ namespace ignition /// \param[in] _longitude Longitude of NavSat public: void SetLongitude(const math::Angle &_longitude); - /// \brief Get the longitude of the NavSat, wrapped between +/- 180 degrees. + /// \brief Get the longitude of the NavSat, wrapped between +/- 180 + /// degrees. /// \return Longitude angle. public: const math::Angle &Longitude() const; diff --git a/src/NavSatSensor.cc b/src/NavSatSensor.cc index 4eb275a4..d9bf65d0 100644 --- a/src/NavSatSensor.cc +++ b/src/NavSatSensor.cc @@ -99,7 +99,8 @@ bool NavSatSensor::Load(const sdf::Sensor &_sdf) if (!this->dataPtr->pub) { - ignerr << "Unable to create publisher on topic [" << this->Topic() << "].\n"; + ignerr << "Unable to create publisher on topic [" << this->Topic() + << "]." << std::endl; return false; }