Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support accessing mutable sensor types #737

Merged
merged 4 commits into from
Nov 2, 2021
Merged

Support accessing mutable sensor types #737

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8b36609
Support accessing mutable sensor types
Oct 29, 2021
dfa0b04
Added simple test
Oct 29, 2021
6f7b832
Added more tests
Nov 2, 2021
7687b22
Added asserts
Nov 2, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 56 additions & 0 deletions include/sdf/Sensor.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -323,13 +365,27 @@ 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
/// Lidar.
/// \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);
Expand Down
48 changes: 48 additions & 0 deletions src/Sensor.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -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)
{
Expand All @@ -622,3 +664,9 @@ const Imu *Sensor::ImuSensor() const
{
return optionalToPointer(this->dataPtr->imu);
}

/////////////////////////////////////////////////
Imu *Sensor::ImuSensor()
{
return optionalToPointer(this->dataPtr->imu);
}
164 changes: 164 additions & 0 deletions src/Sensor_TEST.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -336,3 +336,167 @@ 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();
adlarkin marked this conversation as resolved.
Show resolved Hide resolved
ASSERT_NE(nullptr, altMutable);
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);
adlarkin marked this conversation as resolved.
Show resolved Hide resolved
}

// Air pressure
{
sdf::Sensor sensor;
sensor.SetType(sdf::SensorType::AIR_PRESSURE);

sdf::AirPressure air;
sensor.SetAirPressureSensor(air);

sdf::AirPressure *airMutable = sensor.AirPressureSensor();
ASSERT_NE(nullptr, airMutable);
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();
ASSERT_NE(nullptr, camMutable);
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();
ASSERT_NE(nullptr, ftqMutable);
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();
ASSERT_NE(nullptr, imuMutable);
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();
ASSERT_NE(nullptr, ldrMutable);
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();
ASSERT_NE(nullptr, magMutable);
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();
ASSERT_NE(nullptr, navMutable);
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);
}
}