From 2f5e9320a3281c8e898a4ab4449d9cfc2b98ab0e Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Tue, 29 Dec 2020 10:54:46 +1100 Subject: [PATCH 1/8] Add GPS into SDFormat. Signed-off-by: Dre Westcook --- .gitignore | 1 + include/sdf/CMakeLists.txt | 1 + include/sdf/Gps.hh | 132 ++++++++++++++++++++++++++++ include/sdf/Sensor.hh | 13 +++ src/CMakeLists.txt | 2 + src/Gps.cc | 171 +++++++++++++++++++++++++++++++++++++ src/Gps_TEST.cc | 108 +++++++++++++++++++++++ src/Sensor.cc | 25 ++++++ 8 files changed, 453 insertions(+) create mode 100644 include/sdf/Gps.hh create mode 100644 src/Gps.cc create mode 100644 src/Gps_TEST.cc diff --git a/.gitignore b/.gitignore index 416ecfb5f..08e5eba3e 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build build_* *.*.sw? +.vscode diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index 92498d93b..9a7e86c47 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -17,6 +17,7 @@ set (headers Filesystem.hh Frame.hh Geometry.hh + Gps.hh Gui.hh Heightmap.hh Imu.hh diff --git a/include/sdf/Gps.hh b/include/sdf/Gps.hh new file mode 100644 index 000000000..43195c450 --- /dev/null +++ b/include/sdf/Gps.hh @@ -0,0 +1,132 @@ +/* + * Copyright 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 SDF_GPS_HH_ +#define SDF_GPS_HH_ + +#include +#include +#include +#include + +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + class GpsPrivate; + + /// \brief Gps contains information about a Gps sensor. + /// This sensor can be attached to a link. The Gps sensor can be defined + /// SDF XML by the "gps" type. + /// + /// # Example SDF XML using gps type: + /// + /// ~~~{.xml} + /// + /// 1 2 3 0 0 0 + /// /gps + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// ~~~ + class SDFORMAT_VISIBLE Gps + { + /// \brief Default constructor + public: Gps(); + + /// \brief Copy constructor + /// \param[in] _gps Gps to copy. + public: Gps(const Gps &_gps); + + /// \brief Move constructor + /// \param[in] _gps Gps to move. + public: Gps(Gps &&_gps) noexcept; + + /// \brief Destructor + public: ~Gps(); + + /// \brief Assignment operator + /// \param[in] _gps The gps to set values from. + /// \return *this + public: Gps &operator=(const Gps &_gps); + + /// \brief Move assignment operator + /// \param[in] _gps The gps to set values from. + /// \return *this + public: Gps &operator=(Gps &&_gps) noexcept; + + /// \brief Load the gps based on an element pointer. This is *not* + /// the usual entry point. Typical usage of the SDF DOM is through the Root + /// object. + /// \param[in] _sdf The SDF Element pointer + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors Load(ElementPtr _sdf); + + /// \brief Get a pointer to the SDF element that was used during + /// load. + /// \return SDF element pointer. The value will be nullptr if Load has + /// not been called. + public: sdf::ElementPtr Element() const; + + /// \brief Get the noise values for the position component of the gps sensor. + /// \return Noise values for the position sensor. + public: const Noise &PositionNoise() const; + + /// \brief Set the noise values for the position component of the gps sensor. + /// \param[in] _noise Noise values for the position sensor. + public: void SetPositionNoise(const Noise &_noise); + + /// \brief Get the noise values for the velocity component of the gps sensor. + /// \return Noise values for the velocity sensor. + public: const Noise &VelocityNoise() const; + + /// \brief Set the noise values for the velocity component of the gps sensor. + /// \param[in] _noise Noise values for the velocity sensor. + public: void SetVelocityNoise(const Noise &_noise); + + /// \brief Return true if both Gps objects contain the same values. + /// \param[_in] _gps Gps value to compare. + /// \return True if 'this' == _gps. + public: bool operator==(const Gps &_gps) const; + + /// \brief Return true this Gps object does not contain the same + /// values as the passed in parameter. + /// \param[_in] _gps Gps value to compare. + /// \return True if 'this' != _gps. + public: bool operator!=(const Gps &_gps) const; + + /// \brief Private data pointer. + private: GpsPrivate *dataPtr; + }; + } +} +#endif \ No newline at end of file diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 6b0e75508..235cc943b 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -35,6 +35,7 @@ namespace sdf class AirPressure; class Altimeter; class Camera; + class Gps; class Imu; class Lidar; class Magnetometer; @@ -319,6 +320,18 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Set the GPS sensor. + /// \param[in] _gps The GPS sensor. + public: void SetGpsSensor(const Gps &_gps); + + /// \brief Get a pointer to an GPS sensor, or nullptr if the sensor + /// does not contain an GPS sensor. + /// \return Pointer to the sensor's GPS, or nullptr if the sensor + /// is not an GPS. + /// \sa SensorType Type() const + public: const Gps *GpsSensor() const; + + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 01f348a54..53d690481 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -30,6 +30,7 @@ set (sources FrameSemantics.cc Filesystem.cc Geometry.cc + Gps.cc Gui.cc Heightmap.cc ign.cc @@ -110,6 +111,7 @@ if (BUILD_SDF_TEST) Frame_TEST.cc Filesystem_TEST.cc Geometry_TEST.cc + Gps_TEST.cc Gui_TEST.cc Heightmap_TEST.cc Imu_TEST.cc diff --git a/src/Gps.cc b/src/Gps.cc new file mode 100644 index 000000000..e2df247f1 --- /dev/null +++ b/src/Gps.cc @@ -0,0 +1,171 @@ +/* + * Copyright 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 "sdf/Gps.hh" + +using namespace sdf; +using namespace ignition; + +/// \brief Private gps data. +class sdf::GpsPrivate +{ + /// \brief Noise values for the positioning component of the gps sensor + public: Noise positionNoise; + + /// \brief Noise values for the velocity component of the gps sensor + public: Noise velocityNoise; + + /// \brief The SDF element pointer used during load. + public: sdf::ElementPtr sdf{nullptr}; +}; + +////////////////////////////////////////////////// +Gps::Gps() + : dataPtr(new GpsPrivate) +{ +} + +////////////////////////////////////////////////// +Gps::~Gps() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +Gps::Gps(const Gps &_gps) + : dataPtr(new GpsPrivate(*_gps.dataPtr)) +{ +} + +////////////////////////////////////////////////// +Gps::Gps(Gps &&_gps) noexcept + : dataPtr(std::exchange(_gps.dataPtr, nullptr)) +{ +} + +////////////////////////////////////////////////// +Gps &Gps::operator=(const Gps &_gps) +{ + return *this = Gps(_gps); +} + +////////////////////////////////////////////////// +Gps &Gps::operator=(Gps &&_gps) noexcept +{ + std::swap(this->dataPtr, _gps.dataPtr); + return * this; +} + +////////////////////////////////////////////////// +/// \brief Load the gps based on an element pointer. This is *not* +/// the usual entry point. Typical usage of the SDF DOM is through the Root +/// object. +/// \param[in] _sdf The SDF Element pointer +/// \return Errors, which is a vector of Error objects. Each Error includes +/// an error code and message. An empty vector indicates no error. +Errors Gps::Load(ElementPtr _sdf) +{ + Errors errors; + + this->dataPtr->sdf = _sdf; + + // Check that sdf is a valid pointer + if (!_sdf) + { + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Attempting to load GPS, but the provided SDF " + "element is null."}); + return errors; + } + + // Check that the provided SDF element is a element. + // This is an error that cannot be recovered, so return an error. + if (_sdf->GetName() != "gps" ) + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load GPS, but the provided SDF element is " + "not a ."}); + return errors; + } + + // Load gps sensor properties + if (_sdf->HasElement("position_sense")) + { + sdf::ElementPtr elem = _sdf->GetElement("position_sense"); + if (elem->HasElement("noise")) + this->dataPtr->positionNoise.Load(elem->GetElement("noise")); + + } + + if (_sdf->HasElement("velocity_sense")) + { + sdf::ElementPtr elem = _sdf->GetElement("velocity_sense"); + if (elem->HasElement("noise")) + this->dataPtr->velocityNoise.Load(elem->GetElement("noise")); + } + + return errors; +} + +////////////////////////////////////////////////// +sdf::ElementPtr Gps::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +const Noise &Gps::PositionNoise() const +{ + return this->dataPtr->positionNoise; +} + +////////////////////////////////////////////////// +void Gps::SetPositionNoise(const Noise &_noise) +{ + this->dataPtr->positionNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &Gps::VelocityNoise() const +{ + return this->dataPtr->velocityNoise; +} + +////////////////////////////////////////////////// +void Gps::SetVelocityNoise(const Noise &_noise) +{ + this->dataPtr->velocityNoise = _noise; +} + +////////////////////////////////////////////////// +bool Gps::operator==(const Gps &_gps) const +{ + + if (this->dataPtr->positionNoise != _gps.PositionNoise()) + return false; + + if (this->dataPtr->velocityNoise != _gps.VelocityNoise()) + return false; + + return true; +} + +////////////////////////////////////////////////// +bool Gps::operator!=(const Gps &_gps) const +{ + return !(*this == _gps); +} \ No newline at end of file diff --git a/src/Gps_TEST.cc b/src/Gps_TEST.cc new file mode 100644 index 000000000..d26f7e062 --- /dev/null +++ b/src/Gps_TEST.cc @@ -0,0 +1,108 @@ +/* + * 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 "sdf/Gps.hh" + +///////////////////////////////////////////////// +TEST(DOMGps, Construction) +{ + sdf::Gps gps; + sdf::Noise defaultNoise; + EXPECT_EQ(defaultNoise, gps.PositionNoise()); + EXPECT_EQ(defaultNoise, gps.VelocityNoise()); +} + +///////////////////////////////////////////////// +TEST(DOMGps, Set) +{ + sdf::Gps gps; + sdf::Noise noise; + + noise.SetMean(6.5); + noise.SetStdDev(3.79); + + gps.SetPositionNoise(noise); + gps.SetVelocityNoise(noise); + + EXPECT_EQ(noise, gps.PositionNoise()); + EXPECT_EQ(noise, gps.VelocityNoise()); + + sdf::Noise vnoise; + vnoise.SetMean(1.2); + vnoise.SetStdDev(2.44); + + gps.SetVelocityNoise(vnoise); + + // Test seperate noise profiles pos/vel + EXPECT_EQ(vnoise, gps.VelocityNoise()); + EXPECT_NE(vnoise, gps.PositionNoise()); + EXPECT_EQ(noise, gps.PositionNoise()); + + // Inequality operator + sdf::Gps gps2; + EXPECT_NE(gps2, gps); + + // Copy constructor + sdf::Gps gpsCopied(gps); + EXPECT_EQ(gpsCopied, gps); + + // Assignment operator + sdf::Gps gpsAssigned; + gpsAssigned = gps; + EXPECT_EQ(gpsAssigned, gps); + + // Move constructor + sdf::Gps gpsMoved = std::move(gps); + EXPECT_EQ(gpsCopied, gpsMoved); + + // Test nullptr private class + gps = gpsMoved; + EXPECT_EQ(gpsCopied, gps); + + // Move assignment operator + sdf::Gps gpsMoveAssigned; + gpsMoveAssigned = std::move(gpsCopied); + EXPECT_EQ(gpsAssigned, gpsMoveAssigned); + + // Test nullptr private class + gpsCopied = gpsMoveAssigned; + EXPECT_EQ(gpsAssigned, gpsCopied); + +} + +///////////////////////////////////////////////// +TEST(DOMGps, Load) +{ + sdf::Gps gps; + sdf::Errors errors; + + // Null element + errors = gps.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, gps.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = gps.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, gps.Element()); + +} \ No newline at end of file diff --git a/src/Sensor.cc b/src/Sensor.cc index fdd4e96e2..a720fa25a 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -22,6 +22,7 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" +#include "sdf/Gps.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" @@ -79,6 +80,10 @@ class sdf::SensorPrivate this->magnetometer = std::make_unique( *_sensor.magnetometer); } + if (_sensor.gps) + { + this->gps = std::make_unique(*_sensor.gps); + } if (_sensor.altimeter) { this->altimeter = std::make_unique(*_sensor.altimeter); @@ -137,6 +142,9 @@ class sdf::SensorPrivate /// \brief Pointer to an altimeter. public: std::unique_ptr altimeter; + /// \brief Pointer to GPS sensor. + public: std::unique_ptr gps; + /// \brief Pointer to an air pressure sensor. public: std::unique_ptr airPressure; @@ -221,6 +229,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); + case SensorType::GPS: + return *(this->dataPtr->gps) == *(_sensor.dataPtr->gps); case SensorType::CAMERA: case SensorType::DEPTH_CAMERA: case SensorType::RGBD_CAMERA: @@ -343,6 +353,9 @@ Errors Sensor::Load(ElementPtr _sdf) else if (type == "gps") { this->dataPtr->type = SensorType::GPS; + this->dataPtr->gps.reset(new Gps()); + Errors err = this->dataPtr->gps->Load(_sdf->GetElement("gps")); + errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "gpu_ray" || type == "gpu_lidar") { @@ -626,6 +639,18 @@ const Camera *Sensor::CameraSensor() const return this->dataPtr->camera.get(); } +///////////////////////////////////////////////// +void Sensor::SetGpsSensor(const Gps &_gps) +{ + this->dataPtr->gps = std::make_unique(_gps); +} + +///////////////////////////////////////////////// +const Gps *Sensor::GpsSensor() const +{ + return this->dataPtr->gps.get(); +} + ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { From 5a1fe49fdbe2dd0f729252c628cdb127dfb41b26 Mon Sep 17 00:00:00 2001 From: Dre West Date: Thu, 31 Dec 2020 08:38:53 +1100 Subject: [PATCH 2/8] Update include/sdf/Gps.hh Co-authored-by: Louise Poubel Signed-off-by: Dre Westcook --- include/sdf/Gps.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sdf/Gps.hh b/include/sdf/Gps.hh index 43195c450..0ede05a18 100644 --- a/include/sdf/Gps.hh +++ b/include/sdf/Gps.hh @@ -1,5 +1,5 @@ /* - * Copyright 2019 Open Source Robotics Foundation + * Copyright 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. @@ -129,4 +129,4 @@ namespace sdf }; } } -#endif \ No newline at end of file +#endif From 2726e8cde403e7adc5194def57026d53269d90e7 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 09:23:52 +1100 Subject: [PATCH 3/8] changed gps to follow spec Signed-off-by: Dre Westcook --- include/sdf/Gps.hh | 72 ++++++++++++++++------ src/Gps.cc | 114 +++++++++++++++++++++++++++-------- src/Gps_TEST.cc | 52 ++++++++++------ test/integration/link_dom.cc | 12 ++++ test/sdf/sensors.sdf | 30 +++++++++ 5 files changed, 217 insertions(+), 63 deletions(-) diff --git a/include/sdf/Gps.hh b/include/sdf/Gps.hh index 0ede05a18..fed4f59c7 100644 --- a/include/sdf/Gps.hh +++ b/include/sdf/Gps.hh @@ -39,22 +39,38 @@ namespace sdf /// /// ~~~{.xml} /// - /// 1 2 3 0 0 0 - /// /gps - /// - /// + /// 1 2 3 0 0 0 + /// /gps + /// + /// + /// /// /// 0.98 /// 0.76 /// - /// - /// + /// + /// /// /// 0.98 /// 0.76 /// - /// - /// + /// + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// /// /// ~~~ class SDFORMAT_VISIBLE Gps @@ -97,21 +113,37 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Get the noise values for the position component of the gps sensor. - /// \return Noise values for the position sensor. - public: const Noise &PositionNoise() const; + /// \brief Set the noise values for the horizontal position sensor + /// \param[in] _noise Noise values to set to + public: void SetHorizontalPositionNoise(const Noise &_noise); + + /// \brief Get noise value for horizontal position sensor + /// \return Noise values + public: const Noise &HorizontalPositionNoise() const; + + /// \brief Set the noise values for the vertical position sensor + /// \param[in] _noise Noise values to set to + public: void SetVerticalPositionNoise(const Noise &_noise); + + /// \brief Get noise value for vertical position sensor + /// \return Noise values + public: const Noise &VerticalPositionNoise() const; + + /// \brief Set the noise values for the horizontal velocity sensor + /// \param[in] _noise Noise values to set to + public: void SetHorizontalVelocityNoise(const Noise &_noise); - /// \brief Set the noise values for the position component of the gps sensor. - /// \param[in] _noise Noise values for the position sensor. - public: void SetPositionNoise(const Noise &_noise); + /// \brief Get noise value for horizontal velocity sensor + /// \return Noise values + public: const Noise &HorizontalVelocityNoise() const; - /// \brief Get the noise values for the velocity component of the gps sensor. - /// \return Noise values for the velocity sensor. - public: const Noise &VelocityNoise() const; + /// \brief Set the noise values for the vertical velocity sensor + /// \param[in] _noise Noise values to set to + public: void SetVerticalVelocityNoise(const Noise &_noise); - /// \brief Set the noise values for the velocity component of the gps sensor. - /// \param[in] _noise Noise values for the velocity sensor. - public: void SetVelocityNoise(const Noise &_noise); + /// \brief Get noise value for vertical velocity sensor + /// \return Noise values + public: const Noise &VerticalVelocityNoise() const; /// \brief Return true if both Gps objects contain the same values. /// \param[_in] _gps Gps value to compare. diff --git a/src/Gps.cc b/src/Gps.cc index e2df247f1..2e91c2748 100644 --- a/src/Gps.cc +++ b/src/Gps.cc @@ -22,11 +22,17 @@ using namespace ignition; /// \brief Private gps data. class sdf::GpsPrivate { - /// \brief Noise values for the positioning component of the gps sensor - public: Noise positionNoise; + /// \brief Noise values for the horizontal positioning sensor + public: Noise horizontalPositionNoise; - /// \brief Noise values for the velocity component of the gps sensor - public: Noise velocityNoise; + /// \brief Noise values for the vertical positioning sensor + public: Noise verticalPositionNoise; + + /// \brief Noise values for the horizontal velocity sensor + public: Noise horizontalVelocityNoise; + + /// \brief Noise values for the verical velocity sensor + public: Noise verticalVelocityNoise; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf{nullptr}; @@ -103,21 +109,52 @@ Errors Gps::Load(ElementPtr _sdf) } // Load gps sensor properties - if (_sdf->HasElement("position_sense")) + if (_sdf->HasElement("position_sensing")) { - sdf::ElementPtr elem = _sdf->GetElement("position_sense"); - if (elem->HasElement("noise")) - this->dataPtr->positionNoise.Load(elem->GetElement("noise")); - + sdf::ElementPtr elem = _sdf->GetElement("position_sensing"); + + if (elem->HasElement("horizontal")) + { + sdf::ElementPtr horiz = elem->GetElement("horizontal"); + if(horiz->HasElement("noise")) + { + this->dataPtr->horizontalPositionNoise.Load(horiz->GetElement("noise")); + } + } + + if (elem->HasElement("vertical")){ + + sdf::ElementPtr vert = elem->GetElement("vertical"); + if(vert->HasElement("noise")) + { + this->dataPtr->verticalPositionNoise.Load(vert->GetElement("noise")); + } + } } - - if (_sdf->HasElement("velocity_sense")) + if (_sdf->HasElement("velocity_sensing")) { - sdf::ElementPtr elem = _sdf->GetElement("velocity_sense"); - if (elem->HasElement("noise")) - this->dataPtr->velocityNoise.Load(elem->GetElement("noise")); + sdf::ElementPtr elem = _sdf->GetElement("velocity_sensing"); + + if (elem->HasElement("horizontal")){ + + sdf::ElementPtr horiz = elem->GetElement("horizontal"); + if(horiz->HasElement("noise")) + { + this->dataPtr->horizontalVelocityNoise.Load(horiz->GetElement("noise")); + } + } + + if (elem->HasElement("vertical")){ + + sdf::ElementPtr vert = elem->GetElement("vertical"); + if(vert->HasElement("noise")) + { + this->dataPtr->verticalVelocityNoise.Load(vert->GetElement("noise")); + } + } } + return errors; } @@ -128,37 +165,64 @@ sdf::ElementPtr Gps::Element() const } ////////////////////////////////////////////////// -const Noise &Gps::PositionNoise() const +const Noise &Gps::HorizontalPositionNoise() const +{ + return this->dataPtr->horizontalPositionNoise; +} + +////////////////////////////////////////////////// +void Gps::SetHorizontalPositionNoise(const Noise &_noise) +{ + this->dataPtr->horizontalPositionNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &Gps::HorizontalVelocityNoise() const +{ + return this->dataPtr->horizontalVelocityNoise; +} + +////////////////////////////////////////////////// +void Gps::SetHorizontalVelocityNoise(const Noise &_noise) { - return this->dataPtr->positionNoise; + this->dataPtr->horizontalVelocityNoise = _noise; } ////////////////////////////////////////////////// -void Gps::SetPositionNoise(const Noise &_noise) +const Noise &Gps::VerticalPositionNoise() const { - this->dataPtr->positionNoise = _noise; + return this->dataPtr->verticalPositionNoise; } ////////////////////////////////////////////////// -const Noise &Gps::VelocityNoise() const +void Gps::SetVerticalPositionNoise(const Noise &_noise) { - return this->dataPtr->velocityNoise; + this->dataPtr->verticalPositionNoise = _noise; } ////////////////////////////////////////////////// -void Gps::SetVelocityNoise(const Noise &_noise) +const Noise &Gps::VerticalVelocityNoise() const { - this->dataPtr->velocityNoise = _noise; + return this->dataPtr->verticalVelocityNoise; +} + +////////////////////////////////////////////////// +void Gps::SetVerticalVelocityNoise(const Noise &_noise) +{ + this->dataPtr->verticalVelocityNoise = _noise; } ////////////////////////////////////////////////// bool Gps::operator==(const Gps &_gps) const { - if (this->dataPtr->positionNoise != _gps.PositionNoise()) + if (this->dataPtr->verticalPositionNoise != _gps.VerticalPositionNoise()) return false; - - if (this->dataPtr->velocityNoise != _gps.VelocityNoise()) + if (this->dataPtr->horizontalPositionNoise != _gps.HorizontalPositionNoise()) + return false; + if (this->dataPtr->verticalVelocityNoise != _gps.VerticalVelocityNoise()) + return false; + if (this->dataPtr->horizontalVelocityNoise != _gps.HorizontalVelocityNoise()) return false; return true; diff --git a/src/Gps_TEST.cc b/src/Gps_TEST.cc index d26f7e062..0559f8860 100644 --- a/src/Gps_TEST.cc +++ b/src/Gps_TEST.cc @@ -23,35 +23,51 @@ TEST(DOMGps, Construction) { sdf::Gps gps; sdf::Noise defaultNoise; - EXPECT_EQ(defaultNoise, gps.PositionNoise()); - EXPECT_EQ(defaultNoise, gps.VelocityNoise()); + EXPECT_EQ(defaultNoise, gps.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); } ///////////////////////////////////////////////// TEST(DOMGps, Set) { sdf::Gps gps; + sdf::Noise noise; + sdf::Noise defaultNoise; + //set random values and check they apply. noise.SetMean(6.5); noise.SetStdDev(3.79); - gps.SetPositionNoise(noise); - gps.SetVelocityNoise(noise); - - EXPECT_EQ(noise, gps.PositionNoise()); - EXPECT_EQ(noise, gps.VelocityNoise()); - - sdf::Noise vnoise; - vnoise.SetMean(1.2); - vnoise.SetStdDev(2.44); - - gps.SetVelocityNoise(vnoise); - - // Test seperate noise profiles pos/vel - EXPECT_EQ(vnoise, gps.VelocityNoise()); - EXPECT_NE(vnoise, gps.PositionNoise()); - EXPECT_EQ(noise, gps.PositionNoise()); + gps.SetVerticalPositionNoise(noise); + + EXPECT_EQ(noise, gps.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); + + gps.SetHorizontalPositionNoise(noise); + + EXPECT_EQ(noise, gps.VerticalPositionNoise()); + EXPECT_EQ(noise, gps.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); + + gps.SetVerticalVelocityNoise(noise); + + EXPECT_EQ(noise, gps.VerticalPositionNoise()); + EXPECT_EQ(noise, gps.HorizontalPositionNoise()); + EXPECT_EQ(noise, gps.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); + + gps.SetHorizontalVelocityNoise(noise); + + EXPECT_EQ(noise, gps.VerticalPositionNoise()); + EXPECT_EQ(noise, gps.HorizontalPositionNoise()); + EXPECT_EQ(noise, gps.VerticalVelocityNoise()); + EXPECT_EQ(noise, gps.HorizontalVelocityNoise()); // Inequality operator sdf::Gps gps2; diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 19b018da5..045ad3ace 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -27,6 +27,7 @@ #include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/Imu.hh" +#include "sdf/Gps.hh" #include "sdf/Link.hh" #include "sdf/Magnetometer.hh" #include "sdf/Model.hh" @@ -365,6 +366,17 @@ TEST(DOMLink, Sensors) EXPECT_EQ("gps_sensor", gpsSensor->Name()); EXPECT_EQ(sdf::SensorType::GPS, gpsSensor->Type()); EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), gpsSensor->RawPose()); + const sdf::Gps *gpsSensorObj = gpsSensor->GpsSensor(); + ASSERT_NE(nullptr, gpsSensorObj); + + EXPECT_DOUBLE_EQ(1.2, gpsSensorObj->HorizontalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(3.4, gpsSensorObj->HorizontalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.6, gpsSensorObj->VerticalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(7.8, gpsSensorObj->VerticalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(9.1, gpsSensorObj->HorizontalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(10.11, gpsSensorObj->HorizontalVelocityNoise().StdDev()); + EXPECT_DOUBLE_EQ(12.13, gpsSensorObj->VerticalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(14.15, gpsSensorObj->VerticalVelocityNoise().StdDev()); // Get the gpu_ray sensor const sdf::Sensor *gpuRaySensor = link->SensorByName("gpu_ray_sensor"); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index b65f07d0d..9590eb8fb 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -143,6 +143,36 @@ 13 14 15 0 0 0 + + + + + 1.2 + 3.4 + + + + + 5.6 + 7.8 + + + + + + + 9.1 + 10.11 + + + + + 12.13 + 14.15 + + + + From b6161353f12aa5cde5ce8e2aab9e1e0e8cef1cd9 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Thu, 31 Dec 2020 10:14:12 +1100 Subject: [PATCH 4/8] fixed lints Signed-off-by: Dre Westcook --- src/Gps.cc | 27 +++++++++++---------------- src/Gps_TEST.cc | 13 ++----------- 2 files changed, 13 insertions(+), 27 deletions(-) diff --git a/src/Gps.cc b/src/Gps.cc index 2e91c2748..88f7afb22 100644 --- a/src/Gps.cc +++ b/src/Gps.cc @@ -112,20 +112,18 @@ Errors Gps::Load(ElementPtr _sdf) if (_sdf->HasElement("position_sensing")) { sdf::ElementPtr elem = _sdf->GetElement("position_sensing"); - if (elem->HasElement("horizontal")) { sdf::ElementPtr horiz = elem->GetElement("horizontal"); - if(horiz->HasElement("noise")) + if (horiz->HasElement("noise")) { this->dataPtr->horizontalPositionNoise.Load(horiz->GetElement("noise")); } } - - if (elem->HasElement("vertical")){ - + if (elem->HasElement("vertical")) + { sdf::ElementPtr vert = elem->GetElement("vertical"); - if(vert->HasElement("noise")) + if (vert->HasElement("noise")) { this->dataPtr->verticalPositionNoise.Load(vert->GetElement("noise")); } @@ -134,20 +132,18 @@ Errors Gps::Load(ElementPtr _sdf) if (_sdf->HasElement("velocity_sensing")) { sdf::ElementPtr elem = _sdf->GetElement("velocity_sensing"); - - if (elem->HasElement("horizontal")){ - + if (elem->HasElement("horizontal")) + { sdf::ElementPtr horiz = elem->GetElement("horizontal"); - if(horiz->HasElement("noise")) + if (horiz->HasElement("noise")) { this->dataPtr->horizontalVelocityNoise.Load(horiz->GetElement("noise")); } } - - if (elem->HasElement("vertical")){ - + if (elem->HasElement("vertical")) + { sdf::ElementPtr vert = elem->GetElement("vertical"); - if(vert->HasElement("noise")) + if (vert->HasElement("noise")) { this->dataPtr->verticalVelocityNoise.Load(vert->GetElement("noise")); } @@ -215,7 +211,6 @@ void Gps::SetVerticalVelocityNoise(const Noise &_noise) ////////////////////////////////////////////////// bool Gps::operator==(const Gps &_gps) const { - if (this->dataPtr->verticalPositionNoise != _gps.VerticalPositionNoise()) return false; if (this->dataPtr->horizontalPositionNoise != _gps.HorizontalPositionNoise()) @@ -232,4 +227,4 @@ bool Gps::operator==(const Gps &_gps) const bool Gps::operator!=(const Gps &_gps) const { return !(*this == _gps); -} \ No newline at end of file +} diff --git a/src/Gps_TEST.cc b/src/Gps_TEST.cc index 0559f8860..75e569928 100644 --- a/src/Gps_TEST.cc +++ b/src/Gps_TEST.cc @@ -37,33 +37,26 @@ TEST(DOMGps, Set) sdf::Noise noise; sdf::Noise defaultNoise; - //set random values and check they apply. + // set random values and check they apply. noise.SetMean(6.5); noise.SetStdDev(3.79); gps.SetVerticalPositionNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); EXPECT_EQ(defaultNoise, gps.HorizontalPositionNoise()); EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetHorizontalPositionNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); EXPECT_EQ(noise, gps.HorizontalPositionNoise()); EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetVerticalVelocityNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); EXPECT_EQ(noise, gps.HorizontalPositionNoise()); EXPECT_EQ(noise, gps.VerticalVelocityNoise()); EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetHorizontalVelocityNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); EXPECT_EQ(noise, gps.HorizontalPositionNoise()); EXPECT_EQ(noise, gps.VerticalVelocityNoise()); @@ -98,7 +91,6 @@ TEST(DOMGps, Set) // Test nullptr private class gpsCopied = gpsMoveAssigned; EXPECT_EQ(gpsAssigned, gpsCopied); - } ///////////////////////////////////////////////// @@ -120,5 +112,4 @@ TEST(DOMGps, Load) ASSERT_EQ(1u, errors.size()); EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); EXPECT_NE(nullptr, gps.Element()); - -} \ No newline at end of file +} From 972ac6bdad1275455dad68c6cb3729bab40a3064 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Tue, 9 Feb 2021 19:12:57 +1100 Subject: [PATCH 5/8] change gps to satnav --- include/sdf/CMakeLists.txt | 2 +- include/sdf/{Gps.hh => SatNav.hh} | 66 ++++++++--------- include/sdf/Sensor.hh | 22 +++--- src/CMakeLists.txt | 4 +- src/Gps_TEST.cc | 115 ------------------------------ src/{Gps.cc => SatNav.cc} | 84 ++++++++++------------ src/SatNav_TEST.cc | 115 ++++++++++++++++++++++++++++++ src/Sensor.cc | 33 ++++----- src/Sensor_TEST.cc | 4 +- test/integration/link_dom.cc | 36 +++++----- test/sdf/sensors.sdf | 2 +- 11 files changed, 239 insertions(+), 244 deletions(-) rename include/sdf/{Gps.hh => SatNav.hh} (72%) delete mode 100644 src/Gps_TEST.cc rename src/{Gps.cc => SatNav.cc} (66%) create mode 100644 src/SatNav_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index 9a7e86c47..a93b18a9d 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -17,7 +17,7 @@ set (headers Filesystem.hh Frame.hh Geometry.hh - Gps.hh + SatNav.hh Gui.hh Heightmap.hh Imu.hh diff --git a/include/sdf/Gps.hh b/include/sdf/SatNav.hh similarity index 72% rename from include/sdf/Gps.hh rename to include/sdf/SatNav.hh index fed4f59c7..282082517 100644 --- a/include/sdf/Gps.hh +++ b/include/sdf/SatNav.hh @@ -1,5 +1,5 @@ /* - * Copyright 2020 Open Source Robotics Foundation + * Copyright 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. @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef SDF_GPS_HH_ -#define SDF_GPS_HH_ +#ifndef SDF_SATNAV_HH_ +#define SDF_SATNAV_HH_ #include #include @@ -29,19 +29,19 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - class GpsPrivate; + class SatNavPrivate; - /// \brief Gps contains information about a Gps sensor. - /// This sensor can be attached to a link. The Gps sensor can be defined - /// SDF XML by the "gps" type. + /// \brief SatNav contains information about a SatNav sensor. + /// This sensor can be attached to a link. The SatNav sensor can be defined + /// SDF XML by the "satnav" type. /// - /// # Example SDF XML using gps type: + /// # Example SDF XML using satnav type: /// /// ~~~{.xml} - /// + /// /// 1 2 3 0 0 0 - /// /gps - /// + /// /satnav + /// /// /// /// @@ -70,36 +70,36 @@ namespace sdf /// /// /// - /// + /// /// /// ~~~ - class SDFORMAT_VISIBLE Gps + class SDFORMAT_VISIBLE SatNav { /// \brief Default constructor - public: Gps(); + public: SatNav(); /// \brief Copy constructor - /// \param[in] _gps Gps to copy. - public: Gps(const Gps &_gps); + /// \param[in] _satnav SatNav to copy. + public: SatNav(const SatNav &_satnav); /// \brief Move constructor - /// \param[in] _gps Gps to move. - public: Gps(Gps &&_gps) noexcept; + /// \param[in] _satnav SatNav to move. + public: SatNav(SatNav &&_satnav) noexcept; /// \brief Destructor - public: ~Gps(); + public: ~SatNav(); /// \brief Assignment operator - /// \param[in] _gps The gps to set values from. + /// \param[in] _satnav The satnav to set values from. /// \return *this - public: Gps &operator=(const Gps &_gps); + public: SatNav &operator=(const SatNav &_satnav); /// \brief Move assignment operator - /// \param[in] _gps The gps to set values from. + /// \param[in] _satnav The satnav to set values from. /// \return *this - public: Gps &operator=(Gps &&_gps) noexcept; + public: SatNav &operator=(SatNav &&_satnav) noexcept; - /// \brief Load the gps based on an element pointer. This is *not* + /// \brief Load the satnav based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. /// \param[in] _sdf The SDF Element pointer @@ -145,19 +145,19 @@ namespace sdf /// \return Noise values public: const Noise &VerticalVelocityNoise() const; - /// \brief Return true if both Gps objects contain the same values. - /// \param[_in] _gps Gps value to compare. - /// \return True if 'this' == _gps. - public: bool operator==(const Gps &_gps) const; + /// \brief Return true if both SatNav objects contain the same values. + /// \param[_in] _satnav SatNav value to compare. + /// \return True if 'this' == _satnav. + public: bool operator==(const SatNav &_satnav) const; - /// \brief Return true this Gps object does not contain the same + /// \brief Return true this SatNav object does not contain the same /// values as the passed in parameter. - /// \param[_in] _gps Gps value to compare. - /// \return True if 'this' != _gps. - public: bool operator!=(const Gps &_gps) const; + /// \param[_in] _satnav SatNav value to compare. + /// \return True if 'this' != _satnav. + public: bool operator!=(const SatNav &_satnav) const; /// \brief Private data pointer. - private: GpsPrivate *dataPtr; + private: SatNavPrivate *dataPtr; }; } } diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 235cc943b..78f465776 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -35,7 +35,7 @@ namespace sdf class AirPressure; class Altimeter; class Camera; - class Gps; + class SatNav; class Imu; class Lidar; class Magnetometer; @@ -66,8 +66,8 @@ namespace sdf /// \brief A force-torque sensor. FORCE_TORQUE = 5, - /// \brief A GPS sensor. - GPS = 6, + /// \brief A SatNav sensor, such as GPS. + SATNAV = 6, /// \brief A GPU based lidar sensor. GPU_LIDAR = 7, @@ -320,16 +320,16 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; - /// \brief Set the GPS sensor. - /// \param[in] _gps The GPS sensor. - public: void SetGpsSensor(const Gps &_gps); + /// \brief Set the SATNAV sensor. + /// \param[in] _satnav The SATNAV sensor. + public: void SetSatNavSensor(const SatNav &_satnav); - /// \brief Get a pointer to an GPS sensor, or nullptr if the sensor - /// does not contain an GPS sensor. - /// \return Pointer to the sensor's GPS, or nullptr if the sensor - /// is not an GPS. + /// \brief Get a pointer to an SATNAV sensor, or nullptr if the sensor + /// does not contain an SATNAV sensor. + /// \return Pointer to the sensor's SATNAV, or nullptr if the sensor + /// is not an SATNAV. /// \sa SensorType Type() const - public: const Gps *GpsSensor() const; + public: const SatNav *SatNavSensor() const; /// \brief Set the IMU sensor. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 53d690481..1ea36d692 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -30,7 +30,7 @@ set (sources FrameSemantics.cc Filesystem.cc Geometry.cc - Gps.cc + SatNav.cc Gui.cc Heightmap.cc ign.cc @@ -111,7 +111,7 @@ if (BUILD_SDF_TEST) Frame_TEST.cc Filesystem_TEST.cc Geometry_TEST.cc - Gps_TEST.cc + SatNav_TEST.cc Gui_TEST.cc Heightmap_TEST.cc Imu_TEST.cc diff --git a/src/Gps_TEST.cc b/src/Gps_TEST.cc deleted file mode 100644 index 75e569928..000000000 --- a/src/Gps_TEST.cc +++ /dev/null @@ -1,115 +0,0 @@ -/* - * 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 "sdf/Gps.hh" - -///////////////////////////////////////////////// -TEST(DOMGps, Construction) -{ - sdf::Gps gps; - sdf::Noise defaultNoise; - EXPECT_EQ(defaultNoise, gps.VerticalPositionNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); -} - -///////////////////////////////////////////////// -TEST(DOMGps, Set) -{ - sdf::Gps gps; - - sdf::Noise noise; - sdf::Noise defaultNoise; - - // set random values and check they apply. - noise.SetMean(6.5); - noise.SetStdDev(3.79); - - gps.SetVerticalPositionNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetHorizontalPositionNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); - EXPECT_EQ(noise, gps.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, gps.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetVerticalVelocityNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); - EXPECT_EQ(noise, gps.HorizontalPositionNoise()); - EXPECT_EQ(noise, gps.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, gps.HorizontalVelocityNoise()); - gps.SetHorizontalVelocityNoise(noise); - EXPECT_EQ(noise, gps.VerticalPositionNoise()); - EXPECT_EQ(noise, gps.HorizontalPositionNoise()); - EXPECT_EQ(noise, gps.VerticalVelocityNoise()); - EXPECT_EQ(noise, gps.HorizontalVelocityNoise()); - - // Inequality operator - sdf::Gps gps2; - EXPECT_NE(gps2, gps); - - // Copy constructor - sdf::Gps gpsCopied(gps); - EXPECT_EQ(gpsCopied, gps); - - // Assignment operator - sdf::Gps gpsAssigned; - gpsAssigned = gps; - EXPECT_EQ(gpsAssigned, gps); - - // Move constructor - sdf::Gps gpsMoved = std::move(gps); - EXPECT_EQ(gpsCopied, gpsMoved); - - // Test nullptr private class - gps = gpsMoved; - EXPECT_EQ(gpsCopied, gps); - - // Move assignment operator - sdf::Gps gpsMoveAssigned; - gpsMoveAssigned = std::move(gpsCopied); - EXPECT_EQ(gpsAssigned, gpsMoveAssigned); - - // Test nullptr private class - gpsCopied = gpsMoveAssigned; - EXPECT_EQ(gpsAssigned, gpsCopied); -} - -///////////////////////////////////////////////// -TEST(DOMGps, Load) -{ - sdf::Gps gps; - sdf::Errors errors; - - // Null element - errors = gps.Load(nullptr); - ASSERT_EQ(1u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_EQ(nullptr, gps.Element()); - - // Bad element name - sdf::ElementPtr sdf(new sdf::Element()); - sdf->SetName("bad"); - errors = gps.Load(sdf); - ASSERT_EQ(1u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); - EXPECT_NE(nullptr, gps.Element()); -} diff --git a/src/Gps.cc b/src/SatNav.cc similarity index 66% rename from src/Gps.cc rename to src/SatNav.cc index 88f7afb22..ba2e1891d 100644 --- a/src/Gps.cc +++ b/src/SatNav.cc @@ -1,5 +1,5 @@ /* - * Copyright 2019 Open Source Robotics Foundation + * Copyright 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. @@ -14,13 +14,13 @@ * limitations under the License. * */ -#include "sdf/Gps.hh" +#include "sdf/SatNav.hh" using namespace sdf; using namespace ignition; -/// \brief Private gps data. -class sdf::GpsPrivate +/// \brief Private satnav data. +class sdf::SatNavPrivate { /// \brief Noise values for the horizontal positioning sensor public: Noise horizontalPositionNoise; @@ -39,51 +39,45 @@ class sdf::GpsPrivate }; ////////////////////////////////////////////////// -Gps::Gps() - : dataPtr(new GpsPrivate) +SatNav::SatNav() + : dataPtr(new SatNavPrivate) { } ////////////////////////////////////////////////// -Gps::~Gps() +SatNav::~SatNav() { delete this->dataPtr; this->dataPtr = nullptr; } ////////////////////////////////////////////////// -Gps::Gps(const Gps &_gps) - : dataPtr(new GpsPrivate(*_gps.dataPtr)) +SatNav::SatNav(const SatNav &_satnav) + : dataPtr(new SatNavPrivate(*_satnav.dataPtr)) { } ////////////////////////////////////////////////// -Gps::Gps(Gps &&_gps) noexcept - : dataPtr(std::exchange(_gps.dataPtr, nullptr)) +SatNav::SatNav(SatNav &&_satnav) noexcept + : dataPtr(std::exchange(_satnav.dataPtr, nullptr)) { } ////////////////////////////////////////////////// -Gps &Gps::operator=(const Gps &_gps) +SatNav &SatNav::operator=(const SatNav &_satnav) { - return *this = Gps(_gps); + return *this = SatNav(_satnav); } ////////////////////////////////////////////////// -Gps &Gps::operator=(Gps &&_gps) noexcept +SatNav &SatNav::operator=(SatNav &&_satnav) noexcept { - std::swap(this->dataPtr, _gps.dataPtr); + std::swap(this->dataPtr, _satnav.dataPtr); return * this; } ////////////////////////////////////////////////// -/// \brief Load the gps based on an element pointer. This is *not* -/// the usual entry point. Typical usage of the SDF DOM is through the Root -/// object. -/// \param[in] _sdf The SDF Element pointer -/// \return Errors, which is a vector of Error objects. Each Error includes -/// an error code and message. An empty vector indicates no error. -Errors Gps::Load(ElementPtr _sdf) +Errors SatNav::Load(ElementPtr _sdf) { Errors errors; @@ -93,22 +87,22 @@ Errors Gps::Load(ElementPtr _sdf) if (!_sdf) { errors.push_back({ErrorCode::ELEMENT_MISSING, - "Attempting to load GPS, but the provided SDF " + "Attempting to load SATNAV, but the provided SDF " "element is null."}); return errors; } - // Check that the provided SDF element is a element. + // Check that the provided SDF element is a element. // This is an error that cannot be recovered, so return an error. - if (_sdf->GetName() != "gps" ) + if (_sdf->GetName() != "satnav" && _sdf->GetName() != "gps") { errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, - "Attempting to load GPS, but the provided SDF element is " - "not a ."}); + "Attempting to load SATNAV, but the provided SDF element is " + "not a ."}); return errors; } - // Load gps sensor properties + // Load satnav sensor properties if (_sdf->HasElement("position_sensing")) { sdf::ElementPtr elem = _sdf->GetElement("position_sensing"); @@ -117,7 +111,7 @@ Errors Gps::Load(ElementPtr _sdf) sdf::ElementPtr horiz = elem->GetElement("horizontal"); if (horiz->HasElement("noise")) { - this->dataPtr->horizontalPositionNoise.Load(horiz->GetElement("noise")); + this->dataPtr->horizontalPositionNoise.Load(horiz->GetElement("noise")); } } if (elem->HasElement("vertical")) @@ -155,76 +149,76 @@ Errors Gps::Load(ElementPtr _sdf) } ////////////////////////////////////////////////// -sdf::ElementPtr Gps::Element() const +sdf::ElementPtr SatNav::Element() const { return this->dataPtr->sdf; } ////////////////////////////////////////////////// -const Noise &Gps::HorizontalPositionNoise() const +const Noise &SatNav::HorizontalPositionNoise() const { return this->dataPtr->horizontalPositionNoise; } ////////////////////////////////////////////////// -void Gps::SetHorizontalPositionNoise(const Noise &_noise) +void SatNav::SetHorizontalPositionNoise(const Noise &_noise) { this->dataPtr->horizontalPositionNoise = _noise; } ////////////////////////////////////////////////// -const Noise &Gps::HorizontalVelocityNoise() const +const Noise &SatNav::HorizontalVelocityNoise() const { return this->dataPtr->horizontalVelocityNoise; } ////////////////////////////////////////////////// -void Gps::SetHorizontalVelocityNoise(const Noise &_noise) +void SatNav::SetHorizontalVelocityNoise(const Noise &_noise) { this->dataPtr->horizontalVelocityNoise = _noise; } ////////////////////////////////////////////////// -const Noise &Gps::VerticalPositionNoise() const +const Noise &SatNav::VerticalPositionNoise() const { return this->dataPtr->verticalPositionNoise; } ////////////////////////////////////////////////// -void Gps::SetVerticalPositionNoise(const Noise &_noise) +void SatNav::SetVerticalPositionNoise(const Noise &_noise) { this->dataPtr->verticalPositionNoise = _noise; } ////////////////////////////////////////////////// -const Noise &Gps::VerticalVelocityNoise() const +const Noise &SatNav::VerticalVelocityNoise() const { return this->dataPtr->verticalVelocityNoise; } ////////////////////////////////////////////////// -void Gps::SetVerticalVelocityNoise(const Noise &_noise) +void SatNav::SetVerticalVelocityNoise(const Noise &_noise) { this->dataPtr->verticalVelocityNoise = _noise; } ////////////////////////////////////////////////// -bool Gps::operator==(const Gps &_gps) const +bool SatNav::operator==(const SatNav &_satnav) const { - if (this->dataPtr->verticalPositionNoise != _gps.VerticalPositionNoise()) + if (this->dataPtr->verticalPositionNoise != _satnav.VerticalPositionNoise()) return false; - if (this->dataPtr->horizontalPositionNoise != _gps.HorizontalPositionNoise()) + if (this->dataPtr->horizontalPositionNoise != _satnav.HorizontalPositionNoise()) return false; - if (this->dataPtr->verticalVelocityNoise != _gps.VerticalVelocityNoise()) + if (this->dataPtr->verticalVelocityNoise != _satnav.VerticalVelocityNoise()) return false; - if (this->dataPtr->horizontalVelocityNoise != _gps.HorizontalVelocityNoise()) + if (this->dataPtr->horizontalVelocityNoise != _satnav.HorizontalVelocityNoise()) return false; return true; } ////////////////////////////////////////////////// -bool Gps::operator!=(const Gps &_gps) const +bool SatNav::operator!=(const SatNav &_satnav) const { - return !(*this == _gps); + return !(*this == _satnav); } diff --git a/src/SatNav_TEST.cc b/src/SatNav_TEST.cc new file mode 100644 index 000000000..77db51dcf --- /dev/null +++ b/src/SatNav_TEST.cc @@ -0,0 +1,115 @@ +/* + * 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 "sdf/SatNav.hh" + +///////////////////////////////////////////////// +TEST(DOMSatNav, Construction) +{ + sdf::SatNav satNav; + sdf::Noise defaultNoise; + EXPECT_EQ(defaultNoise, satNav.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); +} + +///////////////////////////////////////////////// +TEST(DOMSatNav, Set) +{ + sdf::SatNav satNav; + + sdf::Noise noise; + sdf::Noise defaultNoise; + + // set random values and check they apply. + noise.SetMean(6.5); + noise.SetStdDev(3.79); + + satNav.SetVerticalPositionNoise(noise); + EXPECT_EQ(noise, satNav.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); + satNav.SetHorizontalPositionNoise(noise); + EXPECT_EQ(noise, satNav.VerticalPositionNoise()); + EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); + satNav.SetVerticalVelocityNoise(noise); + EXPECT_EQ(noise, satNav.VerticalPositionNoise()); + EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); + EXPECT_EQ(noise, satNav.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); + satNav.SetHorizontalVelocityNoise(noise); + EXPECT_EQ(noise, satNav.VerticalPositionNoise()); + EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); + EXPECT_EQ(noise, satNav.VerticalVelocityNoise()); + EXPECT_EQ(noise, satNav.HorizontalVelocityNoise()); + + // Inequality operator + sdf::SatNav satNav2; + EXPECT_NE(satNav2, satNav); + + // Copy constructor + sdf::SatNav satNavCopied(satNav); + EXPECT_EQ(satNavCopied, satNav); + + // Assignment operator + sdf::SatNav satNavAssigned; + satNavAssigned = satNav; + EXPECT_EQ(satNavAssigned, satNav); + + // Move constructor + sdf::SatNav satNavMoved = std::move(satNav); + EXPECT_EQ(satNavCopied, satNavMoved); + + // Test nullptr private class + satNav = satNavMoved; + EXPECT_EQ(satNavCopied, satNav); + + // Move assignment operator + sdf::SatNav satNavMoveAssigned; + satNavMoveAssigned = std::move(satNavCopied); + EXPECT_EQ(satNavAssigned, satNavMoveAssigned); + + // Test nullptr private class + satNavCopied = satNavMoveAssigned; + EXPECT_EQ(satNavAssigned, satNavCopied); +} + +///////////////////////////////////////////////// +TEST(DOMSatNav, Load) +{ + sdf::SatNav satNav; + sdf::Errors errors; + + // Null element + errors = satNav.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, satNav.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = satNav.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, satNav.Element()); +} diff --git a/src/Sensor.cc b/src/Sensor.cc index a720fa25a..cae4963f6 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -22,7 +22,7 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" -#include "sdf/Gps.hh" +#include "sdf/SatNav.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" @@ -42,7 +42,7 @@ const std::vector sensorTypeStrs = "contact", "depth_camera", "force_torque", - "gps", + "satnav", "gpu_lidar", "imu", "logical_camera", @@ -80,9 +80,9 @@ class sdf::SensorPrivate this->magnetometer = std::make_unique( *_sensor.magnetometer); } - if (_sensor.gps) + if (_sensor.satNav) { - this->gps = std::make_unique(*_sensor.gps); + this->satNav = std::make_unique(*_sensor.satNav); } if (_sensor.altimeter) { @@ -142,8 +142,8 @@ class sdf::SensorPrivate /// \brief Pointer to an altimeter. public: std::unique_ptr altimeter; - /// \brief Pointer to GPS sensor. - public: std::unique_ptr gps; + /// \brief Pointer to SATNAV sensor. + public: std::unique_ptr satNav; /// \brief Pointer to an air pressure sensor. public: std::unique_ptr airPressure; @@ -229,8 +229,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); - case SensorType::GPS: - return *(this->dataPtr->gps) == *(_sensor.dataPtr->gps); + case SensorType::SATNAV: + return *(this->dataPtr->satNav) == *(_sensor.dataPtr->satNav); case SensorType::CAMERA: case SensorType::DEPTH_CAMERA: case SensorType::RGBD_CAMERA: @@ -350,11 +350,12 @@ Errors Sensor::Load(ElementPtr _sdf) { this->dataPtr->type = SensorType::FORCE_TORQUE; } - else if (type == "gps") + else if (type == "satnav" || type == "gps") { - this->dataPtr->type = SensorType::GPS; - this->dataPtr->gps.reset(new Gps()); - Errors err = this->dataPtr->gps->Load(_sdf->GetElement("gps")); + this->dataPtr->type = SensorType::SATNAV; + this->dataPtr->satNav.reset(new SatNav()); + Errors err = this->dataPtr->satNav->Load( + _sdf->GetElement(_sdf->HasElement("satnav") ? "satnav" : "gps")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "gpu_ray" || type == "gpu_lidar") @@ -640,15 +641,15 @@ const Camera *Sensor::CameraSensor() const } ///////////////////////////////////////////////// -void Sensor::SetGpsSensor(const Gps &_gps) +void Sensor::SetSatNavSensor(const SatNav &_gps) { - this->dataPtr->gps = std::make_unique(_gps); + this->dataPtr->satNav = std::make_unique(_gps); } ///////////////////////////////////////////////// -const Gps *Sensor::GpsSensor() const +const SatNav *Sensor::SatNavSensor() const { - return this->dataPtr->gps.get(); + return this->dataPtr->satNav.get(); } ///////////////////////////////////////////////// diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index ea170a9a9..c4af6c40d 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -247,7 +247,7 @@ TEST(DOMSensor, Type) sdf::SensorType::CONTACT, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::FORCE_TORQUE, - sdf::SensorType::GPS, + sdf::SensorType::SATNAV, sdf::SensorType::GPU_LIDAR, sdf::SensorType::IMU, sdf::SensorType::LOGICAL_CAMERA, @@ -269,7 +269,7 @@ TEST(DOMSensor, Type) "contact", "depth_camera", "force_torque", - "gps", + "satnav", "gpu_lidar", "imu", "logical_camera", diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 045ad3ace..b8ab1903b 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -27,7 +27,7 @@ #include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/Imu.hh" -#include "sdf/Gps.hh" +#include "sdf/SatNav.hh" #include "sdf/Link.hh" #include "sdf/Magnetometer.hh" #include "sdf/Model.hh" @@ -360,23 +360,23 @@ TEST(DOMLink, Sensors) EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), forceTorqueSensor->RawPose()); - // Get the gps sensor - const sdf::Sensor *gpsSensor = link->SensorByName("gps_sensor"); - ASSERT_NE(nullptr, gpsSensor); - EXPECT_EQ("gps_sensor", gpsSensor->Name()); - EXPECT_EQ(sdf::SensorType::GPS, gpsSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), gpsSensor->RawPose()); - const sdf::Gps *gpsSensorObj = gpsSensor->GpsSensor(); - ASSERT_NE(nullptr, gpsSensorObj); - - EXPECT_DOUBLE_EQ(1.2, gpsSensorObj->HorizontalPositionNoise().Mean()); - EXPECT_DOUBLE_EQ(3.4, gpsSensorObj->HorizontalPositionNoise().StdDev()); - EXPECT_DOUBLE_EQ(5.6, gpsSensorObj->VerticalPositionNoise().Mean()); - EXPECT_DOUBLE_EQ(7.8, gpsSensorObj->VerticalPositionNoise().StdDev()); - EXPECT_DOUBLE_EQ(9.1, gpsSensorObj->HorizontalVelocityNoise().Mean()); - EXPECT_DOUBLE_EQ(10.11, gpsSensorObj->HorizontalVelocityNoise().StdDev()); - EXPECT_DOUBLE_EQ(12.13, gpsSensorObj->VerticalVelocityNoise().Mean()); - EXPECT_DOUBLE_EQ(14.15, gpsSensorObj->VerticalVelocityNoise().StdDev()); + // Get the satnav sensor + const sdf::Sensor *satNavSensor = link->SensorByName("satnav_sensor"); + ASSERT_NE(nullptr, satNavSensor); + EXPECT_EQ("satnav_sensor", satNavSensor->Name()); + EXPECT_EQ(sdf::SensorType::SATNAV, satNavSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), satNavSensor->RawPose()); + const sdf::SatNav *satNavSensorObj = satNavSensor->SatNavSensor(); + ASSERT_NE(nullptr, satNavSensorObj); + + EXPECT_DOUBLE_EQ(1.2, satNavSensorObj->HorizontalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(3.4, satNavSensorObj->HorizontalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.6, satNavSensorObj->VerticalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(7.8, satNavSensorObj->VerticalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(9.1, satNavSensorObj->HorizontalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(10.11, satNavSensorObj->HorizontalVelocityNoise().StdDev()); + EXPECT_DOUBLE_EQ(12.13, satNavSensorObj->VerticalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(14.15, satNavSensorObj->VerticalVelocityNoise().StdDev()); // Get the gpu_ray sensor const sdf::Sensor *gpuRaySensor = link->SensorByName("gpu_ray_sensor"); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index 9590eb8fb..b435945b3 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -141,7 +141,7 @@ 10 11 12 0 0 0 - + 13 14 15 0 0 0 From 6f6648c3729782d1ea7a85af0071bf994bf8e285 Mon Sep 17 00:00:00 2001 From: Dre Westcook Date: Tue, 9 Feb 2021 19:30:03 +1100 Subject: [PATCH 6/8] changed for line limit --- src/SatNav.cc | 6 ++++-- test/integration/link_dom.cc | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/SatNav.cc b/src/SatNav.cc index ba2e1891d..c59105359 100644 --- a/src/SatNav.cc +++ b/src/SatNav.cc @@ -207,11 +207,13 @@ bool SatNav::operator==(const SatNav &_satnav) const { if (this->dataPtr->verticalPositionNoise != _satnav.VerticalPositionNoise()) return false; - if (this->dataPtr->horizontalPositionNoise != _satnav.HorizontalPositionNoise()) + if (this->dataPtr->horizontalPositionNoise != + _satnav.HorizontalPositionNoise()) return false; if (this->dataPtr->verticalVelocityNoise != _satnav.VerticalVelocityNoise()) return false; - if (this->dataPtr->horizontalVelocityNoise != _satnav.HorizontalVelocityNoise()) + if (this->dataPtr->horizontalVelocityNoise != + _satnav.HorizontalVelocityNoise()) return false; return true; diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index b8ab1903b..0f2ffedf1 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -365,7 +365,8 @@ TEST(DOMLink, Sensors) ASSERT_NE(nullptr, satNavSensor); EXPECT_EQ("satnav_sensor", satNavSensor->Name()); EXPECT_EQ(sdf::SensorType::SATNAV, satNavSensor->Type()); - EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), satNavSensor->RawPose()); + EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), + satNavSensor->RawPose()); const sdf::SatNav *satNavSensorObj = satNavSensor->SatNavSensor(); ASSERT_NE(nullptr, satNavSensorObj); From 12526b76b557e0820640417d21e5765be555499b Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Mon, 14 Jun 2021 19:05:01 -0400 Subject: [PATCH 7/8] rename satnav to navsat, and other review commments Signed-off-by: Ashton Larkin --- include/sdf/CMakeLists.txt | 2 +- include/sdf/{SatNav.hh => NavSat.hh} | 64 +++++++-------- include/sdf/Sensor.hh | 28 ++++--- sdf/1.7/navsat.sdf | 40 ++++++++++ sdf/1.7/sensor.sdf | 3 +- src/CMakeLists.txt | 4 +- src/{SatNav.cc => NavSat.cc} | 75 +++++++++-------- src/NavSat_TEST.cc | 115 +++++++++++++++++++++++++++ src/SatNav_TEST.cc | 115 --------------------------- src/Sensor.cc | 37 ++++----- src/Sensor_TEST.cc | 4 +- test/integration/link_dom.cc | 36 ++++----- test/sdf/sensors.sdf | 4 +- 13 files changed, 285 insertions(+), 242 deletions(-) rename include/sdf/{SatNav.hh => NavSat.hh} (74%) create mode 100644 sdf/1.7/navsat.sdf rename src/{SatNav.cc => NavSat.cc} (72%) create mode 100644 src/NavSat_TEST.cc delete mode 100644 src/SatNav_TEST.cc diff --git a/include/sdf/CMakeLists.txt b/include/sdf/CMakeLists.txt index a93b18a9d..4f0bcdc5f 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -17,7 +17,6 @@ set (headers Filesystem.hh Frame.hh Geometry.hh - SatNav.hh Gui.hh Heightmap.hh Imu.hh @@ -30,6 +29,7 @@ set (headers Material.hh Mesh.hh Model.hh + NavSat.hh Noise.hh Param.hh parser.hh diff --git a/include/sdf/SatNav.hh b/include/sdf/NavSat.hh similarity index 74% rename from include/sdf/SatNav.hh rename to include/sdf/NavSat.hh index 282082517..d22c93a6b 100644 --- a/include/sdf/SatNav.hh +++ b/include/sdf/NavSat.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef SDF_SATNAV_HH_ -#define SDF_SATNAV_HH_ +#ifndef SDF_NAVSAT_HH_ +#define SDF_NAVSAT_HH_ #include #include @@ -29,19 +29,19 @@ namespace sdf // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // - class SatNavPrivate; + class NavSatPrivate; - /// \brief SatNav contains information about a SatNav sensor. - /// This sensor can be attached to a link. The SatNav sensor can be defined - /// SDF XML by the "satnav" type. + /// \brief NavSat contains information about a NavSat sensor. + /// This sensor can be attached to a link. The NavSat sensor can be defined + /// SDF XML by the "navsat" type. /// - /// # Example SDF XML using satnav type: + /// # Example SDF XML using navsat type: /// /// ~~~{.xml} - /// + /// /// 1 2 3 0 0 0 - /// /satnav - /// + /// /navsat + /// /// /// /// @@ -70,36 +70,36 @@ namespace sdf /// /// /// - /// + /// /// /// ~~~ - class SDFORMAT_VISIBLE SatNav + class SDFORMAT_VISIBLE NavSat { /// \brief Default constructor - public: SatNav(); + public: NavSat(); /// \brief Copy constructor - /// \param[in] _satnav SatNav to copy. - public: SatNav(const SatNav &_satnav); + /// \param[in] _navsat NavSat to copy. + public: NavSat(const NavSat &_navsat); /// \brief Move constructor - /// \param[in] _satnav SatNav to move. - public: SatNav(SatNav &&_satnav) noexcept; + /// \param[in] _navsat NavSat to move. + public: NavSat(NavSat &&_navsat) noexcept; /// \brief Destructor - public: ~SatNav(); + public: ~NavSat(); /// \brief Assignment operator - /// \param[in] _satnav The satnav to set values from. + /// \param[in] _navsat The navsat to set values from. /// \return *this - public: SatNav &operator=(const SatNav &_satnav); + public: NavSat &operator=(const NavSat &_navsat); /// \brief Move assignment operator - /// \param[in] _satnav The satnav to set values from. + /// \param[in] _navsat The navsat to set values from. /// \return *this - public: SatNav &operator=(SatNav &&_satnav) noexcept; + public: NavSat &operator=(NavSat &&_navsat) noexcept; - /// \brief Load the satnav based on an element pointer. This is *not* + /// \brief Load the navsat based on an element pointer. This is *not* /// the usual entry point. Typical usage of the SDF DOM is through the Root /// object. /// \param[in] _sdf The SDF Element pointer @@ -145,19 +145,19 @@ namespace sdf /// \return Noise values public: const Noise &VerticalVelocityNoise() const; - /// \brief Return true if both SatNav objects contain the same values. - /// \param[_in] _satnav SatNav value to compare. - /// \return True if 'this' == _satnav. - public: bool operator==(const SatNav &_satnav) const; + /// \brief Return true if both NavSat objects contain the same values. + /// \param[_in] _navsat NavSat value to compare. + /// \return True if 'this' == _navsat. + public: bool operator==(const NavSat &_navsat) const; - /// \brief Return true this SatNav object does not contain the same + /// \brief Return true this NavSat object does not contain the same /// values as the passed in parameter. - /// \param[_in] _satnav SatNav value to compare. - /// \return True if 'this' != _satnav. - public: bool operator!=(const SatNav &_satnav) const; + /// \param[_in] _navsat NavSat value to compare. + /// \return True if 'this' != _navsat. + public: bool operator!=(const NavSat &_navsat) const; /// \brief Private data pointer. - private: SatNavPrivate *dataPtr; + private: NavSatPrivate *dataPtr; }; } } diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 78f465776..3b3b3be0d 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -35,10 +35,10 @@ namespace sdf class AirPressure; class Altimeter; class Camera; - class SatNav; class Imu; class Lidar; class Magnetometer; + class NavSat; class SensorPrivate; struct PoseRelativeToGraph; @@ -66,8 +66,8 @@ namespace sdf /// \brief A force-torque sensor. FORCE_TORQUE = 5, - /// \brief A SatNav sensor, such as GPS. - SATNAV = 6, + /// \brief A GPS sensor. + GPS = 6, /// \brief A GPU based lidar sensor. GPU_LIDAR = 7, @@ -110,7 +110,10 @@ namespace sdf RGBD_CAMERA = 19, /// \brief A thermal camera sensor - THERMAL_CAMERA = 20 + THERMAL_CAMERA = 20, + + /// \brief A NavSat sensor, such as GPS. + NAVSAT = 21 }; /// \brief Information about an SDF sensor. @@ -320,17 +323,16 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; - /// \brief Set the SATNAV sensor. - /// \param[in] _satnav The SATNAV sensor. - public: void SetSatNavSensor(const SatNav &_satnav); + /// \brief Set the NAVSAT sensor. + /// \param[in] _navsat The NAVSAT sensor. + public: void SetNavSatSensor(const NavSat &_navsat); - /// \brief Get a pointer to an SATNAV sensor, or nullptr if the sensor - /// does not contain an SATNAV sensor. - /// \return Pointer to the sensor's SATNAV, or nullptr if the sensor - /// is not an SATNAV. + /// \brief Get a pointer to a NAVSAT sensor, or nullptr if the sensor + /// does not contain an NAVSAT sensor. + /// \return Pointer to the sensor's NAVSAT, or nullptr if the sensor + /// is not an NAVSAT. /// \sa SensorType Type() const - public: const SatNav *SatNavSensor() const; - + public: const NavSat *NavSatSensor() const; /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. diff --git a/sdf/1.7/navsat.sdf b/sdf/1.7/navsat.sdf new file mode 100644 index 000000000..8a8f1719b --- /dev/null +++ b/sdf/1.7/navsat.sdf @@ -0,0 +1,40 @@ + + These elements are specific to the NAVSAT sensor. + + + + Parameters related to NAVSAT position measurement. + + + + Noise parameters for horizontal position measurement, in units of meters. + + + + + + Noise parameters for vertical position measurement, in units of meters. + + + + + + + + Parameters related to NAVSAT position measurement. + + + + Noise parameters for horizontal velocity measurement, in units of meters/second. + + + + + + Noise parameters for vertical velocity measurement, in units of meters/second. + + + + + + diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf index 12d0edade..94fe68a33 100644 --- a/sdf/1.7/sensor.sdf +++ b/sdf/1.7/sensor.sdf @@ -22,6 +22,7 @@ logical_camera, magnetometer, multicamera, + navsat, ray, rfid, rfidtag, @@ -30,7 +31,7 @@ thermal_camera, thermal, wireless_receiver, and wireless_transmitter. - The "ray" and "gpu_ray" types are equivalent to "lidar" and "gpu_lidar", respectively. It is preferred to use "lidar" and "gpu_lidar" since "ray" and "gpu_ray" will be deprecated. The "ray" and "gpu_ray" types are maintained for legacy support. + The "ray", "gpu_ray", and "gps" types are equivalent to "lidar", "gpu_lidar", and "navsat", respectively. It is preferred to use "lidar", "gpu_lidar", and "navsat" since "ray", "gpu_ray", and "gps" will be deprecated. The "ray", "gpu_ray", and "gps" types are maintained for legacy support. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1ea36d692..8329a5cf3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -30,7 +30,6 @@ set (sources FrameSemantics.cc Filesystem.cc Geometry.cc - SatNav.cc Gui.cc Heightmap.cc ign.cc @@ -44,6 +43,7 @@ set (sources Material.cc Mesh.cc Model.cc + NavSat.cc Noise.cc parser.cc parser_urdf.cc @@ -111,7 +111,6 @@ if (BUILD_SDF_TEST) Frame_TEST.cc Filesystem_TEST.cc Geometry_TEST.cc - SatNav_TEST.cc Gui_TEST.cc Heightmap_TEST.cc Imu_TEST.cc @@ -124,6 +123,7 @@ if (BUILD_SDF_TEST) Material_TEST.cc Mesh_TEST.cc Model_TEST.cc + NavSat_TEST.cc Noise_TEST.cc parser_urdf_TEST.cc Param_TEST.cc diff --git a/src/SatNav.cc b/src/NavSat.cc similarity index 72% rename from src/SatNav.cc rename to src/NavSat.cc index c59105359..088e5d937 100644 --- a/src/SatNav.cc +++ b/src/NavSat.cc @@ -14,13 +14,13 @@ * limitations under the License. * */ -#include "sdf/SatNav.hh" +#include "sdf/NavSat.hh" using namespace sdf; using namespace ignition; -/// \brief Private satnav data. -class sdf::SatNavPrivate +/// \brief Private navsat data. +class sdf::NavSatPrivate { /// \brief Noise values for the horizontal positioning sensor public: Noise horizontalPositionNoise; @@ -39,45 +39,45 @@ class sdf::SatNavPrivate }; ////////////////////////////////////////////////// -SatNav::SatNav() - : dataPtr(new SatNavPrivate) +NavSat::NavSat() + : dataPtr(new NavSatPrivate) { } ////////////////////////////////////////////////// -SatNav::~SatNav() +NavSat::~NavSat() { delete this->dataPtr; this->dataPtr = nullptr; } ////////////////////////////////////////////////// -SatNav::SatNav(const SatNav &_satnav) - : dataPtr(new SatNavPrivate(*_satnav.dataPtr)) +NavSat::NavSat(const NavSat &_navsat) + : dataPtr(new NavSatPrivate(*_navsat.dataPtr)) { } ////////////////////////////////////////////////// -SatNav::SatNav(SatNav &&_satnav) noexcept - : dataPtr(std::exchange(_satnav.dataPtr, nullptr)) +NavSat::NavSat(NavSat &&_navsat) noexcept + : dataPtr(std::exchange(_navsat.dataPtr, nullptr)) { } ////////////////////////////////////////////////// -SatNav &SatNav::operator=(const SatNav &_satnav) +NavSat &NavSat::operator=(const NavSat &_navsat) { - return *this = SatNav(_satnav); + return *this = NavSat(_navsat); } ////////////////////////////////////////////////// -SatNav &SatNav::operator=(SatNav &&_satnav) noexcept +NavSat &NavSat::operator=(NavSat &&_navsat) noexcept { - std::swap(this->dataPtr, _satnav.dataPtr); + std::swap(this->dataPtr, _navsat.dataPtr); return * this; } ////////////////////////////////////////////////// -Errors SatNav::Load(ElementPtr _sdf) +Errors NavSat::Load(ElementPtr _sdf) { Errors errors; @@ -87,22 +87,22 @@ Errors SatNav::Load(ElementPtr _sdf) if (!_sdf) { errors.push_back({ErrorCode::ELEMENT_MISSING, - "Attempting to load SATNAV, but the provided SDF " + "Attempting to load NAVSAT, but the provided SDF " "element is null."}); return errors; } - // Check that the provided SDF element is a element. + // Check that the provided SDF element is a element. // This is an error that cannot be recovered, so return an error. - if (_sdf->GetName() != "satnav" && _sdf->GetName() != "gps") + if (_sdf->GetName() != "navsat" && _sdf->GetName() != "gps") { errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, - "Attempting to load SATNAV, but the provided SDF element is " - "not a ."}); + "Attempting to load NAVSAT, but the provided SDF element is " + "not a ."}); return errors; } - // Load satnav sensor properties + // Load navsat sensor properties if (_sdf->HasElement("position_sensing")) { sdf::ElementPtr elem = _sdf->GetElement("position_sensing"); @@ -144,83 +144,82 @@ Errors SatNav::Load(ElementPtr _sdf) } } - return errors; } ////////////////////////////////////////////////// -sdf::ElementPtr SatNav::Element() const +sdf::ElementPtr NavSat::Element() const { return this->dataPtr->sdf; } ////////////////////////////////////////////////// -const Noise &SatNav::HorizontalPositionNoise() const +const Noise &NavSat::HorizontalPositionNoise() const { return this->dataPtr->horizontalPositionNoise; } ////////////////////////////////////////////////// -void SatNav::SetHorizontalPositionNoise(const Noise &_noise) +void NavSat::SetHorizontalPositionNoise(const Noise &_noise) { this->dataPtr->horizontalPositionNoise = _noise; } ////////////////////////////////////////////////// -const Noise &SatNav::HorizontalVelocityNoise() const +const Noise &NavSat::HorizontalVelocityNoise() const { return this->dataPtr->horizontalVelocityNoise; } ////////////////////////////////////////////////// -void SatNav::SetHorizontalVelocityNoise(const Noise &_noise) +void NavSat::SetHorizontalVelocityNoise(const Noise &_noise) { this->dataPtr->horizontalVelocityNoise = _noise; } ////////////////////////////////////////////////// -const Noise &SatNav::VerticalPositionNoise() const +const Noise &NavSat::VerticalPositionNoise() const { return this->dataPtr->verticalPositionNoise; } ////////////////////////////////////////////////// -void SatNav::SetVerticalPositionNoise(const Noise &_noise) +void NavSat::SetVerticalPositionNoise(const Noise &_noise) { this->dataPtr->verticalPositionNoise = _noise; } ////////////////////////////////////////////////// -const Noise &SatNav::VerticalVelocityNoise() const +const Noise &NavSat::VerticalVelocityNoise() const { return this->dataPtr->verticalVelocityNoise; } ////////////////////////////////////////////////// -void SatNav::SetVerticalVelocityNoise(const Noise &_noise) +void NavSat::SetVerticalVelocityNoise(const Noise &_noise) { this->dataPtr->verticalVelocityNoise = _noise; } ////////////////////////////////////////////////// -bool SatNav::operator==(const SatNav &_satnav) const +bool NavSat::operator==(const NavSat &_navsat) const { - if (this->dataPtr->verticalPositionNoise != _satnav.VerticalPositionNoise()) + if (this->dataPtr->verticalPositionNoise != _navsat.VerticalPositionNoise()) return false; if (this->dataPtr->horizontalPositionNoise != - _satnav.HorizontalPositionNoise()) + _navsat.HorizontalPositionNoise()) return false; - if (this->dataPtr->verticalVelocityNoise != _satnav.VerticalVelocityNoise()) + if (this->dataPtr->verticalVelocityNoise != _navsat.VerticalVelocityNoise()) return false; if (this->dataPtr->horizontalVelocityNoise != - _satnav.HorizontalVelocityNoise()) + _navsat.HorizontalVelocityNoise()) return false; return true; } ////////////////////////////////////////////////// -bool SatNav::operator!=(const SatNav &_satnav) const +bool NavSat::operator!=(const NavSat &_navsat) const { - return !(*this == _satnav); + return !(*this == _navsat); } diff --git a/src/NavSat_TEST.cc b/src/NavSat_TEST.cc new file mode 100644 index 000000000..a4d10e925 --- /dev/null +++ b/src/NavSat_TEST.cc @@ -0,0 +1,115 @@ +/* + * 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 "sdf/NavSat.hh" + +///////////////////////////////////////////////// +TEST(DOMNavSat, Construction) +{ + sdf::NavSat navSat; + sdf::Noise defaultNoise; + EXPECT_EQ(defaultNoise, navSat.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, navSat.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalVelocityNoise()); +} + +///////////////////////////////////////////////// +TEST(DOMNavSat, Set) +{ + sdf::NavSat navSat; + + sdf::Noise noise; + sdf::Noise defaultNoise; + + // set random values and check they apply. + noise.SetMean(6.5); + noise.SetStdDev(3.79); + + navSat.SetVerticalPositionNoise(noise); + EXPECT_EQ(noise, navSat.VerticalPositionNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, navSat.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalVelocityNoise()); + navSat.SetHorizontalPositionNoise(noise); + EXPECT_EQ(noise, navSat.VerticalPositionNoise()); + EXPECT_EQ(noise, navSat.HorizontalPositionNoise()); + EXPECT_EQ(defaultNoise, navSat.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalVelocityNoise()); + navSat.SetVerticalVelocityNoise(noise); + EXPECT_EQ(noise, navSat.VerticalPositionNoise()); + EXPECT_EQ(noise, navSat.HorizontalPositionNoise()); + EXPECT_EQ(noise, navSat.VerticalVelocityNoise()); + EXPECT_EQ(defaultNoise, navSat.HorizontalVelocityNoise()); + navSat.SetHorizontalVelocityNoise(noise); + EXPECT_EQ(noise, navSat.VerticalPositionNoise()); + EXPECT_EQ(noise, navSat.HorizontalPositionNoise()); + EXPECT_EQ(noise, navSat.VerticalVelocityNoise()); + EXPECT_EQ(noise, navSat.HorizontalVelocityNoise()); + + // Inequality operator + sdf::NavSat navSat2; + EXPECT_NE(navSat2, navSat); + + // Copy constructor + sdf::NavSat navSatCopied(navSat); + EXPECT_EQ(navSatCopied, navSat); + + // Assignment operator + sdf::NavSat navSatAssigned; + navSatAssigned = navSat; + EXPECT_EQ(navSatAssigned, navSat); + + // Move constructor + sdf::NavSat navSatMoved = std::move(navSat); + EXPECT_EQ(navSatCopied, navSatMoved); + + // Test nullptr private class + navSat = navSatMoved; + EXPECT_EQ(navSatCopied, navSat); + + // Move assignment operator + sdf::NavSat navSatMoveAssigned; + navSatMoveAssigned = std::move(navSatCopied); + EXPECT_EQ(navSatAssigned, navSatMoveAssigned); + + // Test nullptr private class + navSatCopied = navSatMoveAssigned; + EXPECT_EQ(navSatAssigned, navSatCopied); +} + +///////////////////////////////////////////////// +TEST(DOMNavSat, Load) +{ + sdf::NavSat navSat; + sdf::Errors errors; + + // Null element + errors = navSat.Load(nullptr); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); + EXPECT_EQ(nullptr, navSat.Element()); + + // Bad element name + sdf::ElementPtr sdf(new sdf::Element()); + sdf->SetName("bad"); + errors = navSat.Load(sdf); + ASSERT_EQ(1u, errors.size()); + EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); + EXPECT_NE(nullptr, navSat.Element()); +} diff --git a/src/SatNav_TEST.cc b/src/SatNav_TEST.cc deleted file mode 100644 index 77db51dcf..000000000 --- a/src/SatNav_TEST.cc +++ /dev/null @@ -1,115 +0,0 @@ -/* - * 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 "sdf/SatNav.hh" - -///////////////////////////////////////////////// -TEST(DOMSatNav, Construction) -{ - sdf::SatNav satNav; - sdf::Noise defaultNoise; - EXPECT_EQ(defaultNoise, satNav.VerticalPositionNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); -} - -///////////////////////////////////////////////// -TEST(DOMSatNav, Set) -{ - sdf::SatNav satNav; - - sdf::Noise noise; - sdf::Noise defaultNoise; - - // set random values and check they apply. - noise.SetMean(6.5); - noise.SetStdDev(3.79); - - satNav.SetVerticalPositionNoise(noise); - EXPECT_EQ(noise, satNav.VerticalPositionNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); - satNav.SetHorizontalPositionNoise(noise); - EXPECT_EQ(noise, satNav.VerticalPositionNoise()); - EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); - EXPECT_EQ(defaultNoise, satNav.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); - satNav.SetVerticalVelocityNoise(noise); - EXPECT_EQ(noise, satNav.VerticalPositionNoise()); - EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); - EXPECT_EQ(noise, satNav.VerticalVelocityNoise()); - EXPECT_EQ(defaultNoise, satNav.HorizontalVelocityNoise()); - satNav.SetHorizontalVelocityNoise(noise); - EXPECT_EQ(noise, satNav.VerticalPositionNoise()); - EXPECT_EQ(noise, satNav.HorizontalPositionNoise()); - EXPECT_EQ(noise, satNav.VerticalVelocityNoise()); - EXPECT_EQ(noise, satNav.HorizontalVelocityNoise()); - - // Inequality operator - sdf::SatNav satNav2; - EXPECT_NE(satNav2, satNav); - - // Copy constructor - sdf::SatNav satNavCopied(satNav); - EXPECT_EQ(satNavCopied, satNav); - - // Assignment operator - sdf::SatNav satNavAssigned; - satNavAssigned = satNav; - EXPECT_EQ(satNavAssigned, satNav); - - // Move constructor - sdf::SatNav satNavMoved = std::move(satNav); - EXPECT_EQ(satNavCopied, satNavMoved); - - // Test nullptr private class - satNav = satNavMoved; - EXPECT_EQ(satNavCopied, satNav); - - // Move assignment operator - sdf::SatNav satNavMoveAssigned; - satNavMoveAssigned = std::move(satNavCopied); - EXPECT_EQ(satNavAssigned, satNavMoveAssigned); - - // Test nullptr private class - satNavCopied = satNavMoveAssigned; - EXPECT_EQ(satNavAssigned, satNavCopied); -} - -///////////////////////////////////////////////// -TEST(DOMSatNav, Load) -{ - sdf::SatNav satNav; - sdf::Errors errors; - - // Null element - errors = satNav.Load(nullptr); - ASSERT_EQ(1u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code()); - EXPECT_EQ(nullptr, satNav.Element()); - - // Bad element name - sdf::ElementPtr sdf(new sdf::Element()); - sdf->SetName("bad"); - errors = satNav.Load(sdf); - ASSERT_EQ(1u, errors.size()); - EXPECT_EQ(sdf::ErrorCode::ELEMENT_INCORRECT_TYPE, errors[0].Code()); - EXPECT_NE(nullptr, satNav.Element()); -} diff --git a/src/Sensor.cc b/src/Sensor.cc index cae4963f6..6096d415b 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -22,7 +22,7 @@ #include "sdf/Altimeter.hh" #include "sdf/Camera.hh" #include "sdf/Error.hh" -#include "sdf/SatNav.hh" +#include "sdf/NavSat.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" @@ -42,7 +42,7 @@ const std::vector sensorTypeStrs = "contact", "depth_camera", "force_torque", - "satnav", + "gps", "gpu_lidar", "imu", "logical_camera", @@ -56,7 +56,8 @@ const std::vector sensorTypeStrs = "wireless_transmitter", "air_pressure", "rgbd_camera", - "thermal_camera" + "thermal_camera", + "navsat" }; class sdf::SensorPrivate @@ -80,9 +81,9 @@ class sdf::SensorPrivate this->magnetometer = std::make_unique( *_sensor.magnetometer); } - if (_sensor.satNav) + if (_sensor.navSat) { - this->satNav = std::make_unique(*_sensor.satNav); + this->navSat = std::make_unique(*_sensor.navSat); } if (_sensor.altimeter) { @@ -142,8 +143,8 @@ class sdf::SensorPrivate /// \brief Pointer to an altimeter. public: std::unique_ptr altimeter; - /// \brief Pointer to SATNAV sensor. - public: std::unique_ptr satNav; + /// \brief Pointer to NAVSAT sensor. + public: std::unique_ptr navSat; /// \brief Pointer to an air pressure sensor. public: std::unique_ptr airPressure; @@ -229,8 +230,8 @@ bool Sensor::operator==(const Sensor &_sensor) const return *(this->dataPtr->airPressure) == *(_sensor.dataPtr->airPressure); case SensorType::IMU: return *(this->dataPtr->imu) == *(_sensor.dataPtr->imu); - case SensorType::SATNAV: - return *(this->dataPtr->satNav) == *(_sensor.dataPtr->satNav); + case SensorType::NAVSAT: + return *(this->dataPtr->navSat) == *(_sensor.dataPtr->navSat); case SensorType::CAMERA: case SensorType::DEPTH_CAMERA: case SensorType::RGBD_CAMERA: @@ -350,12 +351,12 @@ Errors Sensor::Load(ElementPtr _sdf) { this->dataPtr->type = SensorType::FORCE_TORQUE; } - else if (type == "satnav" || type == "gps") + else if (type == "navsat" || type == "gps") { - this->dataPtr->type = SensorType::SATNAV; - this->dataPtr->satNav.reset(new SatNav()); - Errors err = this->dataPtr->satNav->Load( - _sdf->GetElement(_sdf->HasElement("satnav") ? "satnav" : "gps")); + this->dataPtr->type = SensorType::NAVSAT; + this->dataPtr->navSat.reset(new NavSat()); + Errors err = this->dataPtr->navSat->Load( + _sdf->GetElement(_sdf->HasElement("navsat") ? "navsat" : "gps")); errors.insert(errors.end(), err.begin(), err.end()); } else if (type == "gpu_ray" || type == "gpu_lidar") @@ -641,15 +642,15 @@ const Camera *Sensor::CameraSensor() const } ///////////////////////////////////////////////// -void Sensor::SetSatNavSensor(const SatNav &_gps) +void Sensor::SetNavSatSensor(const NavSat &_gps) { - this->dataPtr->satNav = std::make_unique(_gps); + this->dataPtr->navSat = std::make_unique(_gps); } ///////////////////////////////////////////////// -const SatNav *Sensor::SatNavSensor() const +const NavSat *Sensor::NavSatSensor() const { - return this->dataPtr->satNav.get(); + return this->dataPtr->navSat.get(); } ///////////////////////////////////////////////// diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index c4af6c40d..512780ca8 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -247,7 +247,7 @@ TEST(DOMSensor, Type) sdf::SensorType::CONTACT, sdf::SensorType::DEPTH_CAMERA, sdf::SensorType::FORCE_TORQUE, - sdf::SensorType::SATNAV, + sdf::SensorType::NAVSAT, sdf::SensorType::GPU_LIDAR, sdf::SensorType::IMU, sdf::SensorType::LOGICAL_CAMERA, @@ -269,7 +269,7 @@ TEST(DOMSensor, Type) "contact", "depth_camera", "force_torque", - "satnav", + "navsat", "gpu_lidar", "imu", "logical_camera", diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 0f2ffedf1..f8020abbd 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -27,7 +27,7 @@ #include "sdf/Error.hh" #include "sdf/Filesystem.hh" #include "sdf/Imu.hh" -#include "sdf/SatNav.hh" +#include "sdf/NavSat.hh" #include "sdf/Link.hh" #include "sdf/Magnetometer.hh" #include "sdf/Model.hh" @@ -360,24 +360,24 @@ TEST(DOMLink, Sensors) EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), forceTorqueSensor->RawPose()); - // Get the satnav sensor - const sdf::Sensor *satNavSensor = link->SensorByName("satnav_sensor"); - ASSERT_NE(nullptr, satNavSensor); - EXPECT_EQ("satnav_sensor", satNavSensor->Name()); - EXPECT_EQ(sdf::SensorType::SATNAV, satNavSensor->Type()); + // Get the navsat sensor + const sdf::Sensor *navSatSensor = link->SensorByName("navsat_sensor"); + ASSERT_NE(nullptr, navSatSensor); + EXPECT_EQ("navsat_sensor", navSatSensor->Name()); + EXPECT_EQ(sdf::SensorType::NAVSAT, navSatSensor->Type()); EXPECT_EQ(ignition::math::Pose3d(13, 14, 15, 0, 0, 0), - satNavSensor->RawPose()); - const sdf::SatNav *satNavSensorObj = satNavSensor->SatNavSensor(); - ASSERT_NE(nullptr, satNavSensorObj); - - EXPECT_DOUBLE_EQ(1.2, satNavSensorObj->HorizontalPositionNoise().Mean()); - EXPECT_DOUBLE_EQ(3.4, satNavSensorObj->HorizontalPositionNoise().StdDev()); - EXPECT_DOUBLE_EQ(5.6, satNavSensorObj->VerticalPositionNoise().Mean()); - EXPECT_DOUBLE_EQ(7.8, satNavSensorObj->VerticalPositionNoise().StdDev()); - EXPECT_DOUBLE_EQ(9.1, satNavSensorObj->HorizontalVelocityNoise().Mean()); - EXPECT_DOUBLE_EQ(10.11, satNavSensorObj->HorizontalVelocityNoise().StdDev()); - EXPECT_DOUBLE_EQ(12.13, satNavSensorObj->VerticalVelocityNoise().Mean()); - EXPECT_DOUBLE_EQ(14.15, satNavSensorObj->VerticalVelocityNoise().StdDev()); + navSatSensor->RawPose()); + const sdf::NavSat *navSatSensorObj = navSatSensor->NavSatSensor(); + ASSERT_NE(nullptr, navSatSensorObj); + + EXPECT_DOUBLE_EQ(1.2, navSatSensorObj->HorizontalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(3.4, navSatSensorObj->HorizontalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.6, navSatSensorObj->VerticalPositionNoise().Mean()); + EXPECT_DOUBLE_EQ(7.8, navSatSensorObj->VerticalPositionNoise().StdDev()); + EXPECT_DOUBLE_EQ(9.1, navSatSensorObj->HorizontalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(10.11, navSatSensorObj->HorizontalVelocityNoise().StdDev()); + EXPECT_DOUBLE_EQ(12.13, navSatSensorObj->VerticalVelocityNoise().Mean()); + EXPECT_DOUBLE_EQ(14.15, navSatSensorObj->VerticalVelocityNoise().StdDev()); // Get the gpu_ray sensor const sdf::Sensor *gpuRaySensor = link->SensorByName("gpu_ray_sensor"); diff --git a/test/sdf/sensors.sdf b/test/sdf/sensors.sdf index b435945b3..1f7f84f64 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -141,7 +141,7 @@ 10 11 12 0 0 0 - + 13 14 15 0 0 0 @@ -338,7 +338,7 @@ - + 4 5 6 0 0 0 From b98dbac49c9f03792f6f1f6ad680fbe767dbdc22 Mon Sep 17 00:00:00 2001 From: Ashton Larkin Date: Tue, 15 Jun 2021 11:13:45 -0400 Subject: [PATCH 8/8] add missing navsat.sdf include in sensor.sdf Signed-off-by: Ashton Larkin --- sdf/1.7/sensor.sdf | 1 + 1 file changed, 1 insertion(+) diff --git a/sdf/1.7/sensor.sdf b/sdf/1.7/sensor.sdf index 94fe68a33..63deb5ba0 100644 --- a/sdf/1.7/sensor.sdf +++ b/sdf/1.7/sensor.sdf @@ -62,6 +62,7 @@ +