From 8b366097c341e9b7d04aaa9e77a042bd71ffe166 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 29 Oct 2021 14:09:29 -0700 Subject: [PATCH 1/4] Support accessing mutable sensor types Signed-off-by: Nate Koenig --- include/sdf/Sensor.hh | 56 +++++++++++++++++++++++++++++++++++++++++++ src/Sensor.cc | 48 +++++++++++++++++++++++++++++++++++++ 2 files changed, 104 insertions(+) diff --git a/include/sdf/Sensor.hh b/include/sdf/Sensor.hh index f8ac4adcd..ef3ddbd08 100644 --- a/include/sdf/Sensor.hh +++ b/include/sdf/Sensor.hh @@ -253,6 +253,13 @@ namespace sdf /// \sa SensorType Type() const public: const Magnetometer *MagnetometerSensor() const; + /// \brief Get a mutable magnetometer sensor, or nullptr if this sensor type + /// is not a Magnetometer. + /// \return Pointer to the Magnetometer sensor, or nullptr if this + /// Sensor is not a Magnetometer. + /// \sa SensorType Type() const + public: Magnetometer *MagnetometerSensor(); + /// \brief Set the magnetometer sensor. /// \param[in] _mag The magnetometer sensor. public: void SetMagnetometerSensor(const Magnetometer &_mag); @@ -264,6 +271,13 @@ namespace sdf /// \sa SensorType Type() const public: const Altimeter *AltimeterSensor() const; + /// \brief Get a mutable altimeter sensor, or nullptr if this sensor type + /// is not an Altimeter. + /// \return Pointer to the Altimeter sensor, or nullptr if this + /// Sensor is not a Altimeter. + /// \sa SensorType Type() const + public: Altimeter *AltimeterSensor(); + /// \brief Set the altimeter sensor. /// \param[in] _alt The altimeter sensor. public: void SetAltimeterSensor(const Altimeter &_alt); @@ -275,6 +289,13 @@ namespace sdf /// \sa SensorType Type() const public: const AirPressure *AirPressureSensor() const; + /// \brief Get a mutable air pressure sensor, or nullptr if this sensor type + /// is not an AirPressure sensor. + /// \return Pointer to the AirPressure sensor, or nullptr if this + /// Sensor is not a AirPressure sensor. + /// \sa SensorType Type() const + public: AirPressure *AirPressureSensor(); + /// \brief Set the air pressure sensor. /// \param[in] _air The air pressure sensor. public: void SetAirPressureSensor(const AirPressure &_air); @@ -290,6 +311,13 @@ namespace sdf /// \sa SensorType Type() const public: const Camera *CameraSensor() const; + /// \brief Get a mutable camera sensor, or nullptr if the + /// sensor does not contain a camera sensor. + /// \return Pointer to the sensor's camera, or nullptr if the sensor + /// is not a camera. + /// \sa SensorType Type() const + public: Camera *CameraSensor(); + /// \brief Set the NAVSAT sensor. /// \param[in] _navsat The NAVSAT sensor. public: void SetNavSatSensor(const NavSat &_navsat); @@ -301,6 +329,13 @@ namespace sdf /// \sa SensorType Type() const public: const NavSat *NavSatSensor() const; + /// \brief Get a mutable 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: NavSat *NavSatSensor(); + /// \brief Set the force torque sensor. /// \param[in] _ft The force torque sensor. public: void SetForceTorqueSensor(const ForceTorque &_ft); @@ -312,6 +347,13 @@ namespace sdf /// \sa SensorType Type() const public: const ForceTorque *ForceTorqueSensor() const; + /// \brief Get a mutable force torque sensor, or nullptr if the sensor + /// does not contain a force torque sensor. + /// \return Pointer to the force torque sensor, or nullptr if the sensor + /// is not a force torque sensor. + /// \sa SensorType Type() const + public: ForceTorque *ForceTorqueSensor(); + /// \brief Set the IMU sensor. /// \param[in] _imu The IMU sensor. public: void SetImuSensor(const Imu &_imu); @@ -323,6 +365,13 @@ namespace sdf /// \sa SensorType Type() const public: const Imu *ImuSensor() const; + /// \brief Get a mutable IMU sensor, or nullptr if the sensor + /// does not contain an IMU sensor. + /// \return Pointer to the sensor's IMU, or nullptr if the sensor + /// is not an IMU. + /// \sa SensorType Type() const + public: Imu *ImuSensor(); + /// \brief Get the lidar sensor, or nullptr if this sensor type is not a /// Lidar. /// \return Pointer to the Lidar sensor, or nullptr if this Sensor is not a @@ -330,6 +379,13 @@ namespace sdf /// \sa SensorType Type() const public: const Lidar *LidarSensor() const; + /// \brief Get a mutable lidar sensor, or nullptr if this sensor type is + /// not a Lidar. + /// \return Pointer to the Lidar sensor, or nullptr if this Sensor is not a + /// Lidar. + /// \sa SensorType Type() const + public: Lidar *LidarSensor(); + /// \brief Set the lidar sensor. /// \param[in] _lidar The lidar sensor. public: void SetLidarSensor(const Lidar &_lidar); diff --git a/src/Sensor.cc b/src/Sensor.cc index 6f1ac7b3f..eed455cc6 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -512,6 +512,12 @@ const Magnetometer *Sensor::MagnetometerSensor() const return optionalToPointer(this->dataPtr->magnetometer); } +///////////////////////////////////////////////// +Magnetometer *Sensor::MagnetometerSensor() +{ + return optionalToPointer(this->dataPtr->magnetometer); +} + ///////////////////////////////////////////////// void Sensor::SetMagnetometerSensor(const Magnetometer &_mag) { @@ -524,6 +530,12 @@ const Altimeter *Sensor::AltimeterSensor() const return optionalToPointer(this->dataPtr->altimeter); } +///////////////////////////////////////////////// +Altimeter *Sensor::AltimeterSensor() +{ + return optionalToPointer(this->dataPtr->altimeter); +} + ///////////////////////////////////////////////// void Sensor::SetAltimeterSensor(const Altimeter &_alt) { @@ -536,6 +548,12 @@ const AirPressure *Sensor::AirPressureSensor() const return optionalToPointer(this->dataPtr->airPressure); } +///////////////////////////////////////////////// +AirPressure *Sensor::AirPressureSensor() +{ + return optionalToPointer(this->dataPtr->airPressure); +} + ///////////////////////////////////////////////// void Sensor::SetAirPressureSensor(const AirPressure &_air) { @@ -548,6 +566,12 @@ const Lidar *Sensor::LidarSensor() const return optionalToPointer(this->dataPtr->lidar); } +///////////////////////////////////////////////// +Lidar *Sensor::LidarSensor() +{ + return optionalToPointer(this->dataPtr->lidar); +} + ///////////////////////////////////////////////// void Sensor::SetLidarSensor(const Lidar &_lidar) { @@ -587,6 +611,12 @@ const Camera *Sensor::CameraSensor() const return optionalToPointer(this->dataPtr->camera); } +///////////////////////////////////////////////// +Camera *Sensor::CameraSensor() +{ + return optionalToPointer(this->dataPtr->camera); +} + ///////////////////////////////////////////////// void Sensor::SetForceTorqueSensor(const ForceTorque &_ft) { @@ -599,6 +629,12 @@ const ForceTorque *Sensor::ForceTorqueSensor() const return optionalToPointer(this->dataPtr->forceTorque); } +///////////////////////////////////////////////// +ForceTorque *Sensor::ForceTorqueSensor() +{ + return optionalToPointer(this->dataPtr->forceTorque); +} + ///////////////////////////////////////////////// void Sensor::SetNavSatSensor(const NavSat &_gps) { @@ -611,6 +647,12 @@ const NavSat *Sensor::NavSatSensor() const return optionalToPointer(this->dataPtr->navSat); } +///////////////////////////////////////////////// +NavSat *Sensor::NavSatSensor() +{ + return optionalToPointer(this->dataPtr->navSat); +} + ///////////////////////////////////////////////// void Sensor::SetImuSensor(const Imu &_imu) { @@ -622,3 +664,9 @@ const Imu *Sensor::ImuSensor() const { return optionalToPointer(this->dataPtr->imu); } + +///////////////////////////////////////////////// +Imu *Sensor::ImuSensor() +{ + return optionalToPointer(this->dataPtr->imu); +} From dfa0b04c81ec0ca0f95a31a684084d10a9f4cb98 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Fri, 29 Oct 2021 14:16:09 -0700 Subject: [PATCH 2/4] Added simple test Signed-off-by: Nate Koenig --- src/Sensor_TEST.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index e1b66b92e..57caab03e 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -150,6 +150,14 @@ TEST(DOMSensor, MoveAssignment) ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); + + sdf::Magnetometer *magMutable = sensor2.MagnetometerSensor(); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), + sensor2.MagnetometerSensor()->XNoise().Mean()); + noise.SetMean(2.0); + magMutable->SetXNoise(noise); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ(sensor2.MagnetometerSensor()->XNoise().Mean(), 2.0); } ///////////////////////////////////////////////// From 6f7b832509ce6d69cea06a82a3f21c59e87199e5 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 09:19:15 -0700 Subject: [PATCH 3/4] Added more tests Signed-off-by: Nate Koenig --- src/Sensor_TEST.cc | 164 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 156 insertions(+), 8 deletions(-) diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 57caab03e..469d685d7 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -150,14 +150,6 @@ TEST(DOMSensor, MoveAssignment) ASSERT_TRUE(nullptr != sensor2.MagnetometerSensor()); EXPECT_DOUBLE_EQ(mag.XNoise().Mean(), sensor2.MagnetometerSensor()->XNoise().Mean()); - - sdf::Magnetometer *magMutable = sensor2.MagnetometerSensor(); - EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), - sensor2.MagnetometerSensor()->XNoise().Mean()); - noise.SetMean(2.0); - magMutable->SetXNoise(noise); - EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), 2.0); - EXPECT_DOUBLE_EQ(sensor2.MagnetometerSensor()->XNoise().Mean(), 2.0); } ///////////////////////////////////////////////// @@ -344,3 +336,159 @@ TEST(DOMSensor, EnableMetrics) sensor.SetEnableMetrics(false); EXPECT_EQ(false, sensor.EnableMetrics()); } + +///////////////////////////////////////////////// +TEST(DOMSensor, MutableSensors) +{ + // Altimeter + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::ALTIMETER); + + sdf::Altimeter alt; + sensor.SetAltimeterSensor(alt); + + sdf::Altimeter *altMutable = sensor.AltimeterSensor(); + EXPECT_DOUBLE_EQ(altMutable->VerticalPositionNoise().Mean(), + sensor.AltimeterSensor()->VerticalPositionNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + altMutable->SetVerticalPositionNoise(noise); + EXPECT_DOUBLE_EQ(altMutable->VerticalPositionNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.AltimeterSensor()->VerticalPositionNoise().Mean(), 2.0); + } + + // Air pressure + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::AIR_PRESSURE); + + sdf::AirPressure air; + sensor.SetAirPressureSensor(air); + + sdf::AirPressure *airMutable = sensor.AirPressureSensor(); + EXPECT_DOUBLE_EQ(airMutable->ReferenceAltitude(), + sensor.AirPressureSensor()->ReferenceAltitude()); + + airMutable->SetReferenceAltitude(2.0); + EXPECT_DOUBLE_EQ(airMutable->ReferenceAltitude(), 2.0); + EXPECT_DOUBLE_EQ(sensor.AirPressureSensor()->ReferenceAltitude(), 2.0); + } + + // Camera + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::CAMERA); + + sdf::Camera cam; + sensor.SetCameraSensor(cam); + + sdf::Camera *camMutable = sensor.CameraSensor(); + EXPECT_DOUBLE_EQ(camMutable->NearClip(), sensor.CameraSensor()->NearClip()); + + camMutable->SetNearClip(2.0); + EXPECT_DOUBLE_EQ(camMutable->NearClip(), 2.0); + EXPECT_DOUBLE_EQ(sensor.CameraSensor()->NearClip(), 2.0); + } + + // Force torque + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::FORCE_TORQUE); + + sdf::ForceTorque ftq; + sensor.SetForceTorqueSensor(ftq); + + sdf::ForceTorque *ftqMutable = sensor.ForceTorqueSensor(); + EXPECT_DOUBLE_EQ(ftqMutable->ForceXNoise().Mean(), + sensor.ForceTorqueSensor()->ForceXNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + ftqMutable->SetForceXNoise(noise); + EXPECT_DOUBLE_EQ(ftqMutable->ForceXNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.ForceTorqueSensor()->ForceXNoise().Mean(), 2.0); + } + + // IMU + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::FORCE_TORQUE); + + sdf::Imu imu; + sensor.SetImuSensor(imu); + + sdf::Imu *imuMutable = sensor.ImuSensor(); + EXPECT_DOUBLE_EQ(imuMutable->LinearAccelerationXNoise().Mean(), + sensor.ImuSensor()->LinearAccelerationXNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + imuMutable->SetLinearAccelerationXNoise(noise); + EXPECT_DOUBLE_EQ(imuMutable->LinearAccelerationXNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.ImuSensor()->LinearAccelerationXNoise().Mean(), 2.0); + } + + // Lidar + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::LIDAR); + + sdf::Lidar ldr; + sensor.SetLidarSensor(ldr); + + sdf::Lidar *ldrMutable = sensor.LidarSensor(); + EXPECT_DOUBLE_EQ(ldrMutable->LidarNoise().Mean(), + sensor.LidarSensor()->LidarNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + ldrMutable->SetLidarNoise(noise); + EXPECT_DOUBLE_EQ(ldrMutable->LidarNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.LidarSensor()->LidarNoise().Mean(), 2.0); + } + + // Magnetometer + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::MAGNETOMETER); + + sdf::Magnetometer mag; + sensor.SetMagnetometerSensor(mag); + + sdf::Magnetometer *magMutable = sensor.MagnetometerSensor(); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), + sensor.MagnetometerSensor()->XNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + magMutable->SetXNoise(noise); + EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ(sensor.MagnetometerSensor()->XNoise().Mean(), 2.0); + } + + // NavSat + { + sdf::Sensor sensor; + sensor.SetType(sdf::SensorType::NAVSAT); + + sdf::NavSat nav; + sensor.SetNavSatSensor(nav); + + sdf::NavSat *navMutable = sensor.NavSatSensor(); + EXPECT_DOUBLE_EQ(navMutable->HorizontalPositionNoise().Mean(), + sensor.NavSatSensor()->HorizontalPositionNoise().Mean()); + + sdf::Noise noise; + noise.SetMean(2.0); + navMutable->SetHorizontalPositionNoise(noise); + EXPECT_DOUBLE_EQ(navMutable->HorizontalPositionNoise().Mean(), 2.0); + EXPECT_DOUBLE_EQ( + sensor.NavSatSensor()->HorizontalPositionNoise().Mean(), 2.0); + } +} From 7687b2235271d3640b2f04c57cf54d668d1ab6fb Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 2 Nov 2021 10:09:11 -0700 Subject: [PATCH 4/4] Added asserts Signed-off-by: Nate Koenig --- src/Sensor_TEST.cc | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 469d685d7..28ef3cb77 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -349,6 +349,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetAltimeterSensor(alt); sdf::Altimeter *altMutable = sensor.AltimeterSensor(); + ASSERT_NE(nullptr, altMutable); EXPECT_DOUBLE_EQ(altMutable->VerticalPositionNoise().Mean(), sensor.AltimeterSensor()->VerticalPositionNoise().Mean()); @@ -369,6 +370,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetAirPressureSensor(air); sdf::AirPressure *airMutable = sensor.AirPressureSensor(); + ASSERT_NE(nullptr, airMutable); EXPECT_DOUBLE_EQ(airMutable->ReferenceAltitude(), sensor.AirPressureSensor()->ReferenceAltitude()); @@ -386,6 +388,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetCameraSensor(cam); sdf::Camera *camMutable = sensor.CameraSensor(); + ASSERT_NE(nullptr, camMutable); EXPECT_DOUBLE_EQ(camMutable->NearClip(), sensor.CameraSensor()->NearClip()); camMutable->SetNearClip(2.0); @@ -402,6 +405,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetForceTorqueSensor(ftq); sdf::ForceTorque *ftqMutable = sensor.ForceTorqueSensor(); + ASSERT_NE(nullptr, ftqMutable); EXPECT_DOUBLE_EQ(ftqMutable->ForceXNoise().Mean(), sensor.ForceTorqueSensor()->ForceXNoise().Mean()); @@ -422,6 +426,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetImuSensor(imu); sdf::Imu *imuMutable = sensor.ImuSensor(); + ASSERT_NE(nullptr, imuMutable); EXPECT_DOUBLE_EQ(imuMutable->LinearAccelerationXNoise().Mean(), sensor.ImuSensor()->LinearAccelerationXNoise().Mean()); @@ -442,6 +447,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetLidarSensor(ldr); sdf::Lidar *ldrMutable = sensor.LidarSensor(); + ASSERT_NE(nullptr, ldrMutable); EXPECT_DOUBLE_EQ(ldrMutable->LidarNoise().Mean(), sensor.LidarSensor()->LidarNoise().Mean()); @@ -462,6 +468,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetMagnetometerSensor(mag); sdf::Magnetometer *magMutable = sensor.MagnetometerSensor(); + ASSERT_NE(nullptr, magMutable); EXPECT_DOUBLE_EQ(magMutable->XNoise().Mean(), sensor.MagnetometerSensor()->XNoise().Mean()); @@ -481,6 +488,7 @@ TEST(DOMSensor, MutableSensors) sensor.SetNavSatSensor(nav); sdf::NavSat *navMutable = sensor.NavSatSensor(); + ASSERT_NE(nullptr, navMutable); EXPECT_DOUBLE_EQ(navMutable->HorizontalPositionNoise().Mean(), sensor.NavSatSensor()->HorizontalPositionNoise().Mean());