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..4f0bcdc5f 100644 --- a/include/sdf/CMakeLists.txt +++ b/include/sdf/CMakeLists.txt @@ -29,6 +29,7 @@ set (headers Material.hh Mesh.hh Model.hh + NavSat.hh Noise.hh Param.hh parser.hh diff --git a/include/sdf/NavSat.hh b/include/sdf/NavSat.hh new file mode 100644 index 000000000..d22c93a6b --- /dev/null +++ b/include/sdf/NavSat.hh @@ -0,0 +1,164 @@ +/* + * 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. + * 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_NAVSAT_HH_ +#define SDF_NAVSAT_HH_ + +#include +#include +#include +#include + +#include + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + class NavSatPrivate; + + /// \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 navsat type: + /// + /// ~~~{.xml} + /// + /// 1 2 3 0 0 0 + /// /navsat + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// 0.98 + /// 0.76 + /// + /// + /// + /// + /// + /// ~~~ + class SDFORMAT_VISIBLE NavSat + { + /// \brief Default constructor + public: NavSat(); + + /// \brief Copy constructor + /// \param[in] _navsat NavSat to copy. + public: NavSat(const NavSat &_navsat); + + /// \brief Move constructor + /// \param[in] _navsat NavSat to move. + public: NavSat(NavSat &&_navsat) noexcept; + + /// \brief Destructor + public: ~NavSat(); + + /// \brief Assignment operator + /// \param[in] _navsat The navsat to set values from. + /// \return *this + public: NavSat &operator=(const NavSat &_navsat); + + /// \brief Move assignment operator + /// \param[in] _navsat The navsat to set values from. + /// \return *this + public: NavSat &operator=(NavSat &&_navsat) noexcept; + + /// \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 + /// \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 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 Get noise value for horizontal velocity sensor + /// \return Noise values + public: const Noise &HorizontalVelocityNoise() 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 Get noise value for vertical velocity sensor + /// \return Noise values + public: const Noise &VerticalVelocityNoise() 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 NavSat object does not contain the same + /// values as the passed in parameter. + /// \param[_in] _navsat NavSat value to compare. + /// \return True if 'this' != _navsat. + public: bool operator!=(const NavSat &_navsat) const; + + /// \brief Private data pointer. + private: NavSatPrivate *dataPtr; + }; + } +} +#endif diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index 6b0e75508..3b3b3be0d 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -38,6 +38,7 @@ namespace sdf class Imu; class Lidar; class Magnetometer; + class NavSat; class SensorPrivate; struct PoseRelativeToGraph; @@ -109,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. @@ -319,6 +323,17 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Set the NAVSAT sensor. + /// \param[in] _navsat The NAVSAT sensor. + public: void SetNavSatSensor(const NavSat &_navsat); + + /// \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 NavSat *NavSatSensor() const; + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); 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..63deb5ba0 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. @@ -61,6 +62,7 @@ + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 01f348a54..8329a5cf3 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -43,6 +43,7 @@ set (sources Material.cc Mesh.cc Model.cc + NavSat.cc Noise.cc parser.cc parser_urdf.cc @@ -122,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/NavSat.cc b/src/NavSat.cc new file mode 100644 index 000000000..088e5d937 --- /dev/null +++ b/src/NavSat.cc @@ -0,0 +1,225 @@ +/* + * 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. + * 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/NavSat.hh" + +using namespace sdf; +using namespace ignition; + +/// \brief Private navsat data. +class sdf::NavSatPrivate +{ + /// \brief Noise values for the horizontal positioning sensor + public: Noise horizontalPositionNoise; + + /// \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}; +}; + +////////////////////////////////////////////////// +NavSat::NavSat() + : dataPtr(new NavSatPrivate) +{ +} + +////////////////////////////////////////////////// +NavSat::~NavSat() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +NavSat::NavSat(const NavSat &_navsat) + : dataPtr(new NavSatPrivate(*_navsat.dataPtr)) +{ +} + +////////////////////////////////////////////////// +NavSat::NavSat(NavSat &&_navsat) noexcept + : dataPtr(std::exchange(_navsat.dataPtr, nullptr)) +{ +} + +////////////////////////////////////////////////// +NavSat &NavSat::operator=(const NavSat &_navsat) +{ + return *this = NavSat(_navsat); +} + +////////////////////////////////////////////////// +NavSat &NavSat::operator=(NavSat &&_navsat) noexcept +{ + std::swap(this->dataPtr, _navsat.dataPtr); + return * this; +} + +////////////////////////////////////////////////// +Errors NavSat::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 NAVSAT, 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() != "navsat" && _sdf->GetName() != "gps") + { + errors.push_back({ErrorCode::ELEMENT_INCORRECT_TYPE, + "Attempting to load NAVSAT, but the provided SDF element is " + "not a ."}); + return errors; + } + + // Load navsat sensor properties + 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")) + { + 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_sensing")) + { + 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; +} + +////////////////////////////////////////////////// +sdf::ElementPtr NavSat::Element() const +{ + return this->dataPtr->sdf; +} + +////////////////////////////////////////////////// +const Noise &NavSat::HorizontalPositionNoise() const +{ + return this->dataPtr->horizontalPositionNoise; +} + +////////////////////////////////////////////////// +void NavSat::SetHorizontalPositionNoise(const Noise &_noise) +{ + this->dataPtr->horizontalPositionNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &NavSat::HorizontalVelocityNoise() const +{ + return this->dataPtr->horizontalVelocityNoise; +} + +////////////////////////////////////////////////// +void NavSat::SetHorizontalVelocityNoise(const Noise &_noise) +{ + this->dataPtr->horizontalVelocityNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &NavSat::VerticalPositionNoise() const +{ + return this->dataPtr->verticalPositionNoise; +} + +////////////////////////////////////////////////// +void NavSat::SetVerticalPositionNoise(const Noise &_noise) +{ + this->dataPtr->verticalPositionNoise = _noise; +} + +////////////////////////////////////////////////// +const Noise &NavSat::VerticalVelocityNoise() const +{ + return this->dataPtr->verticalVelocityNoise; +} + +////////////////////////////////////////////////// +void NavSat::SetVerticalVelocityNoise(const Noise &_noise) +{ + this->dataPtr->verticalVelocityNoise = _noise; +} + +////////////////////////////////////////////////// +bool NavSat::operator==(const NavSat &_navsat) const +{ + if (this->dataPtr->verticalPositionNoise != _navsat.VerticalPositionNoise()) + return false; + if (this->dataPtr->horizontalPositionNoise != + _navsat.HorizontalPositionNoise()) + return false; + if (this->dataPtr->verticalVelocityNoise != _navsat.VerticalVelocityNoise()) + return false; + if (this->dataPtr->horizontalVelocityNoise != + _navsat.HorizontalVelocityNoise()) + return false; + + return true; +} + +////////////////////////////////////////////////// +bool NavSat::operator!=(const NavSat &_navsat) const +{ + 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/Sensor.cc b/src/Sensor.cc index fdd4e96e2..6096d415b 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/NavSat.hh" #include "sdf/Imu.hh" #include "sdf/Magnetometer.hh" #include "sdf/Lidar.hh" @@ -55,7 +56,8 @@ const std::vector sensorTypeStrs = "wireless_transmitter", "air_pressure", "rgbd_camera", - "thermal_camera" + "thermal_camera", + "navsat" }; class sdf::SensorPrivate @@ -79,6 +81,10 @@ class sdf::SensorPrivate this->magnetometer = std::make_unique( *_sensor.magnetometer); } + if (_sensor.navSat) + { + this->navSat = std::make_unique(*_sensor.navSat); + } if (_sensor.altimeter) { this->altimeter = std::make_unique(*_sensor.altimeter); @@ -137,6 +143,9 @@ class sdf::SensorPrivate /// \brief Pointer to an altimeter. public: std::unique_ptr altimeter; + /// \brief Pointer to NAVSAT sensor. + public: std::unique_ptr navSat; + /// \brief Pointer to an air pressure sensor. public: std::unique_ptr airPressure; @@ -221,6 +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::NAVSAT: + return *(this->dataPtr->navSat) == *(_sensor.dataPtr->navSat); case SensorType::CAMERA: case SensorType::DEPTH_CAMERA: case SensorType::RGBD_CAMERA: @@ -340,9 +351,13 @@ Errors Sensor::Load(ElementPtr _sdf) { this->dataPtr->type = SensorType::FORCE_TORQUE; } - else if (type == "gps") + else if (type == "navsat" || type == "gps") { - this->dataPtr->type = SensorType::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") { @@ -626,6 +641,18 @@ const Camera *Sensor::CameraSensor() const return this->dataPtr->camera.get(); } +///////////////////////////////////////////////// +void Sensor::SetNavSatSensor(const NavSat &_gps) +{ + this->dataPtr->navSat = std::make_unique(_gps); +} + +///////////////////////////////////////////////// +const NavSat *Sensor::NavSatSensor() const +{ + return this->dataPtr->navSat.get(); +} + ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index ea170a9a9..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::GPS, + 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", - "gps", + "navsat", "gpu_lidar", "imu", "logical_camera", diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 19b018da5..f8020abbd 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/NavSat.hh" #include "sdf/Link.hh" #include "sdf/Magnetometer.hh" #include "sdf/Model.hh" @@ -359,12 +360,24 @@ 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()); + // 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), + 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 b65f07d0d..1f7f84f64 100644 --- a/test/sdf/sensors.sdf +++ b/test/sdf/sensors.sdf @@ -141,8 +141,38 @@ 10 11 12 0 0 0 - + 13 14 15 0 0 0 + + + + + 1.2 + 3.4 + + + + + 5.6 + 7.8 + + + + + + + 9.1 + 10.11 + + + + + 12.13 + 14.15 + + + + @@ -308,7 +338,7 @@ - + 4 5 6 0 0 0