From 4ca4703a8674f1859e617693d189215e9fccacef Mon Sep 17 00:00:00 2001 From: Aditya Date: Mon, 20 Jun 2022 11:35:51 -0700 Subject: [PATCH] Ported IGN macros to GZ Signed-off-by: Aditya --- .../src/AcousticCommsPlugin.cc | 2 +- .../src/ControlPanelPlugin.cc | 2 +- .../src/HydrodynamicsPlugin.cc | 2 +- .../src/RangeBearingPlugin.cc | 2 +- lrauv_ignition_plugins/src/ReferenceAxis.cc | 6 ++-- .../src/ScienceSensorsSystem.cc | 20 ++++++------ .../src/SpawnPanelPlugin.cc | 2 +- .../src/TethysCommPlugin.cc | 2 +- .../src/TimeAnalysisPlugin.cc | 2 +- lrauv_ignition_plugins/src/WorldCommPlugin.cc | 4 +-- .../src/WorldConfigPlugin.cc | 2 +- .../src/comms/models/SimpleAcousticModel.cc | 2 +- .../test/dynamics/test_hydrodynamics.cc | 2 +- .../test/missions/test_depth_vbs_mission.cc | 4 +-- .../test/missions/test_pitch_mass_mission.cc | 12 +++---- .../test/missions/test_yoyo_circle_mission.cc | 4 +-- .../test/simulation/test_spawn.cc | 14 ++++---- .../test/simulation/test_state.cc | 12 +++---- lrauv_system_tests/test/vehicle/test_ahrs.cc | 32 +++++++++---------- .../test/vehicle/test_propeller_action.cc | 2 +- .../test/vehicle/test_sensor.cc | 4 +-- .../vehicle/test_sensor_timeinterpolation.cc | 4 +-- 22 files changed, 69 insertions(+), 69 deletions(-) diff --git a/lrauv_ignition_plugins/src/AcousticCommsPlugin.cc b/lrauv_ignition_plugins/src/AcousticCommsPlugin.cc index cee91fe0..cf5dda54 100644 --- a/lrauv_ignition_plugins/src/AcousticCommsPlugin.cc +++ b/lrauv_ignition_plugins/src/AcousticCommsPlugin.cc @@ -355,7 +355,7 @@ void AcousticCommsPlugin::PreUpdate( } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::AcousticCommsPlugin, gz::sim::System, tethys::AcousticCommsPlugin::ISystemConfigure, diff --git a/lrauv_ignition_plugins/src/ControlPanelPlugin.cc b/lrauv_ignition_plugins/src/ControlPanelPlugin.cc index 4a48b090..4c52d5a0 100644 --- a/lrauv_ignition_plugins/src/ControlPanelPlugin.cc +++ b/lrauv_ignition_plugins/src/ControlPanelPlugin.cc @@ -110,5 +110,5 @@ void ControlPanel::SetBuoyancyEngine(qreal _volume) } // Register this plugin -IGNITION_ADD_PLUGIN(tethys::ControlPanel, +GZ_ADD_PLUGIN(tethys::ControlPanel, gz::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc b/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc index 886f9815..cca1d599 100644 --- a/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc +++ b/lrauv_ignition_plugins/src/HydrodynamicsPlugin.cc @@ -361,7 +361,7 @@ void HydrodynamicsPlugin::PreUpdate( }; -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::HydrodynamicsPlugin, gz::sim::System, tethys::HydrodynamicsPlugin::ISystemConfigure, diff --git a/lrauv_ignition_plugins/src/RangeBearingPlugin.cc b/lrauv_ignition_plugins/src/RangeBearingPlugin.cc index d54f500f..b25211d7 100644 --- a/lrauv_ignition_plugins/src/RangeBearingPlugin.cc +++ b/lrauv_ignition_plugins/src/RangeBearingPlugin.cc @@ -342,7 +342,7 @@ void RangeBearingPlugin::PreUpdate( } -IGNITION_ADD_PLUGIN(tethys::RangeBearingPlugin, +GZ_ADD_PLUGIN(tethys::RangeBearingPlugin, gz::sim::System, tethys::RangeBearingPlugin::ISystemConfigure, tethys::RangeBearingPlugin::ISystemPreUpdate) diff --git a/lrauv_ignition_plugins/src/ReferenceAxis.cc b/lrauv_ignition_plugins/src/ReferenceAxis.cc index 4628f61e..f9298ea8 100644 --- a/lrauv_ignition_plugins/src/ReferenceAxis.cc +++ b/lrauv_ignition_plugins/src/ReferenceAxis.cc @@ -219,7 +219,7 @@ void ReferenceAxisPrivate::OnPreRender() { this->nedVis = this->scene->CreateAxisVisual(); this->scene->RootVisual()->AddChild(this->nedVis); - this->nedVis->SetLocalRotation(IGN_PI, 0, IGN_PI * 0.5); + this->nedVis->SetLocalRotation(GZ_PI, 0, GZ_PI * 0.5); this->nedVis->SetLocalScale(0.25, 0.25, 0.25); // Ogre2 doesn't support text yet @@ -265,7 +265,7 @@ void ReferenceAxisPrivate::OnPreRender() vehicleVis->AddChild(fskVis); // TODO(chapulina) This rotation won't be needed if we update the model // https://github.com/osrf/lrauv/issues/80 - fskVis->SetLocalRotation(IGN_PI, 0, IGN_PI * 0.5); + fskVis->SetLocalRotation(GZ_PI, 0, GZ_PI * 0.5); // Ogre2 doesn't support text yet auto textGeom = this->scene->CreateText(); @@ -288,5 +288,5 @@ void ReferenceAxisPrivate::OnPreRender() } // Register this plugin -IGNITION_ADD_PLUGIN(tethys::ReferenceAxis, +GZ_ADD_PLUGIN(tethys::ReferenceAxis, gz::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc index cebf17f4..959bf3d9 100644 --- a/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc +++ b/lrauv_ignition_plugins/src/ScienceSensorsSystem.cc @@ -389,7 +389,7 @@ float ScienceSensorsSystemPrivate::InterpolateInTime( bool ScienceSensorsSystemPrivate::ReadData( const gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE("ScienceSensorsSystemPrivate::ReadData"); + GZ_PROFILE("ScienceSensorsSystemPrivate::ReadData"); if (!this->sphericalCoordinatesInitialized) { @@ -611,7 +611,7 @@ bool ScienceSensorsSystemPrivate::ReadData( ///////////////////////////////////////////////// void ScienceSensorsSystemPrivate::PublishData() { - IGN_PROFILE("ScienceSensorsSystemPrivate::PublishData"); + GZ_PROFILE("ScienceSensorsSystemPrivate::PublishData"); this->tempPub.Publish(this->tempMsg); this->chlorPub.Publish(this->chlorMsg); this->salPub.Publish(this->salMsg); @@ -708,8 +708,8 @@ void ScienceSensorsSystem::PreUpdate( void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE_THREAD_NAME("ScienceSensorsSystem PostUpdate"); - IGN_PROFILE("ScienceSensorsSystem::PostUpdate"); + GZ_PROFILE_THREAD_NAME("ScienceSensorsSystem PostUpdate"); + GZ_PROFILE("ScienceSensorsSystem::PostUpdate"); this->RemoveSensorEntities(_ecm); @@ -777,7 +777,7 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info, // to generate data for that sensor. for (auto &[entity, sensor] : this->entitySensorMap) { - IGN_PROFILE("ScienceSensorsSystem::LookupInterpolators"); + GZ_PROFILE("ScienceSensorsSystem::LookupInterpolators"); auto sensorPosENU = gz::sim::worldPose(entity, _ecm).Pos(); auto spherical = gz::sim::sphericalCoordinates(entity, _ecm).value(); auto sphericalDepthCorrected = gz::math::Vector3d{spherical.X(), spherical.Y(), @@ -834,7 +834,7 @@ void ScienceSensorsSystem::PostUpdate(const gz::sim::UpdateInfo &_info, void ScienceSensorsSystem::RemoveSensorEntities( const gz::sim::EntityComponentManager &_ecm) { - IGN_PROFILE("ScienceSensorsSystem::RemoveSensorEntities"); + GZ_PROFILE("ScienceSensorsSystem::RemoveSensorEntities"); _ecm.EachRemoved( [&](const gz::sim::Entity &_entity, @@ -891,7 +891,7 @@ bool ScienceSensorsSystemPrivate::SalinityService( ////////////////////////////////////////////////// gz::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg() { - IGN_PROFILE("ScienceSensorsSystemPrivate::PointCloudMsg"); + GZ_PROFILE("ScienceSensorsSystemPrivate::PointCloudMsg"); gz::msgs::PointCloudPacked msg; @@ -933,12 +933,12 @@ gz::msgs::PointCloudPacked ScienceSensorsSystemPrivate::PointCloudMsg() return msg; } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::ScienceSensorsSystem, gz::sim::System, tethys::ScienceSensorsSystem::ISystemConfigure, tethys::ScienceSensorsSystem::ISystemPreUpdate, tethys::ScienceSensorsSystem::ISystemPostUpdate) -IGNITION_ADD_PLUGIN_ALIAS(tethys::ScienceSensorsSystem, - "tethys::ScienceSensorsSystem") \ No newline at end of file +GZ_ADD_PLUGIN_ALIAS(tethys::ScienceSensorsSystem, + "tethys::ScienceSensorsSystem") diff --git a/lrauv_ignition_plugins/src/SpawnPanelPlugin.cc b/lrauv_ignition_plugins/src/SpawnPanelPlugin.cc index 4c58cbdc..d9160173 100644 --- a/lrauv_ignition_plugins/src/SpawnPanelPlugin.cc +++ b/lrauv_ignition_plugins/src/SpawnPanelPlugin.cc @@ -94,5 +94,5 @@ void SpawnPanel::Update(const gz::sim::UpdateInfo &, } // Register this plugin -IGNITION_ADD_PLUGIN(tethys::SpawnPanel, +GZ_ADD_PLUGIN(tethys::SpawnPanel, gz::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/TethysCommPlugin.cc b/lrauv_ignition_plugins/src/TethysCommPlugin.cc index 67e9c372..a1bc4612 100644 --- a/lrauv_ignition_plugins/src/TethysCommPlugin.cc +++ b/lrauv_ignition_plugins/src/TethysCommPlugin.cc @@ -710,7 +710,7 @@ void TethysCommPlugin::PostUpdate( } } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::TethysCommPlugin, gz::sim::System, tethys::TethysCommPlugin::ISystemConfigure, diff --git a/lrauv_ignition_plugins/src/TimeAnalysisPlugin.cc b/lrauv_ignition_plugins/src/TimeAnalysisPlugin.cc index 5716e159..53870c42 100644 --- a/lrauv_ignition_plugins/src/TimeAnalysisPlugin.cc +++ b/lrauv_ignition_plugins/src/TimeAnalysisPlugin.cc @@ -143,7 +143,7 @@ void TimeAnalysisPlugin::PostUpdate( } } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::TimeAnalysisPlugin, gz::sim::System, tethys::TimeAnalysisPlugin::ISystemConfigure, diff --git a/lrauv_ignition_plugins/src/WorldCommPlugin.cc b/lrauv_ignition_plugins/src/WorldCommPlugin.cc index 90e37c4f..8ba4b194 100644 --- a/lrauv_ignition_plugins/src/WorldCommPlugin.cc +++ b/lrauv_ignition_plugins/src/WorldCommPlugin.cc @@ -203,7 +203,7 @@ void WorldCommPlugin::SpawnCallback( // The robot model is facing its own -X, so with zero ENU orientation it faces // West. We add an extra 90 degree yaw so zero means North, to conform with // NED. - auto rotRobot = gz::math::Quaterniond(0.0, 0.0, -IGN_PI * 0.5) * rotENU; + auto rotRobot = gz::math::Quaterniond(0.0, 0.0, -GZ_PI * 0.5) * rotENU; gz::msgs::Set(factoryReq.mutable_pose()->mutable_orientation(), rotRobot); @@ -313,7 +313,7 @@ std::string WorldCommPlugin::TethysSdfString(const lrauv_ignition_plugins::msgs: return sdfStr; } -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( tethys::WorldCommPlugin, gz::sim::System, tethys::WorldCommPlugin::ISystemConfigure) diff --git a/lrauv_ignition_plugins/src/WorldConfigPlugin.cc b/lrauv_ignition_plugins/src/WorldConfigPlugin.cc index 6d8ef5b7..3e2d7363 100644 --- a/lrauv_ignition_plugins/src/WorldConfigPlugin.cc +++ b/lrauv_ignition_plugins/src/WorldConfigPlugin.cc @@ -66,5 +66,5 @@ void WorldConfig::SetFilePath(QUrl _filePath) } // Register this plugin -IGNITION_ADD_PLUGIN(tethys::WorldConfig, +GZ_ADD_PLUGIN(tethys::WorldConfig, gz::gui::Plugin) diff --git a/lrauv_ignition_plugins/src/comms/models/SimpleAcousticModel.cc b/lrauv_ignition_plugins/src/comms/models/SimpleAcousticModel.cc index 3b5f14a0..f775b8d8 100644 --- a/lrauv_ignition_plugins/src/comms/models/SimpleAcousticModel.cc +++ b/lrauv_ignition_plugins/src/comms/models/SimpleAcousticModel.cc @@ -123,6 +123,6 @@ class SimpleAcousticModel : public ICommsModel } }; -IGNITION_ADD_PLUGIN(SimpleAcousticModel, +GZ_ADD_PLUGIN(SimpleAcousticModel, SimpleAcousticModel::ICommsModel) } diff --git a/lrauv_system_tests/test/dynamics/test_hydrodynamics.cc b/lrauv_system_tests/test/dynamics/test_hydrodynamics.cc index 4ddaadcd..7eb05a4f 100644 --- a/lrauv_system_tests/test/dynamics/test_hydrodynamics.cc +++ b/lrauv_system_tests/test/dynamics/test_hydrodynamics.cc @@ -107,7 +107,7 @@ TEST(HydrodynamicsTest, DampForwardThrust) gz::msgs::Double thrustCommand; // Vehicle is supposed to move at around 1 m/s with a 300 RPM thrust. // 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s - thrustCommand.set_data(10. * IGN_PI); + thrustCommand.set_data(10. * GZ_PI); for (auto &publisher : fixture.ThrustPublishers()) { using namespace std::literals::chrono_literals; diff --git a/lrauv_system_tests/test/missions/test_depth_vbs_mission.cc b/lrauv_system_tests/test/missions/test_depth_vbs_mission.cc index d62bb925..bc876f9d 100644 --- a/lrauv_system_tests/test/missions/test_depth_vbs_mission.cc +++ b/lrauv_system_tests/test/missions/test_depth_vbs_mission.cc @@ -66,8 +66,8 @@ TEST(MissionTest, DepthVBS) if (times[i] > 3min) // Initialization is complete { // Vehicle should not go vertical when tilting nose - EXPECT_LT(poses[i].Rot().Euler().X(), IGN_DTOR(10)); - EXPECT_GT(poses[i].Rot().Euler().X(), IGN_DTOR(-10)); + EXPECT_LT(poses[i].Rot().Euler().X(), GZ_DTOR(10)); + EXPECT_GT(poses[i].Rot().Euler().X(), GZ_DTOR(-10)); } // Vehicle should not exceed 20m depth diff --git a/lrauv_system_tests/test/missions/test_pitch_mass_mission.cc b/lrauv_system_tests/test/missions/test_pitch_mass_mission.cc index 4f250a60..241ce28e 100644 --- a/lrauv_system_tests/test/missions/test_pitch_mass_mission.cc +++ b/lrauv_system_tests/test/missions/test_pitch_mass_mission.cc @@ -80,13 +80,13 @@ TEST(MissionTest, PitchMass) // Max Pitch 20 degrees, allowing for an error of // up to 5 degrees to accomodate for oscillations // during pitch control. - EXPECT_LT(poses[i].Rot().Euler().X(), IGN_DTOR(25)); - EXPECT_GT(poses[i].Rot().Euler().X(), IGN_DTOR(-5)); + EXPECT_LT(poses[i].Rot().Euler().X(), GZ_DTOR(25)); + EXPECT_GT(poses[i].Rot().Euler().X(), GZ_DTOR(-5)); } // No roll or yaw - EXPECT_NEAR(poses[i].Rot().Euler().Y(), IGN_DTOR(0), 1e-2); - EXPECT_NEAR(poses[i].Rot().Euler().Z(), IGN_DTOR(0), 1e-2); + EXPECT_NEAR(poses[i].Rot().Euler().Y(), GZ_DTOR(0), 1e-2); + EXPECT_NEAR(poses[i].Rot().Euler().Z(), GZ_DTOR(0), 1e-2); // Used later for oscillation check. if (!firstPitch) @@ -96,7 +96,7 @@ TEST(MissionTest, PitchMass) } // Check if we cross the 20 degree mark - if (prevPitch <= IGN_DTOR(20) && poses[i].Rot().Euler().X() >= IGN_DTOR(20)) + if (prevPitch <= GZ_DTOR(20) && poses[i].Rot().Euler().X() >= GZ_DTOR(20)) { reachedTarget = true; } @@ -110,7 +110,7 @@ TEST(MissionTest, PitchMass) // Check for oscillation by summing over abs delta in pitch // Essentially \Sigma abs(f'(x)) < C. In this case C should be near 2*20 // degrees as the vehicle first pitches down and then comes back up. - EXPECT_LE(totalPitchChange, IGN_DTOR(20) * 2.); + EXPECT_LE(totalPitchChange, GZ_DTOR(20) * 2.); // Make sure the vehicle actually pitched to 20 degrees. EXPECT_TRUE(reachedTarget); diff --git a/lrauv_system_tests/test/missions/test_yoyo_circle_mission.cc b/lrauv_system_tests/test/missions/test_yoyo_circle_mission.cc index 1e744afb..2023aa07 100644 --- a/lrauv_system_tests/test/missions/test_yoyo_circle_mission.cc +++ b/lrauv_system_tests/test/missions/test_yoyo_circle_mission.cc @@ -62,8 +62,8 @@ TEST(MissionTest, YoYoCircle) for (size_t i = 0; i < times.size(); ++i) { // Pitch should be between -20 and 20 degrees - EXPECT_LT(IGN_DTOR(-20), poses[i].Rot().Pitch()); - EXPECT_GT(IGN_DTOR(20), poses[i].Rot().Pitch()); + EXPECT_LT(GZ_DTOR(-20), poses[i].Rot().Pitch()); + EXPECT_GT(GZ_DTOR(20), poses[i].Rot().Pitch()); if (times[i] > 2min) { diff --git a/lrauv_system_tests/test/simulation/test_spawn.cc b/lrauv_system_tests/test/simulation/test_spawn.cc index 175686a8..cbc9399f 100644 --- a/lrauv_system_tests/test/simulation/test_spawn.cc +++ b/lrauv_system_tests/test/simulation/test_spawn.cc @@ -93,8 +93,8 @@ TEST(VehicleSpawnTest, Spawn) ASSERT_TRUE(WaitForConnections(spawnPublisher, 5s)); // No specific orientation, vehicle will face North - const gz::math::Angle lat1 = IGN_DTOR(20.0); - const gz::math::Angle lon1 = IGN_DTOR(20.0); + const gz::math::Angle lat1 = GZ_DTOR(20.0); + const gz::math::Angle lon1 = GZ_DTOR(20.0); { lrauv_ignition_plugins::msgs::LRAUVInit spawnMessage; spawnMessage.mutable_id_()->set_data("vehicle1"); @@ -118,9 +118,9 @@ TEST(VehicleSpawnTest, Spawn) // Spawn vehicle facing South // Orientation is in NED, so 180 degrees yaw is South - const gz::math::Angle lat2 = IGN_DTOR(20.1); - const gz::math::Angle lon2 = IGN_DTOR(20.1); - const gz::math::Angle yaw2 = IGN_DTOR(180); + const gz::math::Angle lat2 = GZ_DTOR(20.1); + const gz::math::Angle lon2 = GZ_DTOR(20.1); + const gz::math::Angle yaw2 = GZ_DTOR(180); const double depth2 = 10.0; { lrauv_ignition_plugins::msgs::LRAUVInit spawnMessage; @@ -171,7 +171,7 @@ TEST(VehicleSpawnTest, Spawn) // Facing North (-90 rotation from default West orientation) EXPECT_NEAR(0.0, lastPose1.Rot().Roll(), tightTol); EXPECT_NEAR(0.0, lastPose1.Rot().Pitch(), tightTol); - EXPECT_NEAR(-IGN_PI*0.5, lastPose1.Rot().Yaw(), tightTol); + EXPECT_NEAR(-GZ_PI*0.5, lastPose1.Rot().Yaw(), tightTol); const auto &lastLatLon1 = observer1.SphericalCoordinates().back(); EXPECT_NEAR(lat1.Degree(), lastLatLon1.X(), tightTol); @@ -202,5 +202,5 @@ TEST(VehicleSpawnTest, Spawn) // it has a 90 degree yaw in ENU EXPECT_NEAR(0.0, lastPose2.Rot().Roll(), tightTol); EXPECT_NEAR(0.0, lastPose2.Rot().Pitch(), tightTol); - EXPECT_NEAR(IGN_DTOR(90), lastPose2.Rot().Yaw(), tightTol); + EXPECT_NEAR(GZ_DTOR(90), lastPose2.Rot().Yaw(), tightTol); } diff --git a/lrauv_system_tests/test/simulation/test_state.cc b/lrauv_system_tests/test/simulation/test_state.cc index 3575bc43..718f9821 100644 --- a/lrauv_system_tests/test/simulation/test_state.cc +++ b/lrauv_system_tests/test/simulation/test_state.cc @@ -46,9 +46,9 @@ class VehicleStateTest : public ::testing::Test protected: lrauv_system_tests::VehicleStateTestFixture fixture; - protected: const gz::math::Angle initialLat{IGN_DTOR(35.5999984741211)}; + protected: const gz::math::Angle initialLat{GZ_DTOR(35.5999984741211)}; - protected: const gz::math::Angle initialLon{IGN_DTOR(-121.779998779297)}; + protected: const gz::math::Angle initialLon{GZ_DTOR(-121.779998779297)}; }; // Checks that don't change throughout the test @@ -124,7 +124,7 @@ TEST_F(VehicleStateTest, ThrustState) // Propel vehicle forward by giving the propeller a positive // angular velocity. Vehicle is supposed to move at around // 1 m/s with 300 RPM. 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s - command.set_propomegaaction_(10 * IGN_PI); + command.set_propomegaaction_(10 * GZ_PI); // Neutral buoyancy command.set_buoyancyaction_(0.0005); @@ -153,7 +153,7 @@ TEST_F(VehicleStateTest, ThrustState) CheckInvariants(latestState); // Actuators - EXPECT_NEAR(10.0 * IGN_PI, latestState.propomega_(), 1e-6); + EXPECT_NEAR(10.0 * GZ_PI, latestState.propomega_(), 1e-6); EXPECT_NEAR(0.0, latestState.rudderangle_(), 1e-3); EXPECT_NEAR(0.0005, latestState.buoyancyposition_(), 1e-6); @@ -196,7 +196,7 @@ TEST_F(VehicleStateTest, ThrustAndTurnState) // Propel vehicle forward by giving the propeller a positive // angular velocity. Vehicle is supposed to move at around // 1 m/s with 300 RPM. 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s - command.set_propomegaaction_(10 * IGN_PI); + command.set_propomegaaction_(10 * GZ_PI); // Neutral buoyancy command.set_buoyancyaction_(0.0005); @@ -236,7 +236,7 @@ TEST_F(VehicleStateTest, ThrustAndTurnState) CheckInvariants(latestState); // Actuators - EXPECT_NEAR(10.0 * IGN_PI, latestState.propomega_(), 1e-3); + EXPECT_NEAR(10.0 * GZ_PI, latestState.propomega_(), 1e-3); EXPECT_NEAR(-0.5, latestState.rudderangle_(), 0.06); EXPECT_NEAR(0.0005, latestState.buoyancyposition_(), 1e-6); diff --git a/lrauv_system_tests/test/vehicle/test_ahrs.cc b/lrauv_system_tests/test/vehicle/test_ahrs.cc index 943c37a9..af4cccbb 100644 --- a/lrauv_system_tests/test/vehicle/test_ahrs.cc +++ b/lrauv_system_tests/test/vehicle/test_ahrs.cc @@ -169,7 +169,7 @@ TEST(AHRSTest, FrameConventionsAreCorrect) // Make vehicle face west auto &manipulator = fixture.VehicleManipulator(); - manipulator.SetOrientation({0., 0., IGN_DTOR(90.)}); + manipulator.SetOrientation({0., 0., GZ_DTOR(90.)}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -199,13 +199,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - 0., 0., IGN_DTOR(-90.)), + 0., 0., GZ_DTOR(-90.)), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Make vehicle face south - manipulator.SetOrientation({0., 0., IGN_DTOR(180.)}); + manipulator.SetOrientation({0., 0., GZ_DTOR(180.)}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -235,13 +235,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - 0., 0., IGN_DTOR(-180.)), + 0., 0., GZ_DTOR(-180.)), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Make vehicle face east - manipulator.SetOrientation({0., 0., IGN_DTOR(-90.)}); + manipulator.SetOrientation({0., 0., GZ_DTOR(-90.)}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -271,13 +271,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - 0., 0., IGN_DTOR(90.)), + 0., 0., GZ_DTOR(90.)), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Put vehicle in a vertical position heading upwards - manipulator.SetOrientation({IGN_DTOR(90.), 0., 0.}); + manipulator.SetOrientation({GZ_DTOR(90.), 0., 0.}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -307,13 +307,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - 0., IGN_DTOR(90.), 0.), + 0., GZ_DTOR(90.), 0.), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Put vehicle in a vertical position heading downwards - manipulator.SetOrientation({IGN_DTOR(-90.), -0., 0.}); + manipulator.SetOrientation({GZ_DTOR(-90.), -0., 0.}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -343,13 +343,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - 0., IGN_DTOR(-90.), 0.), + 0., GZ_DTOR(-90.), 0.), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Make vehicle roll towards starboard - manipulator.SetOrientation({0., IGN_DTOR(90.), 0.}); + manipulator.SetOrientation({0., GZ_DTOR(90.), 0.}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -379,13 +379,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - IGN_DTOR(90.), 0., 0.), + GZ_DTOR(90.), 0., 0.), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Make vehicle roll towards port - manipulator.SetOrientation({0., IGN_DTOR(-90.), 0.}); + manipulator.SetOrientation({0., GZ_DTOR(-90.), 0.}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -415,13 +415,13 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - IGN_DTOR(-90.), 0., 0.), + GZ_DTOR(-90.), 0., 0.), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; // Make vehicle do a half barrel roll - manipulator.SetOrientation({0., IGN_DTOR(180.), 0.}); + manipulator.SetOrientation({0., GZ_DTOR(180.), 0.}); EXPECT_LT(0, fixture.Step(200ms)); measurement = fixture.ReadLastMeasurement(); @@ -451,7 +451,7 @@ TEST(AHRSTest, FrameConventionsAreCorrect) AreQuaternionsEqual( measurement.orientation, gz::math::Quaterniond( - IGN_DTOR(180.), 0., 0.), + GZ_DTOR(180.), 0., 0.), orientationTolerance)) << "Last orientation estimate was " << measurement.orientation; diff --git a/lrauv_system_tests/test/vehicle/test_propeller_action.cc b/lrauv_system_tests/test/vehicle/test_propeller_action.cc index f838ea5b..8aee4844 100644 --- a/lrauv_system_tests/test/vehicle/test_propeller_action.cc +++ b/lrauv_system_tests/test/vehicle/test_propeller_action.cc @@ -44,7 +44,7 @@ TEST(PropellerActionTest, ForwardThrust) // angular velocity. Vehicle is supposed to move at around // 1 m/s with 300 RPM. 300 RPM = 300 * 2 pi / 60 = 10 pi rad/s lrauv_ignition_plugins::msgs::LRAUVCommand command; - command.set_propomegaaction_(10. * IGN_PI); + command.set_propomegaaction_(10. * GZ_PI); // Neutral buoyancy command.set_dropweightstate_(true); diff --git a/lrauv_system_tests/test/vehicle/test_sensor.cc b/lrauv_system_tests/test/vehicle/test_sensor.cc index 87088638..f43c9153 100644 --- a/lrauv_system_tests/test/vehicle/test_sensor.cc +++ b/lrauv_system_tests/test/vehicle/test_sensor.cc @@ -84,8 +84,8 @@ void SpawnVehicle( const double _lat, const double _lon, const double _depth, const int _acommsAddr) { - gz::math::Angle lat1 = IGN_DTOR(_lat); - gz::math::Angle lon1 = IGN_DTOR(_lon); + gz::math::Angle lat1 = GZ_DTOR(_lat); + gz::math::Angle lon1 = GZ_DTOR(_lon); lrauv_ignition_plugins::msgs::LRAUVInit spawnMsg; spawnMsg.mutable_id_()->set_data(_modelName); diff --git a/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc b/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc index 87f6ea4c..3ea9a324 100644 --- a/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc +++ b/lrauv_system_tests/test/vehicle/test_sensor_timeinterpolation.cc @@ -66,8 +66,8 @@ void SpawnVehicle( const double _lat, const double _lon, const double _depth, const int _acommsAddr) { - gz::math::Angle lat1 = IGN_DTOR(_lat); - gz::math::Angle lon1 = IGN_DTOR(_lon); + gz::math::Angle lat1 = GZ_DTOR(_lat); + gz::math::Angle lon1 = GZ_DTOR(_lon); lrauv_ignition_plugins::msgs::LRAUVInit spawnMsg; spawnMsg.mutable_id_()->set_data(_modelName);