diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index a1c121ea4d..fbcac678cf 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1 +1 @@ -See the [Gazebo contributing guide](https://gazebosim.org/docs/all/contributing). +See [Gazebo's contribution guide](https://gazebosim.org/docs/all/contributing). diff --git a/Changelog.md b/Changelog.md index f53327fc19..adbce79903 100644 --- a/Changelog.md +++ b/Changelog.md @@ -433,6 +433,71 @@ ## Gazebo Sim 6.x +### Gazebo Sim 6.13.0 (2022-11-04) + +1. Fix two tests on Windows + * [Pull request #1779](https://github.com/gazebosim/ign-gazebo/pull/1779) + +1. 3 to 6 20221013 + * [Pull request #1762](https://github.com/gazebosim/ign-gazebo/pull/1762) + +1. Some minor changes to hydrodynamic flags test + * [Pull request #1772](https://github.com/gazebosim/ign-gazebo/pull/1772) + +1. Fix thruster integration test + * [Pull request #1767](https://github.com/gazebosim/ign-gazebo/pull/1767) + +1. Fix scene_broadcaster_system test + * [Pull request #1766](https://github.com/gazebosim/ign-gazebo/pull/1766) + +1. Script and tutorial for generating procedural datasets with Blender + * [Pull request #1412](https://github.com/gazebosim/ign-gazebo/pull/1412) + +1. Enable use of ign gazebo -s on Windows (take two) + * [Pull request #1764](https://github.com/gazebosim/ign-gazebo/pull/1764) + +1. Removed unused speedlimit file + * [Pull request #1761](https://github.com/gazebosim/ign-gazebo/pull/1761) + +1. Fortress: Removed warnings + * [Pull request #1754](https://github.com/gazebosim/ign-gazebo/pull/1754) + +1. Enable/Disable individual hydrodynamic components. + * [Pull request #1692](https://github.com/gazebosim/ign-gazebo/pull/1692) + +1. Adding thrust coefficient calculation + * [Pull request #1652](https://github.com/gazebosim/ign-gazebo/pull/1652) + +1. Restore Add System GUI plugin + * [Pull request #1685](https://github.com/gazebosim/ign-gazebo/pull/1685) + +1. Return absolute path when finding a resource + * [Pull request #1741](https://github.com/gazebosim/ign-gazebo/pull/1741) + +1. Adds sky cubemap URI to the sky.proto's header + * [Pull request #1739](https://github.com/gazebosim/ign-gazebo/pull/1739) + +1. Update triggered_publisher.sdf + * [Pull request #1737](https://github.com/gazebosim/ign-gazebo/pull/1737) + +1. Add ResourceSpawner example file + * [Pull request #1701](https://github.com/gazebosim/ign-gazebo/pull/1701) + +1. Enable inherited model topic name. + * [Pull request #1689](https://github.com/gazebosim/ign-gazebo/pull/1689) + +1. Fix loading render engine plugins in GUI + * [Pull request #1694](https://github.com/gazebosim/ign-gazebo/pull/1694) + +1. Add a service to trigger functionality + * [Pull request #1611](https://github.com/gazebosim/ign-gazebo/pull/1611) + +1. Fix installation instructions on Ubuntu 22.04 + * [Pull request #1686](https://github.com/gazebosim/ign-gazebo/pull/1686) + +1. Fix reference link in ackermann steering + * [Pull request #1683](https://github.com/gazebosim/ign-gazebo/pull/1683) + ### Gazebo Sim 6.12.0 (2022-08-30) 1. Add topic parameter to thrust plugin diff --git a/Migration.md b/Migration.md index 679847aafc..3ca625815c 100644 --- a/Migration.md +++ b/Migration.md @@ -59,6 +59,13 @@ message's header. + In the Hydrodynamics plugin, inverted the added mass contribution to make it act in the correct direction. + +## Gazebo Sim 6.11.X to 6.12.X + + * **Modified**: + + In the Hydrodynamics plugin, inverted the added mass contribution to make it + act in the correct direction. + ## Gazebo Sim 6.1 to 6.2 * If no `` is given to the `Thruster` plugin, the namespace now diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index c1d033d353..2b70a68648 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -1,11 +1,6 @@ diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index ee8d1d59ed..9464922c46 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -332,7 +332,7 @@ TEST_F(ComponentTest, OStream) EXPECT_EQ("Mass: 0", ostr.str()); } - // Component with a gz::msgs type that gets serialized by the default + // Component with a msgs type that gets serialized by the default // serializer { using Custom = components::ComponentMassMatrix().Mass()); } - // Component with a gz::msgs type that gets deserialized by the message + // Component with a msgs type that gets deserialized by the message // deserializer { using Custom = components::Componentset_sec(secNsec.first); _msg->set_nsec(secNsec.second); } @@ -1062,17 +1062,17 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) msgs::MagnetometerSensor *sensor = out.mutable_magnetometer(); if (_in.MagnetometerSensor()->XNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_x_noise(), + sim::set(sensor->mutable_x_noise(), _in.MagnetometerSensor()->XNoise()); } if (_in.MagnetometerSensor()->YNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_y_noise(), + sim::set(sensor->mutable_y_noise(), _in.MagnetometerSensor()->YNoise()); } if (_in.MagnetometerSensor()->ZNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_z_noise(), + sim::set(sensor->mutable_z_noise(), _in.MagnetometerSensor()->ZNoise()); } } @@ -1160,13 +1160,13 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) if (_in.AltimeterSensor()->VerticalPositionNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_vertical_position_noise(), + sim::set(sensor->mutable_vertical_position_noise(), _in.AltimeterSensor()->VerticalPositionNoise()); } if (_in.AltimeterSensor()->VerticalVelocityNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_vertical_velocity_noise(), + sim::set(sensor->mutable_vertical_velocity_noise(), _in.AltimeterSensor()->VerticalVelocityNoise()); } } @@ -1185,7 +1185,7 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) if (_in.AirPressureSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_pressure_noise(), + sim::set(sensor->mutable_pressure_noise(), _in.AirPressureSensor()->PressureNoise()); } sensor->set_reference_altitude( @@ -1206,38 +1206,38 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) if (sdfImu->LinearAccelerationXNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_linear_acceleration()->mutable_x_noise(), sdfImu->LinearAccelerationXNoise()); } if (sdfImu->LinearAccelerationYNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_linear_acceleration()->mutable_y_noise(), sdfImu->LinearAccelerationYNoise()); } if (sdfImu->LinearAccelerationZNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_linear_acceleration()->mutable_z_noise(), sdfImu->LinearAccelerationZNoise()); } if (sdfImu->AngularVelocityXNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_angular_velocity()->mutable_x_noise(), sdfImu->AngularVelocityXNoise()); } if (sdfImu->AngularVelocityYNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_angular_velocity()->mutable_y_noise(), sdfImu->AngularVelocityYNoise()); } if (sdfImu->AngularVelocityZNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set( + sim::set( sensor->mutable_angular_velocity()->mutable_z_noise(), sdfImu->AngularVelocityZNoise()); } @@ -1271,7 +1271,7 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) if (sdfLidar->LidarNoise().Type() != sdf::NoiseType::NONE) { - gz::sim::set(sensor->mutable_noise(), sdfLidar->LidarNoise()); + sim::set(sensor->mutable_noise(), sdfLidar->LidarNoise()); } sensor->set_horizontal_samples(sdfLidar->HorizontalScanSamples()); sensor->set_horizontal_resolution(sdfLidar->HorizontalScanResolution()); @@ -1318,17 +1318,17 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) { if (_in.magnetometer().has_x_noise()) { - sensor.SetXNoise(gz::sim::convert( + sensor.SetXNoise(sim::convert( _in.magnetometer().x_noise())); } if (_in.magnetometer().has_y_noise()) { - sensor.SetYNoise(gz::sim::convert( + sensor.SetYNoise(sim::convert( _in.magnetometer().y_noise())); } if (_in.magnetometer().has_z_noise()) { - sensor.SetZNoise(gz::sim::convert( + sensor.SetZNoise(sim::convert( _in.magnetometer().z_noise())); } } @@ -1383,13 +1383,13 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) { if (_in.altimeter().has_vertical_position_noise()) { - sensor.SetVerticalPositionNoise(gz::sim::convert( + sensor.SetVerticalPositionNoise(sim::convert( _in.altimeter().vertical_position_noise())); } if (_in.altimeter().has_vertical_velocity_noise()) { - sensor.SetVerticalVelocityNoise(gz::sim::convert( + sensor.SetVerticalVelocityNoise(sim::convert( _in.altimeter().vertical_velocity_noise())); } } @@ -1408,7 +1408,7 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) { if (_in.air_pressure().has_pressure_noise()) { - sensor.SetPressureNoise(gz::sim::convert( + sensor.SetPressureNoise(sim::convert( _in.air_pressure().pressure_noise())); } @@ -1432,19 +1432,19 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) if (_in.imu().linear_acceleration().has_x_noise()) { sensor.SetLinearAccelerationXNoise( - gz::sim::convert( + sim::convert( _in.imu().linear_acceleration().x_noise())); } if (_in.imu().linear_acceleration().has_y_noise()) { sensor.SetLinearAccelerationYNoise( - gz::sim::convert( + sim::convert( _in.imu().linear_acceleration().y_noise())); } if (_in.imu().linear_acceleration().has_z_noise()) { sensor.SetLinearAccelerationZNoise( - gz::sim::convert( + sim::convert( _in.imu().linear_acceleration().z_noise())); } } @@ -1454,19 +1454,19 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) if (_in.imu().angular_velocity().has_x_noise()) { sensor.SetAngularVelocityXNoise( - gz::sim::convert( + sim::convert( _in.imu().angular_velocity().x_noise())); } if (_in.imu().angular_velocity().has_y_noise()) { sensor.SetAngularVelocityYNoise( - gz::sim::convert( + sim::convert( _in.imu().angular_velocity().y_noise())); } if (_in.imu().angular_velocity().has_z_noise()) { sensor.SetAngularVelocityZNoise( - gz::sim::convert( + sim::convert( _in.imu().angular_velocity().z_noise())); } } @@ -1523,7 +1523,7 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) if (_in.lidar().has_noise()) { - sensor.SetLidarNoise(gz::sim::convert( + sensor.SetLidarNoise(sim::convert( _in.lidar().noise())); } } diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 19f14bde38..93c045e1a9 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -58,8 +58,8 @@ TEST(Conversions, Light) light.SetPoseRelativeTo("world"); light.SetCastShadows(true); light.SetVisualize(true); - light.SetDiffuse(gz::math::Color(0.4f, 0.5f, 0.6f, 1.0)); - light.SetSpecular(gz::math::Color(0.8f, 0.9f, 0.1f, 1.0)); + light.SetDiffuse(math::Color(0.4f, 0.5f, 0.6f, 1.0)); + light.SetSpecular(math::Color(0.8f, 0.9f, 0.1f, 1.0)); light.SetAttenuationRange(3.2); light.SetConstantAttenuationFactor(0.5); light.SetLinearAttenuationFactor(0.1); @@ -246,11 +246,11 @@ TEST(Conversions, Time) TEST(Conversions, Material) { sdf::Material material; - material.SetDiffuse(gz::math::Color(0.1f, 0.2f, 0.3f, 0.4f)); - material.SetSpecular(gz::math::Color(0.5f, 0.6f, 0.7f, 0.8f)); - material.SetAmbient(gz::math::Color(0.9f, 1.0f, 1.1f, 1.2f)); + material.SetDiffuse(math::Color(0.1f, 0.2f, 0.3f, 0.4f)); + material.SetSpecular(math::Color(0.5f, 0.6f, 0.7f, 0.8f)); + material.SetAmbient(math::Color(0.9f, 1.0f, 1.1f, 1.2f)); material.SetShininess(0.5); - material.SetEmissive(gz::math::Color(1.3f, 1.4f, 1.5f, 1.6f)); + material.SetEmissive(math::Color(1.3f, 1.4f, 1.5f, 1.6f)); material.SetLighting(true); material.SetRenderOrder(2.5); material.SetDoubleSided(true); @@ -344,7 +344,7 @@ TEST(Conversions, GeometryBox) geometry.SetType(sdf::GeometryType::BOX); sdf::Box boxShape; - boxShape.SetSize(gz::math::Vector3d(1, 2, 3)); + boxShape.SetSize(math::Vector3d(1, 2, 3)); geometry.SetBoxShape(boxShape); auto geometryMsg = convert(geometry); @@ -458,7 +458,7 @@ TEST(Conversions, GeometryMesh) geometry.SetType(sdf::GeometryType::MESH); sdf::Mesh meshShape; - meshShape.SetScale(gz::math::Vector3d(1, 2, 3)); + meshShape.SetScale(math::Vector3d(1, 2, 3)); meshShape.SetUri("file://watermelon"); meshShape.SetSubmesh("grape"); meshShape.SetCenterSubmesh(true); @@ -489,8 +489,8 @@ TEST(Conversions, GeometryPlane) geometry.SetType(sdf::GeometryType::PLANE); sdf::Plane planeShape; - planeShape.SetSize(gz::math::Vector2d(1, 2)); - planeShape.SetNormal(gz::math::Vector3d::UnitY); + planeShape.SetSize(math::Vector2d(1, 2)); + planeShape.SetNormal(math::Vector3d::UnitY); geometry.SetPlaneShape(planeShape); auto geometryMsg = convert(geometry); @@ -684,8 +684,8 @@ TEST(Conversions, JointAxis) TEST(Conversions, Scene) { sdf::Scene scene; - scene.SetAmbient(gz::math::Color(0.1f, 0.2f, 0.3f, 0.4f)); - scene.SetBackground(gz::math::Color(0.5f, 0.6f, 0.7f, 0.8f)); + scene.SetAmbient(math::Color(0.1f, 0.2f, 0.3f, 0.4f)); + scene.SetBackground(math::Color(0.5f, 0.6f, 0.7f, 0.8f)); scene.SetShadows(true); scene.SetGrid(true); scene.SetOriginVisual(true); @@ -777,7 +777,7 @@ TEST(Conversions, MagnetometerSensor) sensor.SetType(sdf::SensorType::MAGNETOMETER); sensor.SetUpdateRate(12.4); sensor.SetTopic("my_topic"); - sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Noise noise; noise.SetType(sdf::NoiseType::GAUSSIAN); @@ -817,7 +817,7 @@ TEST(Conversions, AltimeterSensor) sensor.SetType(sdf::SensorType::ALTIMETER); sensor.SetUpdateRate(12.4); sensor.SetTopic("my_topic"); - sensor.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + sensor.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Noise noise; noise.SetType(sdf::NoiseType::GAUSSIAN); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 9236144d06..99c8a6baec 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -1547,9 +1547,9 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, } ////////////////////////////////////////////////// -gz::msgs::SerializedState EntityComponentManager::ChangedState() const +msgs::SerializedState EntityComponentManager::ChangedState() const { - gz::msgs::SerializedState stateMsg; + msgs::SerializedState stateMsg; // New entities for (const auto &entity : this->dataPtr->newlyCreatedEntities) @@ -1574,7 +1574,7 @@ gz::msgs::SerializedState EntityComponentManager::ChangedState() const ////////////////////////////////////////////////// void EntityComponentManager::ChangedState( - gz::msgs::SerializedStateMap &_state) const + msgs::SerializedStateMap &_state) const { // New entities for (const auto &entity : this->dataPtr->newlyCreatedEntities) @@ -1642,11 +1642,11 @@ void EntityComponentManagerPrivate::CalculateStateThreadLoad() } ////////////////////////////////////////////////// -gz::msgs::SerializedState EntityComponentManager::State( +msgs::SerializedState EntityComponentManager::State( const std::unordered_set &_entities, const std::unordered_set &_types) const { - gz::msgs::SerializedState stateMsg; + msgs::SerializedState stateMsg; for (const auto &it : this->dataPtr->componentTypeIndex) { auto entity = it.first; @@ -1712,7 +1712,7 @@ void EntityComponentManager::State( ////////////////////////////////////////////////// void EntityComponentManager::SetState( - const gz::msgs::SerializedState &_stateMsg) + const msgs::SerializedState &_stateMsg) { GZ_PROFILE("EntityComponentManager::SetState Non-map"); // Create / remove / update entities @@ -1810,7 +1810,7 @@ void EntityComponentManager::SetState( ////////////////////////////////////////////////// void EntityComponentManager::SetState( - const gz::msgs::SerializedStateMap &_stateMsg) + const msgs::SerializedStateMap &_stateMsg) { GZ_PROFILE("EntityComponentManager::SetState Map"); // Create / remove / update entities diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index e17cac0940..cf942fb717 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -52,27 +52,27 @@ using IntComponent = components::Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.IntComponent", IntComponent) -using UIntComponent = components::Component; +using UIntComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.UIntComponent", UIntComponent) -using DoubleComponent = components::Component; +using DoubleComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.DoubleComponent", DoubleComponent) using StringComponent = - components::Component; + Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.StringComponent", StringComponent) -using BoolComponent = components::Component; +using BoolComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.BoolComponent", BoolComponent) -using Even = components::Component; +using Even = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Even", Even) -using Odd = components::Component; +using Odd = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Odd", Odd) struct Custom @@ -80,7 +80,7 @@ struct Custom int dummy{123}; }; -using CustomComponent = components::Component; +using CustomComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.CustomComponent", CustomComponent) } @@ -406,19 +406,19 @@ TEST_P(EntityComponentManagerFixture, } { - const auto *value = manager.Component(ePose); + const auto *value = manager.Component(ePose); ASSERT_NE(nullptr, value); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), value->Data()); - auto data = manager.ComponentData(ePose); + auto data = manager.ComponentData(ePose); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), data); - EXPECT_TRUE(manager.SetComponentData(ePose, + EXPECT_TRUE(manager.SetComponentData(ePose, {4, 5, 6, 0, 0, 0})); - data = manager.ComponentData(ePose); + data = manager.ComponentData(ePose); EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), data); - EXPECT_FALSE(manager.SetComponentData(ePose, + EXPECT_FALSE(manager.SetComponentData(ePose, {4, 5, 6, 0, 0, 0})); } @@ -1545,11 +1545,11 @@ TEST_P(EntityComponentManagerFixture, EXPECT_TRUE(manager.SetParentEntity(e6, e2)); EXPECT_TRUE(manager.SetParentEntity(e7, e2)); - EXPECT_FALSE(manager.SetParentEntity(e1, sim::Entity(1000))); - EXPECT_FALSE(manager.SetParentEntity(sim::Entity(1000), e1)); + EXPECT_FALSE(manager.SetParentEntity(e1, Entity(1000))); + EXPECT_FALSE(manager.SetParentEntity(Entity(1000), e1)); // Check their parents - EXPECT_EQ(sim::kNullEntity, manager.ParentEntity(e1)); + EXPECT_EQ(kNullEntity, manager.ParentEntity(e1)); EXPECT_EQ(e1, manager.ParentEntity(e2)); EXPECT_EQ(e1, manager.ParentEntity(e3)); EXPECT_EQ(e2, manager.ParentEntity(e4)); @@ -1558,8 +1558,8 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(e2, manager.ParentEntity(e7)); // Detach from graph - EXPECT_TRUE(manager.SetParentEntity(e7, sim::kNullEntity)); - EXPECT_EQ(sim::kNullEntity, manager.ParentEntity(e7)); + EXPECT_TRUE(manager.SetParentEntity(e7, kNullEntity)); + EXPECT_EQ(kNullEntity, manager.ParentEntity(e7)); // Reparent EXPECT_TRUE(manager.SetParentEntity(e5, e3)); @@ -3018,11 +3018,11 @@ TEST_P(EntityComponentManagerFixture, // create an entity and component auto entity = originalECMStateMap.CreateEntity(); - originalECMStateMap.CreateComponent(entity, components::IntComponent(1)); + originalECMStateMap.CreateComponent(entity, IntComponent(1)); int foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *) { foundEntities++; return true; @@ -3034,8 +3034,8 @@ TEST_P(EntityComponentManagerFixture, originalECMStateMap.State(stateMapMsg); otherECMStateMap.SetState(stateMapMsg); foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(1, _intComp->Data()); @@ -3045,12 +3045,12 @@ TEST_P(EntityComponentManagerFixture, // modify a component and then share the update with the other ECM stateMapMsg.Clear(); - originalECMStateMap.SetComponentData(entity, 2); + originalECMStateMap.SetComponentData(entity, 2); originalECMStateMap.State(stateMapMsg); otherECMStateMap.SetState(stateMapMsg); foundEntities = 0; - otherECMStateMap.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMStateMap.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(2, _intComp->Data()); @@ -3064,8 +3064,8 @@ TEST_P(EntityComponentManagerFixture, EntityComponentManager otherECMState; foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *) + otherECMState.Each( + [&](const Entity &, const IntComponent *) { foundEntities++; return true; @@ -3073,13 +3073,13 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(0, foundEntities); entity = originalECMState.CreateEntity(); - originalECMState.CreateComponent(entity, components::IntComponent(1)); + originalECMState.CreateComponent(entity, IntComponent(1)); auto stateMsg = originalECMState.State(); otherECMState.SetState(stateMsg); foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMState.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(1, _intComp->Data()); @@ -3088,12 +3088,12 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(1, foundEntities); stateMsg.Clear(); - originalECMState.SetComponentData(entity, 2); + originalECMState.SetComponentData(entity, 2); stateMsg = originalECMState.State(); otherECMState.SetState(stateMsg); foundEntities = 0; - otherECMState.Each( - [&](const Entity &, const components::IntComponent *_intComp) + otherECMState.Each( + [&](const Entity &, const IntComponent *_intComp) { foundEntities++; EXPECT_EQ(2, _intComp->Data()); @@ -3229,18 +3229,18 @@ TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) { Entity entity1 = manager.CreateEntity(); math::Pose3d testPose{1, 2, 3, 0.1, 0.2, 0.3}; - manager.CreateComponent(entity1, components::Pose{testPose}); - manager.CreateComponent(entity1, components::Name{"entity1"}); + manager.CreateComponent(entity1, Pose{testPose}); + manager.CreateComponent(entity1, Name{"entity1"}); Entity entity2 = manager.CreateEntity(); - manager.CreateComponent(entity2, components::Name{"entity2"}); + manager.CreateComponent(entity2, Name{"entity2"}); EntityCompMgrTest managerCopy; managerCopy.CopyFrom(manager); // Add entity3 after a copy has been made. Entity entity3 = manager.CreateEntity(); - manager.CreateComponent(entity3, components::Name{"entity3"}); + manager.CreateComponent(entity3, Name{"entity3"}); // Emulate a step so that entity1 can be actually removed. manager.RunClearNewlyCreatedEntities(); @@ -3256,8 +3256,8 @@ TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) { std::vector removedEntities; - manager.EachRemoved( - [&](const Entity &_entity, const components::Name *) + manager.EachRemoved( + [&](const Entity &_entity, const Name *) { removedEntities.push_back(_entity); return true; diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc index eb4c55d668..470088753b 100644 --- a/src/SdfEntityCreator_TEST.cc +++ b/src/SdfEntityCreator_TEST.cc @@ -59,7 +59,7 @@ using namespace gz; using namespace sim; ///////////////////////////////////////////////// -class EntityCompMgrTest : public sim::EntityComponentManager +class EntityCompMgrTest : public EntityComponentManager { public: void ProcessEntityRemovals() { @@ -167,35 +167,35 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (modelCount == 1) { - EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), _pose->Data()); EXPECT_EQ("box", _name->Data()); boxModelEntity = _entity; } else if (modelCount == 2) { - EXPECT_EQ(gz::math::Pose3d(-1, -2, -3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, 0, 1), _pose->Data()); EXPECT_EQ("cylinder", _name->Data()); cylModelEntity = _entity; } else if (modelCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 1), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 1), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; } else if (modelCount == 4) { - EXPECT_EQ(gz::math::Pose3d(-4, -5, -6, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-4, -5, -6, 0, 0, 1), _pose->Data()); EXPECT_EQ("capsule", _name->Data()); capModelEntity = _entity; } else if (modelCount == 5) { - EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 1), + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 1), _pose->Data()); EXPECT_EQ("ellipsoid", _name->Data()); ellipModelEntity = _entity; @@ -236,7 +236,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (linkCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_link", _name->Data()); @@ -247,7 +247,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (linkCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_link", _name->Data()); @@ -258,7 +258,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (linkCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); @@ -370,7 +370,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (collisionCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_collision", _name->Data()); @@ -385,7 +385,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (collisionCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_collision", _name->Data()); @@ -400,7 +400,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (collisionCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_collision", _name->Data()); @@ -485,7 +485,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) if (visualCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_visual", _name->Data()); @@ -511,7 +511,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (visualCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_visual", _name->Data()); @@ -537,7 +537,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) } else if (visualCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -636,7 +636,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) lightCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("sun", _name->Data()); @@ -646,19 +646,19 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_EQ("sun", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ("", _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(1000, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(gz::math::Vector3d(-0.5, 0.1, -0.9), + EXPECT_EQ(math::Vector3d(-0.5, 0.1, -0.9), _light->Data().Direction()); return true; }); @@ -733,7 +733,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ(worldEntity, this->ecm.ParentEntity(_entity)); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -764,7 +764,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) linkCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -803,7 +803,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) visualCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -845,7 +845,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // light attached to link if (lightCount == 1u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("link_light_point", _name->Data()); @@ -854,13 +854,13 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("link_light_point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.0f, 0.0f, 1.0f, 1), + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(2, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.05, _light->Data().ConstantAttenuationFactor()); @@ -870,7 +870,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // directional light in the world else if (lightCount == 2u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("directional", _name->Data()); @@ -879,25 +879,25 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("directional", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(100, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(gz::math::Vector3d(0.5, 0.2, -0.9), + EXPECT_EQ(math::Vector3d(0.5, 0.2, -0.9), _light->Data().Direction()); } // point light in the world else if (lightCount == 3u) { - EXPECT_EQ(gz::math::Pose3d(0.0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, -1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("point", _name->Data()); @@ -906,13 +906,13 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, -1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(1.0f, 0.0f, 0.0f, 1), + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(4, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.2, _light->Data().ConstantAttenuationFactor()); @@ -922,7 +922,7 @@ TEST_F(SdfEntityCreatorTest, CreateLights) // spot light in the world else if (lightCount == 4u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("spot", _name->Data()); @@ -931,19 +931,19 @@ TEST_F(SdfEntityCreatorTest, CreateLights) EXPECT_EQ("spot", _light->Data().Name()); EXPECT_EQ(sdf::LightType::SPOT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.0f, 1.0f, 0.0f, 1), + EXPECT_EQ(math::Color(0.0f, 1.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(5, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.3, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.4, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(gz::math::Vector3d(0.0, 0.0, -1.0), + EXPECT_EQ(math::Vector3d(0.0, 0.0, -1.0), _light->Data().Direction()); EXPECT_DOUBLE_EQ(0.1, _light->Data().SpotInnerAngle().Radian()); EXPECT_DOUBLE_EQ(0.5, _light->Data().SpotOuterAngle().Radian()); @@ -1218,7 +1218,7 @@ size_t removedCount(EntityCompMgrTest &_manager) { size_t count = 0; _manager.EachRemoved( - [&](const gz::sim::Entity &, const Ts *...) -> bool + [&](const Entity &, const Ts *...) -> bool { ++count; return true; diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 6c325ba684..5461e18707 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -297,7 +297,7 @@ class gz::sim::ServerConfigPrivate public: std::string logRecordCompressPath = ""; /// \brief Path to where simulation resources, such as models downloaded - /// from fuel.ignitionrobotics.org, should be stored. + /// from fuel.gazebosim.org, should be stored. public: std::string resourceCache = ""; /// \brief File containing physics engine plugin. If empty, DART will be used. @@ -543,7 +543,7 @@ unsigned int ServerConfig::Seed() const void ServerConfig::SetSeed(unsigned int _seed) { this->dataPtr->seed = _seed; - gz::math::Rand::Seed(_seed); + math::Rand::Seed(_seed); } ///////////////////////////////////////////////// @@ -904,7 +904,7 @@ parsePluginsFromDoc(const tinyxml2::XMLDocument &_doc) ///////////////////////////////////////////////// std::list -gz::sim::parsePluginsFromFile(const std::string &_fname) +sim::parsePluginsFromFile(const std::string &_fname) { tinyxml2::XMLDocument doc; doc.LoadFile(_fname.c_str()); @@ -913,7 +913,7 @@ gz::sim::parsePluginsFromFile(const std::string &_fname) ///////////////////////////////////////////////// std::list -gz::sim::parsePluginsFromString(const std::string &_str) +sim::parsePluginsFromString(const std::string &_str) { tinyxml2::XMLDocument doc; doc.Parse(_str.c_str()); @@ -922,21 +922,21 @@ gz::sim::parsePluginsFromString(const std::string &_str) ///////////////////////////////////////////////// std::list -gz::sim::loadPluginInfo(bool _isPlayback) +sim::loadPluginInfo(bool _isPlayback) { std::list ret; // 1. Check contents of environment variable std::string envConfig; - bool configSet = gz::common::env(sim::kServerConfigPathEnv, - envConfig, - true); + bool configSet = common::env(sim::kServerConfigPathEnv, + envConfig, + true); if (!configSet) { - configSet = gz::common::env("IGN_GAZEBO_SERVER_CONFIG_PATH", - envConfig, - true); + configSet = common::env("IGN_GAZEBO_SERVER_CONFIG_PATH", + envConfig, + true); if (configSet) { gzwarn << "Config path found using deprecated environment variable " @@ -947,16 +947,16 @@ gz::sim::loadPluginInfo(bool _isPlayback) if (configSet) { - if (gz::common::exists(envConfig)) + if (common::exists(envConfig)) { // Parse configuration stored in environment variable - ret = gz::sim::parsePluginsFromFile(envConfig); + ret = sim::parsePluginsFromFile(envConfig); if (ret.empty()) { // This may be desired behavior, but warn just in case. // Some users may want to defer all loading until later // during runtime. - gzwarn << sim::kServerConfigPathEnv + gzwarn << kServerConfigPathEnv << " set but no plugins found\n"; } gzdbg << "Loaded (" << ret.size() << ") plugins from file " << @@ -969,7 +969,7 @@ gz::sim::loadPluginInfo(bool _isPlayback) // This may be desired behavior, but warn just in case. // Some users may want to defer all loading until late // during runtime. - gzwarn << sim::kServerConfigPathEnv + gzwarn << kServerConfigPathEnv << " set but no file found," << " no plugins loaded\n"; return ret; @@ -987,27 +987,27 @@ gz::sim::loadPluginInfo(bool _isPlayback) } std::string defaultConfigDir; - gz::common::env(GZ_HOMEDIR, defaultConfigDir); - defaultConfigDir = gz::common::joinPaths(defaultConfigDir, ".gz", + common::env(GZ_HOMEDIR, defaultConfigDir); + defaultConfigDir = common::joinPaths(defaultConfigDir, ".gz", "sim", GZ_SIM_MAJOR_VERSION_STR); - auto defaultConfig = gz::common::joinPaths(defaultConfigDir, + auto defaultConfig = common::joinPaths(defaultConfigDir, configFilename); - if (!gz::common::exists(defaultConfig)) + if (!common::exists(defaultConfig)) { - auto installedConfig = gz::common::joinPaths( + auto installedConfig = common::joinPaths( GZ_SIM_SERVER_CONFIG_PATH, configFilename); - if (!gz::common::createDirectories(defaultConfigDir)) + if (!common::createDirectories(defaultConfigDir)) { gzerr << "Failed to create directory [" << defaultConfigDir << "]." << std::endl; return ret; } - if (!gz::common::exists(installedConfig)) + if (!common::exists(installedConfig)) { gzerr << "Failed to copy installed config [" << installedConfig << "] to default config [" << defaultConfig << "]." @@ -1015,7 +1015,7 @@ gz::sim::loadPluginInfo(bool _isPlayback) << std::endl; return ret; } - else if (!gz::common::copyFile(installedConfig, defaultConfig)) + else if (!common::copyFile(installedConfig, defaultConfig)) { gzerr << "Failed to copy installed config [" << installedConfig << "] to default config [" << defaultConfig << "]." @@ -1030,7 +1030,7 @@ gz::sim::loadPluginInfo(bool _isPlayback) } } - ret = gz::sim::parsePluginsFromFile(defaultConfig); + ret = sim::parsePluginsFromFile(defaultConfig); if (ret.empty()) { diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 2f7d250408..4fdbaf8f1d 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -179,11 +179,11 @@ TEST(ParsePluginsFromFile, PlaybackConfig) TEST(LoadPluginInfo, FromEmptyEnv) { // Set environment to something that doesn't exist - ASSERT_TRUE(common::setenv(sim::kServerConfigPathEnv, "foo")); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, "foo")); auto plugins = loadPluginInfo(); EXPECT_EQ(0u, plugins.size()); - EXPECT_TRUE(common::unsetenv(sim::kServerConfigPathEnv)); + EXPECT_TRUE(common::unsetenv(kServerConfigPathEnv)); } ////////////////////////////////////////////////// @@ -192,7 +192,7 @@ TEST(LoadPluginInfo, FromValidEnv) auto validPath = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "server_valid2.config"); - ASSERT_TRUE(common::setenv(sim::kServerConfigPathEnv, validPath)); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, validPath)); auto plugins = loadPluginInfo(); ASSERT_EQ(2u, plugins.size()); @@ -211,7 +211,7 @@ TEST(LoadPluginInfo, FromValidEnv) EXPECT_EQ("TestModelSystem", plugin->Plugin().Filename()); EXPECT_EQ("gz::sim::TestModelSystem", plugin->Plugin().Name()); - EXPECT_TRUE(common::unsetenv(sim::kServerConfigPathEnv)); + EXPECT_TRUE(common::unsetenv(kServerConfigPathEnv)); } ////////////////////////////////////////////////// diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 8b1d0ac4ec..97b3930c55 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -385,7 +385,7 @@ void ServerPrivate::SetupTransport() } ////////////////////////////////////////////////// -bool ServerPrivate::WorldsService(gz::msgs::StringMsg_V &_res) +bool ServerPrivate::WorldsService(msgs::StringMsg_V &_res) { std::lock_guard lock(this->worldsMutex); @@ -401,7 +401,7 @@ bool ServerPrivate::WorldsService(gz::msgs::StringMsg_V &_res) ////////////////////////////////////////////////// bool ServerPrivate::ServerControlService( - const gz::msgs::ServerControl &_req, msgs::Boolean &_res) + const msgs::ServerControl &_req, msgs::Boolean &_res) { _res.set_data(false); @@ -444,7 +444,7 @@ bool ServerPrivate::ServerControlService( ////////////////////////////////////////////////// void ServerPrivate::AddResourcePathsService( - const gz::msgs::StringMsg_V &_req) + const msgs::StringMsg_V &_req) { std::vector paths; for (int i = 0; i < _req.data_size(); ++i) @@ -467,7 +467,7 @@ void ServerPrivate::AddResourcePathsService( ////////////////////////////////////////////////// bool ServerPrivate::ResourcePathsService( - gz::msgs::StringMsg_V &_res) + msgs::StringMsg_V &_res) { _res.Clear(); diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index ff80cd09fe..3c8731253b 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -54,7 +54,7 @@ class ServerFixture : public InternalFixture<::testing::TestWithParam> // See https://github.com/gazebosim/gz-sim/issues/1175 TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(DefaultServerConfig)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; EXPECT_TRUE(serverConfig.SdfFile().empty()); EXPECT_TRUE(serverConfig.SdfString().empty()); EXPECT_FALSE(serverConfig.UpdateRate()); @@ -112,7 +112,7 @@ TEST_P(ServerFixture, ServerConfigPluginInfo) sdfPlugin.SetName("interface"); pluginInfo.SetPlugin(sdfPlugin); - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.AddPlugin(pluginInfo); const std::list &plugins = serverConfig.Plugins(); @@ -352,7 +352,7 @@ TEST_P(ServerFixture, ///////////////////////////////////////////////// TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(SdfServerConfig)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); EXPECT_TRUE(serverConfig.SdfFile().empty()); @@ -497,7 +497,7 @@ TEST_P(ServerFixture, ///////////////////////////////////////////////// TEST_P(ServerFixture, SdfStringServerConfig) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "shapes.sdf")); @@ -716,7 +716,7 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RunOncePaused)) ///////////////////////////////////////////////// TEST_P(ServerFixture, RunNonBlockingMultiple) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfString(TestWorldSansPhysics::World()); sim::Server server(serverConfig); @@ -817,7 +817,7 @@ TEST_P(ServerFixture, ServerControlStop) ///////////////////////////////////////////////// TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(AddSystemWhileRunning)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "shapes.sdf")); @@ -867,7 +867,7 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(AddSystemWhileRunning)) ///////////////////////////////////////////////// TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(AddSystemAfterLoad)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "shapes.sdf")); @@ -932,18 +932,18 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(AddSystemAfterLoad)) ///////////////////////////////////////////////// TEST_P(ServerFixture, Seed) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; EXPECT_EQ(0u, serverConfig.Seed()); unsigned int mySeed = 12345u; serverConfig.SetSeed(mySeed); EXPECT_EQ(mySeed, serverConfig.Seed()); - EXPECT_EQ(mySeed, gz::math::Rand::Seed()); + EXPECT_EQ(mySeed, math::Rand::Seed()); } ///////////////////////////////////////////////// TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) { - gz::common::setenv("GZ_SIM_RESOURCE_PATH", + common::setenv("GZ_SIM_RESOURCE_PATH", (common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds:") + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models")).c_str()); @@ -1003,7 +1003,7 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) // Check physics system loaded meshes and got their BB correct eachCount = 0; _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::AxisAlignedBox *_box)->bool { auto box = _box->Data(); @@ -1032,7 +1032,7 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) ///////////////////////////////////////////////// TEST_P(ServerFixture, GetResourcePaths) { - gz::common::setenv("GZ_SIM_RESOURCE_PATH", + common::setenv("GZ_SIM_RESOURCE_PATH", std::string("/tmp/some/path") + common::SystemPaths::Delimiter() + std::string("/home/user/another_path")); @@ -1064,12 +1064,12 @@ TEST_P(ServerFixture, GetResourcePaths) ///////////////////////////////////////////////// TEST_P(ServerFixture, AddResourcePaths) { - gz::common::setenv("GZ_SIM_RESOURCE_PATH", + common::setenv("GZ_SIM_RESOURCE_PATH", std::string("/tmp/some/path") + common::SystemPaths::Delimiter() + std::string("/home/user/another_path")); - gz::common::setenv("SDF_PATH", ""); - gz::common::setenv("GZ_FILE_PATH", ""); + common::setenv("SDF_PATH", ""); + common::setenv("GZ_FILE_PATH", ""); ServerConfig serverConfig; sim::Server server(serverConfig); @@ -1174,8 +1174,8 @@ TEST_P(ServerFixture, ResolveResourcePaths) // Make sure the resource path is clear common::setenv("GZ_SIM_RESOURCE_PATH", ""); - // An absolute path should return the same absolute path - test(PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + // A valid path should be returned as an absolute path + test(PROJECT_SOURCE_PATH, common::absPath(PROJECT_SOURCE_PATH), true); // An absolute path, with the file:// prefix, should return the absolute path test(std::string("file://") + diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index afb3909147..2172895678 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -211,7 +211,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, { gzmsg << "No systems loaded from SDF, loading defaults" << std::endl; bool isPlayback = !this->serverConfig.LogPlaybackPath().empty(); - auto plugins = gz::sim::loadPluginInfo(isPlayback); + auto plugins = sim::loadPluginInfo(isPlayback); this->LoadServerPlugins(plugins); } @@ -417,14 +417,14 @@ void SimulationRunner::PublishStats() GZ_PROFILE("SimulationRunner::PublishStats"); // Create the world statistics message. - gz::msgs::WorldStatistics msg; + msgs::WorldStatistics msg; msg.set_real_time_factor(this->realTimeFactor); auto realTimeSecNsec = - gz::math::durationToSecNsec(this->currentInfo.realTime); + math::durationToSecNsec(this->currentInfo.realTime); auto simTimeSecNsec = - gz::math::durationToSecNsec(this->currentInfo.simTime); + math::durationToSecNsec(this->currentInfo.simTime); msg.mutable_real_time()->set_sec(realTimeSecNsec.first); msg.mutable_real_time()->set_nsec(realTimeSecNsec.second); @@ -453,7 +453,7 @@ void SimulationRunner::PublishStats() // Create and publish the clock message. The clock message is not // throttled. - gz::msgs::Clock clockMsg; + msgs::Clock clockMsg; clockMsg.mutable_real()->set_sec(realTimeSecNsec.first); clockMsg.mutable_real()->set_nsec(realTimeSecNsec.second); clockMsg.mutable_sim()->set_sec(simTimeSecNsec.first); @@ -544,7 +544,7 @@ void SimulationRunner::UpdateSystems() { GZ_PROFILE("SimulationRunner::UpdateSystems"); // \todo(nkoenig) Systems used to be updated in parallel using - // a gz::common::WorkerPool. There is overhead associated with + // a common::WorkerPool. There is overhead associated with // this, most notably the creation and destruction of WorkOrders (see // WorkerPool.cc). We could turn on parallel updates in the future, and/or // turn it on if there are sufficient systems. More testing is required. @@ -666,14 +666,14 @@ bool SimulationRunner::Run(const uint64_t _iterations) // https://github.com/gazebosim/gz-gui/pull/306 and // https://github.com/gazebosim/gz-sim/pull/1163) advertOpts.SetMsgsPerSec(10); - this->statsPub = this->node->Advertise( + this->statsPub = this->node->Advertise( "stats", advertOpts); } if (!this->rootStatsPub.Valid()) { // Check for the existence of other publishers on `/stats` - std::vector publishers; + std::vector publishers; this->node->TopicInfo("/stats", publishers); if (!publishers.empty()) @@ -700,13 +700,13 @@ bool SimulationRunner::Run(const uint64_t _iterations) // Create the clock publisher. if (!this->clockPub.Valid()) - this->clockPub = this->node->Advertise("clock"); + this->clockPub = this->node->Advertise("clock"); // Create the global clock publisher. if (!this->rootClockPub.Valid()) { // Check for the existence of other publishers on `/clock` - std::vector publishers; + std::vector publishers; this->node->TopicInfo("/clock", publishers); if (!publishers.empty()) @@ -726,7 +726,7 @@ bool SimulationRunner::Run(const uint64_t _iterations) { gzmsg << "Found no publishers on /clock, adding root clock topic" << std::endl; - this->rootClockPub = this->node->Advertise( + this->rootClockPub = this->node->Advertise( "/clock"); } } @@ -1446,7 +1446,7 @@ bool SimulationRunner::RequestRemoveEntity(const Entity _entity, } ////////////////////////////////////////////////// -bool SimulationRunner::GuiInfoService(gz::msgs::GUI &_res) +bool SimulationRunner::GuiInfoService(msgs::GUI &_res) { _res.Clear(); diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 978c704290..1c15509d25 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -72,11 +72,11 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { -using IntComponent = components::Component; +using IntComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.IntComponent", IntComponent) -using DoubleComponent = components::Component; +using DoubleComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.DoubleComponent", DoubleComponent) } @@ -122,31 +122,31 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::World::typeId)); + World::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Model::typeId)); + Model::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::CanonicalLink::typeId)); + CanonicalLink::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Link::typeId)); + Link::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Collision::typeId)); + Collision::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Visual::typeId)); + Visual::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Light::typeId)); + Light::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Name::typeId)); + Name::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentEntity::typeId)); + ParentEntity::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Geometry::typeId)); + Geometry::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( components::Material::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Inertial::typeId)); + Inertial::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Wind::typeId)); + Wind::typeId)); // Check entities // 1 x world + 1 x (default) level + 1 x wind + 5 x model + 5 x link + 5 x @@ -156,10 +156,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check worlds unsigned int worldCount{0}; Entity worldEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::World *_world, + const World *_world, const components::Name *_name)->bool { EXPECT_NE(nullptr, _world); @@ -187,14 +187,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) Entity sphModelEntity = kNullEntity; Entity capModelEntity = kNullEntity; Entity ellipModelEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Model *_model, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Model *_model, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _model); @@ -207,35 +207,35 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(worldEntity, _parent->Data()); if (modelCount == 1) { - EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), _pose->Data()); EXPECT_EQ("box", _name->Data()); boxModelEntity = _entity; } else if (modelCount == 2) { - EXPECT_EQ(gz::math::Pose3d(-1, -2, -3, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-1, -2, -3, 0, 0, 1), _pose->Data()); EXPECT_EQ("cylinder", _name->Data()); cylModelEntity = _entity; } else if (modelCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 1), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 1), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; } else if (modelCount == 4) { - EXPECT_EQ(gz::math::Pose3d(-4, -5, -6, 0, 0, 1), + EXPECT_EQ(math::Pose3d(-4, -5, -6, 0, 0, 1), _pose->Data()); EXPECT_EQ("capsule", _name->Data()); capModelEntity = _entity; } else if (modelCount == 5) { - EXPECT_EQ(gz::math::Pose3d(4, 5, 6, 0, 0, 1), + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 1), _pose->Data()); EXPECT_EQ("ellipsoid", _name->Data()); ellipModelEntity = _entity; @@ -257,14 +257,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) Entity sphLinkEntity = kNullEntity; Entity capLinkEntity = kNullEntity; Entity ellipLinkEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Link *_link, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Link *_link, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _link); @@ -276,7 +276,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (linkCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.1, 0.1, 0.1, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_link", _name->Data()); EXPECT_EQ(boxModelEntity, _parent->Data()); @@ -284,7 +284,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.2, 0.2, 0.2, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_link", _name->Data()); EXPECT_EQ(cylModelEntity, _parent->Data()); @@ -292,7 +292,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.3, 0.3, 0.3, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -300,7 +300,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 4) { - EXPECT_EQ(gz::math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.5, 0.5, 0.5, 0, 0, 0), _pose->Data()); EXPECT_EQ("capsule_link", _name->Data()); EXPECT_EQ(capModelEntity, _parent->Data()); @@ -308,7 +308,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (linkCount == 5) { - EXPECT_EQ(gz::math::Pose3d(0.8, 0.8, 0.8, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.8, 0.8, 0.8, 0, 0, 0), _pose->Data()); EXPECT_EQ("ellipsoid_link", _name->Data()); EXPECT_EQ(ellipModelEntity, _parent->Data()); @@ -326,10 +326,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check inertials unsigned int inertialCount{0}; - runner.EntityCompMgr().Each( + runner.EntityCompMgr().Each( [&](const Entity & _entity, - const components::Link *_link, - const components::Inertial *_inertial)->bool + const Link *_link, + const Inertial *_inertial)->bool { EXPECT_NE(nullptr, _link); EXPECT_NE(nullptr, _inertial); @@ -373,16 +373,16 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check collisions unsigned int collisionCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Collision *_collision, - const components::Geometry *_geometry, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Collision *_collision, + const Geometry *_geometry, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _collision); @@ -395,7 +395,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (collisionCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.11, 0.11, 0.11, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_collision", _name->Data()); @@ -409,7 +409,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.21, 0.21, 0.21, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_collision", _name->Data()); @@ -423,7 +423,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.31, 0.31, 0.31, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_collision", _name->Data()); @@ -436,7 +436,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 4) { - EXPECT_EQ(gz::math::Pose3d(0.51, 0.51, 0.51, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.51, 0.51, 0.51, 0, 0, 0), _pose->Data()); EXPECT_EQ("capsule_collision", _name->Data()); @@ -450,7 +450,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (collisionCount == 5) { - EXPECT_EQ(gz::math::Pose3d(0.81, 0.81, 0.81, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.81, 0.81, 0.81, 0, 0, 0), _pose->Data()); EXPECT_EQ("ellipsoid_collision", _name->Data()); @@ -459,7 +459,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(sdf::GeometryType::ELLIPSOID, _geometry->Data().Type()); EXPECT_NE(nullptr, _geometry->Data().EllipsoidShape()); - EXPECT_EQ(gz::math::Vector3d(0.4, 0.6, 1.6), + EXPECT_EQ(math::Vector3d(0.4, 0.6, 1.6), _geometry->Data().EllipsoidShape()->Radii()); } return true; @@ -469,18 +469,18 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check visuals unsigned int visualCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Visual *_visual, - const components::Geometry *_geometry, + const Visual *_visual, + const Geometry *_geometry, const components::Material *_material, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _visual); @@ -494,7 +494,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) if (visualCount == 1) { - EXPECT_EQ(gz::math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.12, 0.12, 0.12, 0, 0, 0), _pose->Data()); EXPECT_EQ("box_visual", _name->Data()); @@ -513,7 +513,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 2) { - EXPECT_EQ(gz::math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.22, 0.22, 0.22, 0, 0, 0), _pose->Data()); EXPECT_EQ("cylinder_visual", _name->Data()); @@ -532,7 +532,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 3) { - EXPECT_EQ(gz::math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.32, 0.32, 0.32, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -550,7 +550,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 4) { - EXPECT_EQ(gz::math::Pose3d(0.52, 0.52, 0.52, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.52, 0.52, 0.52, 0, 0, 0), _pose->Data()); EXPECT_EQ("capsule_visual", _name->Data()); @@ -569,7 +569,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) } else if (visualCount == 5) { - EXPECT_EQ(gz::math::Pose3d(0.82, 0.82, 0.82, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.82, 0.82, 0.82, 0, 0, 0), _pose->Data()); EXPECT_EQ("ellipsoid_visual", _name->Data()); @@ -578,7 +578,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(sdf::GeometryType::ELLIPSOID, _geometry->Data().Type()); EXPECT_NE(nullptr, _geometry->Data().EllipsoidShape()); - EXPECT_EQ(gz::math::Vector3d(0.4, 0.6, 1.6), + EXPECT_EQ(math::Vector3d(0.4, 0.6, 1.6), _geometry->Data().EllipsoidShape()->Radii()); EXPECT_EQ(math::Color(0.0f, 0.0f, 0.0f), _material->Data().Emissive()); @@ -593,14 +593,14 @@ TEST_P(SimulationRunnerTest, CreateEntities) // Check lights unsigned int lightCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Light *_light, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Light *_light, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _light); @@ -610,7 +610,7 @@ TEST_P(SimulationRunnerTest, CreateEntities) lightCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("sun", _name->Data()); @@ -619,20 +619,20 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ("sun", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ("", _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(1000, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); EXPECT_DOUBLE_EQ(1.0, _light->Data().Intensity()); - EXPECT_EQ(gz::math::Vector3d(-0.5, 0.1, -0.9), + EXPECT_EQ(math::Vector3d(-0.5, 0.1, -0.9), _light->Data().Direction()); return true; }); @@ -662,10 +662,10 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check worlds unsigned int worldCount{0}; Entity worldEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::World *_world, + const World *_world, const components::Name *_name)->bool { EXPECT_NE(nullptr, _world); @@ -685,14 +685,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check model unsigned int modelCount{0}; Entity sphModelEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Model *_model, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Model *_model, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _model); @@ -703,7 +703,7 @@ TEST_P(SimulationRunnerTest, CreateLights) modelCount++; EXPECT_EQ(worldEntity, _parent->Data()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere", _name->Data()); sphModelEntity = _entity; @@ -717,14 +717,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check link unsigned int linkCount{0}; Entity sphLinkEntity = kNullEntity; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Link *_link, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Link *_link, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _link); @@ -734,7 +734,7 @@ TEST_P(SimulationRunnerTest, CreateLights) linkCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_link", _name->Data()); EXPECT_EQ(sphModelEntity, _parent->Data()); @@ -748,18 +748,18 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check visuals unsigned int visualCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Visual *_visual, - const components::Geometry *_geometry, + const Visual *_visual, + const Geometry *_geometry, const components::Material *_material, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _visual); @@ -771,7 +771,7 @@ TEST_P(SimulationRunnerTest, CreateLights) visualCount++; - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 0.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("sphere_visual", _name->Data()); @@ -792,14 +792,14 @@ TEST_P(SimulationRunnerTest, CreateLights) // Check lights unsigned int lightCount{0}; - runner.EntityCompMgr().Each( [&](const Entity &/*_entity*/, - const components::Light *_light, - const components::Pose *_pose, - const components::ParentEntity *_parent, + const Light *_light, + const Pose *_pose, + const ParentEntity *_parent, const components::Name *_name)->bool { EXPECT_NE(nullptr, _light); @@ -812,19 +812,19 @@ TEST_P(SimulationRunnerTest, CreateLights) // light attached to link if (lightCount == 1u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), _pose->Data()); EXPECT_EQ("link_light_point", _name->Data()); EXPECT_EQ(sphLinkEntity, _parent->Data()); EXPECT_EQ("link_light_point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 1, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 1, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.0f, 0.0f, 1.0f, 1), + EXPECT_EQ(math::Color(0.0f, 0.0f, 1.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(2, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.05, _light->Data().ConstantAttenuationFactor()); @@ -834,43 +834,43 @@ TEST_P(SimulationRunnerTest, CreateLights) // directional light in the world else if (lightCount == 2u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 0.0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 0.0, 10, 0, 0, 0), _pose->Data()); EXPECT_EQ("directional", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("directional", _light->Data().Name()); EXPECT_EQ(sdf::LightType::DIRECTIONAL, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 0, 10, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 0, 10, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_TRUE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.8f, 0.8f, 0.8f, 1), + EXPECT_EQ(math::Color(0.8f, 0.8f, 0.8f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(100, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.9, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.01, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(gz::math::Vector3d(0.5, 0.2, -0.9), + EXPECT_EQ(math::Vector3d(0.5, 0.2, -0.9), _light->Data().Direction()); } // point light in the world else if (lightCount == 3u) { - EXPECT_EQ(gz::math::Pose3d(0.0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, -1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("point", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("point", _light->Data().Name()); EXPECT_EQ(sdf::LightType::POINT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, -1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, -1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(1.0f, 0.0f, 0.0f, 1), + EXPECT_EQ(math::Color(1.0f, 0.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.1f, 0.1f, 0.1f, 1), + EXPECT_EQ(math::Color(0.1f, 0.1f, 0.1f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(4, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.2, _light->Data().ConstantAttenuationFactor()); @@ -880,25 +880,25 @@ TEST_P(SimulationRunnerTest, CreateLights) // spot light in the world else if (lightCount == 4u) { - EXPECT_EQ(gz::math::Pose3d(0.0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0.0, 1.5, 3, 0, 0, 0), _pose->Data()); EXPECT_EQ("spot", _name->Data()); EXPECT_EQ(worldEntity, _parent->Data()); EXPECT_EQ("spot", _light->Data().Name()); EXPECT_EQ(sdf::LightType::SPOT, _light->Data().Type()); - EXPECT_EQ(gz::math::Pose3d(0, 1.5, 3, 0, 0, 0), + EXPECT_EQ(math::Pose3d(0, 1.5, 3, 0, 0, 0), _light->Data().RawPose()); EXPECT_EQ(std::string(), _light->Data().PoseRelativeTo()); EXPECT_FALSE(_light->Data().CastShadows()); - EXPECT_EQ(gz::math::Color(0.0f, 1.0f, 0.0f, 1), + EXPECT_EQ(math::Color(0.0f, 1.0f, 0.0f, 1), _light->Data().Diffuse()); - EXPECT_EQ(gz::math::Color(0.2f, 0.2f, 0.2f, 1), + EXPECT_EQ(math::Color(0.2f, 0.2f, 0.2f, 1), _light->Data().Specular()); EXPECT_DOUBLE_EQ(5, _light->Data().AttenuationRange()); EXPECT_DOUBLE_EQ(0.3, _light->Data().ConstantAttenuationFactor()); EXPECT_DOUBLE_EQ(0.4, _light->Data().LinearAttenuationFactor()); EXPECT_DOUBLE_EQ(0.001, _light->Data().QuadraticAttenuationFactor()); - EXPECT_EQ(gz::math::Vector3d(0.0, 0.0, -1.0), + EXPECT_EQ(math::Vector3d(0.0, 0.0, -1.0), _light->Data().Direction()); EXPECT_DOUBLE_EQ(0.1, _light->Data().SpotInnerAngle().Radian()); EXPECT_DOUBLE_EQ(0.5, _light->Data().SpotOuterAngle().Radian()); @@ -926,34 +926,34 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) // Check component types EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::World::typeId)); + World::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::CanonicalLink::typeId)); + CanonicalLink::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Link::typeId)); + Link::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Joint::typeId)); + Joint::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::JointAxis::typeId)); + JointAxis::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::JointType::typeId)); + JointType::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ChildLinkName::typeId)); + ChildLinkName::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentLinkName::typeId)); + ParentLinkName::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::ParentEntity::typeId)); + ParentEntity::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Pose::typeId)); + Pose::typeId)); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType( - components::Name::typeId)); + Name::typeId)); const sdf::Model *model = root.WorldByIndex(0)->ModelByIndex(1); // Check canonical links unsigned int canonicalLinkCount{0}; - runner.EntityCompMgr().Each( - [&](const Entity &, const components::CanonicalLink *)->bool + runner.EntityCompMgr().Each( + [&](const Entity &, const CanonicalLink *)->bool { canonicalLinkCount++; return true; @@ -973,11 +973,11 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) }; auto testJoint = [&testAxis](const sdf::Joint *_joint, - const components::JointAxis *_axis, - const components::JointAxis2 *_axis2, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName, - const components::Pose *_pose, + const JointAxis *_axis, + const JointAxis2 *_axis2, + const ParentLinkName *_parentLinkName, + const ChildLinkName *_childLinkName, + const Pose *_pose, const components::Name *_name, bool _checkAxis = true, bool _checkAxis2 = false) @@ -1012,25 +1012,25 @@ TEST_P(SimulationRunnerTest, CreateJointEntities) }; std::set jointTypes; - runner.EntityCompMgr().Each( [&](const Entity &_entity, - const components::Joint * /*_joint*/, - const components::JointType *_jointType, - const components::ParentLinkName *_parentLinkName, - const components::ChildLinkName *_childLinkName, - const components::Pose *_pose, + const Joint * /*_joint*/, + const JointType *_jointType, + const ParentLinkName *_parentLinkName, + const ChildLinkName *_childLinkName, + const Pose *_pose, const components::Name *_name)->bool { jointTypes.insert(_jointType->Data()); auto axis = - runner.EntityCompMgr().Component(_entity); + runner.EntityCompMgr().Component(_entity); auto axis2 = - runner.EntityCompMgr().Component(_entity); + runner.EntityCompMgr().Component(_entity); const sdf::Joint *joint = model->JointByName(_name->Data()); @@ -1200,9 +1200,9 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Get world entity Entity worldId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::World *_world)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const World *_world)->bool { EXPECT_NE(nullptr, _world); worldId = _entity; @@ -1212,9 +1212,9 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Get model entity Entity modelId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::Model *_model)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Model *_model)->bool { EXPECT_NE(nullptr, _model); modelId = _entity; @@ -1224,9 +1224,9 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Get sensor entity Entity sensorId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::Sensor *_sensor)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Sensor *_sensor)->bool { EXPECT_NE(nullptr, _sensor); sensorId = _entity; @@ -1236,9 +1236,9 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Get visual entity Entity visualId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::Visual *_visual)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const components::Visual *_visual)->bool { EXPECT_NE(nullptr, _visual); visualId = _entity; @@ -1248,7 +1248,7 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; - auto worldComponentId = gz::common::hash64(worldComponentName); + auto worldComponentId = common::hash64(worldComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(worldComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(worldId, @@ -1256,7 +1256,7 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Check component registered by model plugin std::string modelComponentName{"ModelPluginComponent"}; - auto modelComponentId = gz::common::hash64(modelComponentName); + auto modelComponentId = common::hash64(modelComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(modelComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(modelId, @@ -1264,7 +1264,7 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Check component registered by sensor plugin std::string sensorComponentName{"SensorPluginComponent"}; - auto sensorComponentId = gz::common::hash64(sensorComponentName); + auto sensorComponentId = common::hash64(sensorComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(sensorComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, @@ -1272,7 +1272,7 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // Check component registered by visual plugin std::string visualComponentName{"VisualPluginComponent"}; - auto visualComponentId = gz::common::hash64(visualComponentName); + auto visualComponentId = common::hash64(visualComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(visualComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(visualId, @@ -1285,10 +1285,10 @@ TEST_P(SimulationRunnerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LoadPlugins) ) // reproduce? Maybe we need to test unloading plugins, but we have no API for // it yet. #if defined (__clang__) - components::Factory::Instance()->Unregister(worldComponentId); - components::Factory::Instance()->Unregister(modelComponentId); - components::Factory::Instance()->Unregister(sensorComponentId); - components::Factory::Instance()->Unregister(visualComponentId); + Factory::Instance()->Unregister(worldComponentId); + Factory::Instance()->Unregister(modelComponentId); + Factory::Instance()->Unregister(sensorComponentId); + Factory::Instance()->Unregister(visualComponentId); #endif } @@ -1304,7 +1304,7 @@ TEST_P(SimulationRunnerTest, // ServerConfig will fall back to environment variable auto config = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "server_valid2.config"); - ASSERT_EQ(true, common::setenv(sim::kServerConfigPathEnv, config)); + ASSERT_EQ(true, common::setenv(kServerConfigPathEnv, config)); ServerConfig serverConfig; // Create simulation runner @@ -1345,9 +1345,9 @@ TEST_P(SimulationRunnerTest, // Get world entity Entity worldId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::World *_world)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const World *_world)->bool { EXPECT_NE(nullptr, _world); worldId = _entity; @@ -1357,9 +1357,9 @@ TEST_P(SimulationRunnerTest, // Get model entity Entity modelId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::Model *_model)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Model *_model)->bool { EXPECT_NE(nullptr, _model); modelId = _entity; @@ -1369,9 +1369,9 @@ TEST_P(SimulationRunnerTest, // Get sensor entity Entity sensorId{kNullEntity}; - runner.EntityCompMgr().Each([&]( - const gz::sim::Entity &_entity, - const gz::sim::components::Sensor *_sensor)->bool + runner.EntityCompMgr().Each([&]( + const Entity &_entity, + const Sensor *_sensor)->bool { EXPECT_NE(nullptr, _sensor); sensorId = _entity; @@ -1381,7 +1381,7 @@ TEST_P(SimulationRunnerTest, // Check component registered by world plugin std::string worldComponentName{"WorldPluginComponent"}; - auto worldComponentId = gz::common::hash64(worldComponentName); + auto worldComponentId = common::hash64(worldComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(worldComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(worldId, @@ -1389,7 +1389,7 @@ TEST_P(SimulationRunnerTest, // Check component registered by model plugin std::string modelComponentName{"ModelPluginComponent"}; - auto modelComponentId = gz::common::hash64(modelComponentName); + auto modelComponentId = common::hash64(modelComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(modelComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(modelId, @@ -1397,7 +1397,7 @@ TEST_P(SimulationRunnerTest, // Check component registered by sensor plugin std::string sensorComponentName{"SensorPluginComponent"}; - auto sensorComponentId = gz::common::hash64(sensorComponentName); + auto sensorComponentId = common::hash64(sensorComponentName); EXPECT_TRUE(runner.EntityCompMgr().HasComponentType(sensorComponentId)); EXPECT_TRUE(runner.EntityCompMgr().EntityHasComponentType(sensorId, @@ -1410,9 +1410,9 @@ TEST_P(SimulationRunnerTest, // reproduce? Maybe we need to test unloading plugins, but we have no API for // it yet. #if defined (__clang__) - components::Factory::Instance()->Unregister(worldComponentId); - components::Factory::Instance()->Unregister(modelComponentId); - components::Factory::Instance()->Unregister(sensorComponentId); + Factory::Instance()->Unregister(worldComponentId); + Factory::Instance()->Unregister(modelComponentId); + Factory::Instance()->Unregister(sensorComponentId); #endif } @@ -1429,13 +1429,13 @@ TEST_P(SimulationRunnerTest, // The user may have modified their local config. auto config = common::joinPaths(PROJECT_SOURCE_PATH, "include", "gz", "sim", "server.config"); - ASSERT_TRUE(common::setenv(sim::kServerConfigPathEnv, config)); + ASSERT_TRUE(common::setenv(kServerConfigPathEnv, config)); // Create simulation runner auto systemLoader = std::make_shared(); SimulationRunner runner(rootWithout.WorldByIndex(0), systemLoader); ASSERT_EQ(3u, runner.SystemCount()); - common::unsetenv(sim::kServerConfigPathEnv); + common::unsetenv(kServerConfigPathEnv); } ///////////////////////////////////////////////// @@ -1455,24 +1455,24 @@ TEST_P(SimulationRunnerTest, // Get model entities auto boxEntity = runner.EntityCompMgr().EntityByComponents( - gz::sim::components::Model(), - gz::sim::components::Name("box")); + Model(), + components::Name("box")); EXPECT_NE(kNullEntity, boxEntity); auto sphereEntity = runner.EntityCompMgr().EntityByComponents( - gz::sim::components::Model(), - gz::sim::components::Name("sphere")); + Model(), + components::Name("sphere")); EXPECT_NE(kNullEntity, sphereEntity); auto cylinderEntity = runner.EntityCompMgr().EntityByComponents( - gz::sim::components::Model(), - gz::sim::components::Name("cylinder")); + Model(), + components::Name("cylinder")); EXPECT_NE(kNullEntity, cylinderEntity); // We can't access the type registered by the plugin unless we link against // it, but we know its name to check std::string componentName{"ModelPluginComponent"}; - auto componentId = gz::common::hash64(componentName); + auto componentId = common::hash64(componentName); // Check there's no double component EXPECT_FALSE(runner.EntityCompMgr().HasComponentType(componentId)); @@ -1544,7 +1544,7 @@ TEST_P(SimulationRunnerTest, // ServerConfig will fall back to environment variable auto config = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "server_valid2.config"); - ASSERT_EQ(true, common::setenv(sim::kServerConfigPathEnv, config)); + ASSERT_EQ(true, common::setenv(kServerConfigPathEnv, config)); ServerConfig serverConfig; // Create simulation runner diff --git a/src/TestFixture.cc b/src/TestFixture.cc index b5541af33b..34626f5c04 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -113,7 +113,7 @@ class gz::sim::TestFixturePrivate public: void Init(const ServerConfig &_config); /// \brief Pointer to underlying server - public: std::shared_ptr server{nullptr}; + public: std::shared_ptr server{nullptr}; /// \brief Pointer to underlying Helper interface public: std::shared_ptr helperSystem{nullptr}; @@ -149,7 +149,7 @@ TestFixture::~TestFixture() void TestFixturePrivate::Init(const ServerConfig &_config) { this->helperSystem = std::make_shared(); - this->server = std::make_shared(_config); + this->server = std::make_shared(_config); } ////////////////////////////////////////////////// @@ -208,7 +208,7 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const +std::shared_ptr TestFixture::Server() const { return this->dataPtr->server; } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index d7faaffe19..c29e0281df 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -824,7 +824,7 @@ TEST_F(UtilTest, EntityFromMsg) ecm.CreateComponent(actorDEntity, components::ParentEntity(worldEntity)); // Check entities - auto createMsg = [&ecm](Entity _id, const std::string &_name = "", + auto createMsg = [&](Entity _id, const std::string &_name = "", msgs::Entity::Type _type = msgs::Entity::NONE) -> msgs::Entity { msgs::Entity msg; diff --git a/src/gui/AboutDialogHandler.cc b/src/gui/AboutDialogHandler.cc index f5188bc604..ef8963b34e 100644 --- a/src/gui/AboutDialogHandler.cc +++ b/src/gui/AboutDialogHandler.cc @@ -28,15 +28,16 @@ using namespace sim::gui; ///////////////////////////////////////////////// AboutDialogHandler::AboutDialogHandler() { + aboutText += "Gazebo " + std::string(GZ_DISTRIBUTION) + "
"; aboutText += std::string(GZ_SIM_VERSION_HEADER); aboutText += "" "" "" "" "" diff --git a/src/gui/GuiFileHandler.cc b/src/gui/GuiFileHandler.cc index 6006cd4c2d..407f528c7c 100644 --- a/src/gui/GuiFileHandler.cc +++ b/src/gui/GuiFileHandler.cc @@ -49,7 +49,7 @@ void GuiFileHandler::SaveWorldAs(const QString &_fileUrl, std::string localPath = url.toLocalFile().toStdString() + suffix; std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worldsMsg; + msgs::StringMsg_V worldsMsg; bool result{false}; unsigned int timeout{5000}; diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index ba9abd4970..b983f150e1 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -259,7 +259,7 @@ void GuiRunner::RequestState() } } - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(reqSrv); // Subscribe to periodic updates. diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 354238a3d4..73b63bb8a0 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -53,10 +53,10 @@ TEST_F(GuiTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PathManager)) common::Console::SetVerbosity(4); gzdbg << "Start test" << std::endl; - gz::common::setenv("GZ_SIM_RESOURCE_PATH", + common::setenv("GZ_SIM_RESOURCE_PATH", "/from_env:/tmp/more_env"); - gz::common::setenv("SDF_PATH", ""); - gz::common::setenv("GZ_FILE_PATH", ""); + common::setenv("SDF_PATH", ""); + common::setenv("GZ_FILE_PATH", ""); gzdbg << "Environment set" << std::endl; transport::Node node; @@ -96,7 +96,7 @@ TEST_F(GuiTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PathManager)) node.Advertise("/gazebo/resource_paths/get", pathsCb); gzdbg << "Paths advertised" << std::endl; - auto app = gz::sim::gui::createGui( + auto app = createGui( gg_argc, gg_argv, nullptr, nullptr, false, nullptr); EXPECT_NE(nullptr, app); gzdbg << "GUI created" << std::endl; diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index 4e6677cc46..6996ff8384 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -352,14 +352,14 @@ void AlignTool::Align() this->dataPtr->scene = rendering::sceneFromFirstRenderEngine(); // Get current list of selected entities - std::vector selectedList; - gz::rendering::VisualPtr relativeVisual; + std::vector selectedList; + rendering::VisualPtr relativeVisual; for (const auto &entityId : this->dataPtr->selectedEntities) { for (auto i = 0u; i < this->dataPtr->scene->VisualCount(); ++i) { - gz::rendering::VisualPtr vis = + rendering::VisualPtr vis = this->dataPtr->scene->VisualByIndex(i); if (!vis) continue; @@ -388,8 +388,8 @@ void AlignTool::Align() (relativeVisual = selectedList.back()); // Callback function for Gazebo node request - std::function cb = - [](const gz::msgs::Boolean &/* _rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/* _rep*/, const bool _result) { if (!_result) gzerr << "Error setting pose" << std::endl; @@ -403,7 +403,7 @@ void AlignTool::Align() } int axisIndex = static_cast(this->dataPtr->axis); - gz::msgs::Pose req; + msgs::Pose req; gz::math::AxisAlignedBox targetBox = relativeVisual->BoundingBox(); gz::math::Vector3d targetMin = targetBox.Min(); @@ -565,10 +565,10 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - gz::sim::gui::events::EntitiesSelected::kType) + gui::events::EntitiesSelected::kType) { auto selectedEvent = - reinterpret_cast( + reinterpret_cast( _event); // Only update if a valid cast, the data isn't empty, and @@ -588,7 +588,7 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - gz::sim::gui::events::DeselectAllEntities::kType) + gui::events::DeselectAllEntities::kType) { this->dataPtr->selectedEntities.clear(); } @@ -596,5 +596,5 @@ bool AlignTool::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::AlignTool, - gz::gui::Plugin) +GZ_ADD_PLUGIN(AlignTool, + gz::gui::Plugin) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 9965a71a13..26036d3c1d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -208,7 +208,7 @@ void gz::sim::setData(QStandardItem *_item, const msgs::Light &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, +void sim::setData(QStandardItem *_item, const math::Vector3d &_data) { if (nullptr == _item) @@ -225,7 +225,7 @@ void gz::sim::setData(QStandardItem *_item, ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const std::string &_data) +void sim::setData(QStandardItem *_item, const std::string &_data) { if (nullptr == _item) return; @@ -238,7 +238,7 @@ void gz::sim::setData(QStandardItem *_item, const std::string &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, +void sim::setData(QStandardItem *_item, const std::ostringstream &_data) { if (nullptr == _item) @@ -252,7 +252,7 @@ void gz::sim::setData(QStandardItem *_item, ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const bool &_data) +void sim::setData(QStandardItem *_item, const bool &_data) { if (nullptr == _item) return; @@ -264,7 +264,7 @@ void gz::sim::setData(QStandardItem *_item, const bool &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const int &_data) +void sim::setData(QStandardItem *_item, const int &_data) { if (nullptr == _item) return; @@ -276,14 +276,14 @@ void gz::sim::setData(QStandardItem *_item, const int &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const Entity &_data) +void sim::setData(QStandardItem *_item, const Entity &_data) { setData(_item, static_cast(_data)); } ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const double &_data) +void sim::setData(QStandardItem *_item, const double &_data) { if (nullptr == _item) return; @@ -295,7 +295,7 @@ void gz::sim::setData(QStandardItem *_item, const double &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, const sdf::Physics &_data) +void sim::setData(QStandardItem *_item, const sdf::Physics &_data) { if (nullptr == _item) return; @@ -310,7 +310,7 @@ void gz::sim::setData(QStandardItem *_item, const sdf::Physics &_data) ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, +void sim::setData(QStandardItem *_item, const sdf::Material &_data) { if (nullptr == _item) @@ -343,7 +343,7 @@ void gz::sim::setData(QStandardItem *_item, ////////////////////////////////////////////////// template<> -void gz::sim::setData(QStandardItem *_item, +void sim::setData(QStandardItem *_item, const math::SphericalCoordinates &_data) { if (nullptr == _item) @@ -362,7 +362,7 @@ void gz::sim::setData(QStandardItem *_item, } ////////////////////////////////////////////////// -void gz::sim::setUnit(QStandardItem *_item, const std::string &_unit) +void sim::setUnit(QStandardItem *_item, const std::string &_unit) { if (nullptr == _item) return; @@ -391,7 +391,7 @@ ComponentsModel::ComponentsModel() : QStandardItemModel() ///////////////////////////////////////////////// QStandardItem *ComponentsModel::AddComponentType( - gz::sim::ComponentTypeId _typeId) + sim::ComponentTypeId _typeId) { GZ_PROFILE_THREAD_NAME("Qt thread"); GZ_PROFILE("ComponentsModel::AddComponentType"); @@ -422,7 +422,7 @@ QStandardItem *ComponentsModel::AddComponentType( ///////////////////////////////////////////////// void ComponentsModel::RemoveComponentType( - gz::sim::ComponentTypeId _typeId) + sim::ComponentTypeId _typeId) { GZ_PROFILE_THREAD_NAME("Qt thread"); GZ_PROFILE("ComponentsModel::RemoveComponentType"); @@ -459,7 +459,7 @@ QHash ComponentsModel::RoleNames() ComponentInspector::ComponentInspector() : GuiSystem(), dataPtr(std::make_unique()) { - qRegisterMetaType(); + qRegisterMetaType(); qRegisterMetaType("Entity"); } @@ -903,7 +903,7 @@ void ComponentInspector::Update(const UpdateInfo &, } // Remove components no longer present - list items to remove - std::list itemsToRemove; + std::list itemsToRemove; for (auto itemIt : this->dataPtr->componentsModel.items) { auto typeId = itemIt.first; @@ -919,7 +919,7 @@ void ComponentInspector::Update(const UpdateInfo &, QMetaObject::invokeMethod(&this->dataPtr->componentsModel, "RemoveComponentType", Qt::QueuedConnection, - Q_ARG(gz::sim::ComponentTypeId, typeId)); + Q_ARG(sim::ComponentTypeId, typeId)); } } @@ -935,7 +935,7 @@ bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) { if (!this->dataPtr->locked) { - if (_event->type() == sim::gui::events::EntitiesSelected::kType) + if (_event->type() == gui::events::EntitiesSelected::kType) { auto event = reinterpret_cast(_event); if (event && !event->Data().empty()) @@ -944,7 +944,7 @@ bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) } } - if (_event->type() == sim::gui::events::DeselectAllEntities::kType) + if (_event->type() == gui::events::DeselectAllEntities::kType) { auto event = reinterpret_cast( _event); @@ -1086,14 +1086,14 @@ void ComponentInspector::OnLight( ///////////////////////////////////////////////// void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error setting physics parameters" << std::endl; }; - gz::msgs::Physics req; + msgs::Physics req; req.set_max_step_size(_stepSize); req.set_real_time_factor(_realTimeFactor); auto physicsCmdService = "/world/" + this->dataPtr->worldName @@ -1303,5 +1303,5 @@ void ComponentInspector::OnAddSystem(const QString &_name, } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::ComponentInspector, - gz::gui::Plugin) +GZ_ADD_PLUGIN(ComponentInspector, + gz::gui::Plugin) diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 6fb372dac2..2218ef0549 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -526,7 +526,7 @@ void EntityTree::OnLoadMesh(const QString &_mesh) ///////////////////////////////////////////////// bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == gz::sim::gui::events::EntitiesSelected::kType) + if (_event->type() == gui::events::EntitiesSelected::kType) { auto selectedEvent = reinterpret_cast(_event); @@ -544,7 +544,7 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - gz::sim::gui::events::DeselectAllEntities::kType) + gui::events::DeselectAllEntities::kType) { auto deselectAllEvent = reinterpret_cast(_event); @@ -577,5 +577,5 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::EntityTree, - gz::gui::Plugin) +GZ_ADD_PLUGIN(EntityTree, + gz::gui::Plugin) diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc index b51c8f1280..e202513024 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc @@ -291,7 +291,7 @@ void JointPositionController::Update(const UpdateInfo &, QMetaObject::invokeMethod(&this->dataPtr->jointsModel, "RemoveJoint", Qt::QueuedConnection, - Q_ARG(Entity, jointEntity)); + Q_ARG(sim::Entity, jointEntity)); } } } @@ -374,7 +374,7 @@ void JointPositionController::OnCommand(const QString &_jointName, double _pos) { std::string jointName = _jointName.toStdString(); - gz::msgs::Double msg; + msgs::Double msg; msg.set_data(_pos); auto topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->modelName.toStdString() + "/joint/" + jointName + @@ -387,7 +387,7 @@ void JointPositionController::OnCommand(const QString &_jointName, double _pos) return; } - auto pub = this->dataPtr->node.Advertise(topic); + auto pub = this->dataPtr->node.Advertise(topic); pub.Publish(msg); } @@ -404,7 +404,7 @@ void JointPositionController::OnReset() continue; } - gz::msgs::Double msg; + msgs::Double msg; msg.set_data(0); auto topic = transport::TopicUtils::AsValidTopic("/model/" + this->dataPtr->modelName.toStdString() + "/joint/" + jointName + @@ -417,11 +417,11 @@ void JointPositionController::OnReset() return; } - auto pub = this->dataPtr->node.Advertise(topic); + auto pub = this->dataPtr->node.Advertise(topic); pub.Publish(msg); } } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::gui::JointPositionController, +GZ_ADD_PLUGIN(JointPositionController, gz::gui::Plugin) diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index bb724de6ee..bee1896e98 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -86,7 +86,7 @@ using namespace sim; void GzSimPlugin::registerTypes(const char *_uri) { // Register our 'EntityContextMenuItem' in qml engine - qmlRegisterType(_uri, 1, 0, + qmlRegisterType(_uri, 1, 0, "EntityContextMenuItem"); } @@ -163,14 +163,14 @@ void EntityContextMenu::OnRemove( "/world/" + this->dataPtr->worldName + "/remove"; } - std::function cb = - [](const gz::msgs::Boolean &_rep, const bool _result) + std::function cb = + [](const msgs::Boolean &_rep, const bool _result) { if (!_result || !_rep.data()) gzerr << "Error sending remove request" << std::endl; }; - gz::msgs::Entity req; + msgs::Entity req; req.set_name(_data.toStdString()); req.set_type(convert(_type.toStdString())); @@ -180,8 +180,8 @@ void EntityContextMenu::OnRemove( ///////////////////////////////////////////////// void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error sending move to request" << std::endl; @@ -190,13 +190,13 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) std::string request = _request.toStdString(); if (request == "move_to") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->moveToService, req, cb); } else if (request == "follow") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } @@ -232,7 +232,7 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) } else if (request == "view_collisions") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewCollisionsService, req, cb); } diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc index b2a2086d86..56008273a7 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc @@ -251,5 +251,5 @@ void PlaybackScrubber::OnDrop(double _value) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::PlaybackScrubber, - gz::gui::Plugin) +GZ_ADD_PLUGIN(PlaybackScrubber, + gz::gui::Plugin) diff --git a/src/gui/plugins/plot_3d/Plot3D.cc b/src/gui/plugins/plot_3d/Plot3D.cc index c8e921b1cc..6e5e9e1314 100644 --- a/src/gui/plugins/plot_3d/Plot3D.cc +++ b/src/gui/plugins/plot_3d/Plot3D.cc @@ -165,7 +165,7 @@ void Plot3D::ClearPlot() // Clear previous plot if (this->dataPtr->markerMsg.point().size() > 0) { - this->dataPtr->markerMsg.set_action(gz::msgs::Marker::DELETE_MARKER); + this->dataPtr->markerMsg.set_action(msgs::Marker::DELETE_MARKER); this->dataPtr->node.Request("/marker", this->dataPtr->markerMsg); } } @@ -245,7 +245,7 @@ void Plot3D::Update(const UpdateInfo &, EntityComponentManager &_ecm) return; this->dataPtr->prevPos = point; - gz::msgs::Set(this->dataPtr->markerMsg.add_point(), point); + msgs::Set(this->dataPtr->markerMsg.add_point(), point); // Reduce message array if (this->dataPtr->markerMsg.point_size() > this->dataPtr->maxPoints) @@ -401,5 +401,5 @@ void Plot3D::SetMaxPoints(int _maxPoints) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::gui::Plot3D, - gz::gui::Plugin) +GZ_ADD_PLUGIN(Plot3D, + gz::gui::Plugin) diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index f0f9b24dc3..77178315c3 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -218,7 +218,7 @@ ResourceSpawner::ResourceSpawner() gz::gui::App()->Engine()->rootContext()->setContextProperty( "OwnerList", &this->dataPtr->ownerModel); this->dataPtr->fuelClient = - std::make_unique(); + std::make_unique(); } ///////////////////////////////////////////////// @@ -483,7 +483,7 @@ void ResourceSpawner::OnDownloadFuelResource(const QString &_path, // Set the waiting cursor while the resource downloads QGuiApplication::setOverrideCursor(Qt::WaitCursor); if (this->dataPtr->fuelClient->DownloadModel( - gz::common::URI(_path.toStdString()), localPath)) + common::URI(_path.toStdString()), localPath)) { // Successful download, set thumbnail std::string thumbnailPath = common::joinPaths(localPath, "thumbnails"); @@ -565,7 +565,7 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) std::set ownerSet; for (auto const &server : servers) { - std::vector models; + std::vector models; for (auto iter = this->dataPtr->fuelClient->Models(server); iter; ++iter) { models.push_back(iter->Identification()); @@ -585,10 +585,10 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) // If the resource is cached, we can go ahead and populate the // respective information if (this->dataPtr->fuelClient->CachedModel( - gz::common::URI(id.UniqueName()), path)) + common::URI(id.UniqueName()), path)) { resource.isDownloaded = true; - resource.sdfPath = gz::common::joinPaths(path, "model.sdf"); + resource.sdfPath = common::joinPaths(path, "model.sdf"); std::string thumbnailPath = common::joinPaths(path, "thumbnails"); this->SetThumbnail(thumbnailPath, resource); } @@ -634,5 +634,5 @@ void ResourceSpawner::OnResourceSpawn(const QString &_sdfPath) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::ResourceSpawner, - gz::gui::Plugin) +GZ_ADD_PLUGIN(ResourceSpawner, + gz::gui::Plugin) diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index e4c282363d..a2466c58e9 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -81,5 +81,5 @@ void Shapes::OnMode(const QString &_mode) } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::Shapes, - gz::gui::Plugin) +GZ_ADD_PLUGIN(Shapes, + gz::gui::Plugin) diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 44e4344588..d53d385702 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -934,5 +934,5 @@ void TransformControlPrivate::SnapPoint( } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::TransformControl, - gz::gui::Plugin) +GZ_ADD_PLUGIN(TransformControl, + gz::gui::Plugin) diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index beb4e6f379..1425476071 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -328,7 +328,7 @@ void VideoRecorder::LoadConfig(const tinyxml2::XMLElement * _pluginElem) ///////////////////////////////////////////////// bool VideoRecorder::eventFilter(QObject *_obj, QEvent *_event) { - if (_event->type() == gz::gui::events::Render::kType) + if (_event->type() == gui::events::Render::kType) { this->dataPtr->OnRender(); } @@ -394,5 +394,5 @@ void VideoRecorder::OnCancel() } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::VideoRecorder, - gz::gui::Plugin) +GZ_ADD_PLUGIN(VideoRecorder, + gz::gui::Plugin) diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index 772c8678bd..18e88ae8ef 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -399,5 +399,5 @@ bool ViewAnglePrivate::UpdateQtCamClipDist() } // Register this plugin -GZ_ADD_PLUGIN(gz::sim::ViewAngle, - gz::gui::Plugin) +GZ_ADD_PLUGIN(ViewAngle, + gz::gui::Plugin) diff --git a/src/gz.cc b/src/gz.cc index 1337ab96e1..d1e0e21a51 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -57,7 +57,7 @@ extern "C" void cmdVerbosity( const char *_verbosity) { int verbosity = std::atoi(_verbosity); - gz::common::Console::SetVerbosity(verbosity); + common::Console::SetVerbosity(verbosity); // SDFormat only has 2 levels: quiet / loud. Let sim users suppress all SDF // console output with zero verbosity. @@ -79,17 +79,17 @@ extern "C" const char *findFuelResource( { std::string path; std::string worldPath; - gz::fuel_tools::FuelClient fuelClient; + fuel_tools::FuelClient fuelClient; // Attempt to find cached copy, and then attempt download - if (fuelClient.CachedWorld(gz::common::URI(_pathToResource), path)) + if (fuelClient.CachedWorld(common::URI(_pathToResource), path)) { gzmsg << "Cached world found." << std::endl; worldPath = path; } // cppcheck-suppress syntaxError - else if (gz::fuel_tools::Result result = - fuelClient.DownloadWorld(gz::common::URI(_pathToResource), path); + else if (fuel_tools::Result result = + fuelClient.DownloadWorld(common::URI(_pathToResource), path); result) { gzmsg << "Successfully downloaded world from fuel." << std::endl; @@ -102,20 +102,20 @@ extern "C" const char *findFuelResource( return ""; } - if (!gz::common::exists(worldPath)) + if (!common::exists(worldPath)) return ""; // Find the first sdf file in the world path for now, the later intention is // to load an optional world config file first and if that does not exist, // continue to load the first sdf file found as done below - for (gz::common::DirIter file(worldPath); - file != gz::common::DirIter(); ++file) + for (common::DirIter file(worldPath); + file != common::DirIter(); ++file) { std::string current(*file); - if (gz::common::isFile(current)) + if (common::isFile(current)) { - std::string fileName = gz::common::basename(current); + std::string fileName = common::basename(current); std::string::size_type fileExtensionIndex = fileName.rfind("."); std::string fileExtension = fileName.substr(fileExtensionIndex + 1); @@ -139,7 +139,7 @@ extern "C" int runServer(const char *_sdfString, int _headless, float _recordPeriod) { std::string startingWorldPath{""}; - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; // Lock until the starting world is received from Gui if (_waitGui == 1) @@ -174,7 +174,7 @@ extern "C" int runServer(const char *_sdfString, // Path for compressed log, used to check for duplicates std::string cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(gz::common::separator(""))) + if (!std::string(1, cmpPath.back()).compare(common::separator(""))) { // Remove the separator at end of path cmpPath = cmpPath.substr(0, cmpPath.length() - 1); @@ -208,7 +208,7 @@ extern "C" int runServer(const char *_sdfString, // Update compressed file path to name of recording directory path cmpPath = std::string(recordPathMod); - if (!std::string(1, cmpPath.back()).compare(gz::common::separator( + if (!std::string(1, cmpPath.back()).compare(common::separator( ""))) { // Remove the separator at end of path @@ -217,8 +217,8 @@ extern "C" int runServer(const char *_sdfString, cmpPath += ".zip"; // Check if path or compressed file with same prefix exists - if (gz::common::exists(recordPathMod) || - gz::common::exists(cmpPath)) + if (common::exists(recordPathMod) || + common::exists(cmpPath)) { // Overwrite if flag specified if (_logOverwrite > 0) @@ -226,15 +226,15 @@ extern "C" int runServer(const char *_sdfString, bool recordMsg = false; bool cmpMsg = false; // Remove files before initializing console log files on top of them - if (gz::common::exists(recordPathMod)) + if (common::exists(recordPathMod)) { recordMsg = true; - gz::common::removeAll(recordPathMod); + common::removeAll(recordPathMod); } - if (gz::common::exists(cmpPath)) + if (common::exists(cmpPath)) { cmpMsg = true; - gz::common::removeFile(cmpPath); + common::removeFile(cmpPath); } // Create log file before printing any messages so they can be logged @@ -261,7 +261,7 @@ extern "C" int runServer(const char *_sdfString, { // Remove the separator at end of path if (!std::string(1, recordPathMod.back()).compare( - gz::common::separator(""))) + common::separator(""))) { recordPathMod = recordPathMod.substr(0, recordPathMod.length() - 1); @@ -272,8 +272,8 @@ extern "C" int runServer(const char *_sdfString, // Keep renaming until path does not exist for both directory and // compressed file - while (gz::common::exists(recordPathMod) || - gz::common::exists(cmpPath)) + while (common::exists(recordPathMod) || + common::exists(cmpPath)) { recordPathMod = recordOrigPrefix + "(" + std::to_string(count++) + ")"; @@ -281,7 +281,7 @@ extern "C" int runServer(const char *_sdfString, cmpPath = std::string(recordPathMod); // Remove the separator at end of path if (!std::string(1, cmpPath.back()).compare( - gz::common::separator(""))) + common::separator(""))) { cmpPath = cmpPath.substr(0, cmpPath.length() - 1); } @@ -313,7 +313,7 @@ extern "C" int runServer(const char *_sdfString, } serverConfig.SetLogRecordPath(recordPathMod); - std::vector topics = gz::common::split( + std::vector topics = common::split( _recordTopics, ":"); for (const std::string &topic : topics) { @@ -380,7 +380,7 @@ extern "C" int runServer(const char *_sdfString, else { gzmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(gz::common::absPath( + serverConfig.SetLogPlaybackPath(common::absPath( std::string(_playback))); } } @@ -403,7 +403,7 @@ extern "C" int runServer(const char *_sdfString, } // Create the Gazebo server - gz::sim::Server server(serverConfig); + sim::Server server(serverConfig); // Run the server server.Run(true, _iterations, _run == 0); diff --git a/src/gz.hh b/src/gz.hh index 7f54fed938..e2d7da672d 100644 --- a/src/gz.hh +++ b/src/gz.hh @@ -82,7 +82,7 @@ extern "C" GZ_SIM_VISIBLE int runGui(const char *_guiConfig, /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, -/// https://staging-fuel.ignitionrobotics.org/1.0/gmas/worlds/ShapesClone +/// https://staging-fuel.gazebosim.org/1.0/gmas/worlds/ShapesClone /// \return C-string containing the path to the local world sdf file extern "C" GZ_SIM_VISIBLE const char *findFuelResource( char *_pathToResource); diff --git a/src/network/PeerInfo.cc b/src/network/PeerInfo.cc index d234d72a6b..2751895ad8 100644 --- a/src/network/PeerInfo.cc +++ b/src/network/PeerInfo.cc @@ -24,8 +24,8 @@ using namespace sim; ///////////////////////////////////////////////// PeerInfo::PeerInfo(const NetworkRole &_role): - id(gz::common::Uuid().String()), - hostname(gz::transport::hostname()), + id(common::Uuid().String()), + hostname(transport::hostname()), role(_role) { } @@ -42,36 +42,36 @@ std::string PeerInfo::Namespace() const } ///////////////////////////////////////////////// -gz::sim::private_msgs::PeerInfo gz::sim::toProto( +private_msgs::PeerInfo sim::toProto( const PeerInfo &_info) { - gz::sim::private_msgs::PeerInfo proto; + private_msgs::PeerInfo proto; proto.set_id(_info.id); proto.set_hostname(_info.hostname); switch (_info.role) { case NetworkRole::ReadOnly: - proto.set_role(gz::sim::private_msgs::PeerInfo::READ_ONLY); + proto.set_role(private_msgs::PeerInfo::READ_ONLY); break; case NetworkRole::SimulationPrimary: proto.set_role( - gz::sim::private_msgs::PeerInfo::SIMULATION_PRIMARY); + private_msgs::PeerInfo::SIMULATION_PRIMARY); break; case NetworkRole::SimulationSecondary: proto.set_role( - gz::sim::private_msgs::PeerInfo::SIMULATION_SECONDARY); + private_msgs::PeerInfo::SIMULATION_SECONDARY); break; case NetworkRole::None: default: - proto.set_role(gz::sim::private_msgs::PeerInfo::NONE); + proto.set_role(private_msgs::PeerInfo::NONE); } return proto; } ///////////////////////////////////////////////// -PeerInfo gz::sim::fromProto( - const gz::sim::private_msgs::PeerInfo& _proto) +PeerInfo sim::fromProto( + const private_msgs::PeerInfo& _proto) { PeerInfo info; info.id = _proto.id(); @@ -79,16 +79,16 @@ PeerInfo gz::sim::fromProto( switch (_proto.role()) { - case gz::sim::private_msgs::PeerInfo::READ_ONLY: + case private_msgs::PeerInfo::READ_ONLY: info.role = NetworkRole::ReadOnly; break; - case gz::sim::private_msgs::PeerInfo::SIMULATION_PRIMARY: + case private_msgs::PeerInfo::SIMULATION_PRIMARY: info.role = NetworkRole::SimulationPrimary; break; - case gz::sim::private_msgs::PeerInfo::SIMULATION_SECONDARY: + case private_msgs::PeerInfo::SIMULATION_SECONDARY: info.role = NetworkRole::SimulationSecondary; break; - case gz::sim::private_msgs::PeerInfo::NONE: + case private_msgs::PeerInfo::NONE: default: info.role = NetworkRole::None; break; diff --git a/src/network/PeerTracker.cc b/src/network/PeerTracker.cc index 989f1acf80..3a60e82fc2 100644 --- a/src/network/PeerTracker.cc +++ b/src/network/PeerTracker.cc @@ -26,7 +26,7 @@ using namespace sim; PeerTracker::PeerTracker( PeerInfo _info, EventManager *_eventMgr, - const gz::transport::NodeOptions &_options): + const transport::NodeOptions &_options): info(std::move(_info)), eventMgr(_eventMgr), node(_options) diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 71de81707f..9f8e559875 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -50,50 +50,50 @@ class gz::sim::MarkerManagerPrivate /// \brief Processes a marker message. /// \param[in] _msg The message data. /// \return True if the marker was processed successfully. - public: bool ProcessMarkerMsg(const gz::msgs::Marker &_msg); + public: bool ProcessMarkerMsg(const msgs::Marker &_msg); /// \brief Converts a Gazebo msg render type to Gazebo Sim Rendering /// \param[in] _msg The message data /// \return Converted rendering type, if any. - public: gz::rendering::MarkerType MsgToType( - const gz::msgs::Marker &_msg); + public: rendering::MarkerType MsgToType( + const msgs::Marker &_msg); /// \brief Converts a Gazebo msg material to Gazebo Sim Rendering // material. // \param[in] _msg The message data. // \return Converted rendering material, if any. public: rendering::MaterialPtr MsgToMaterial( - const gz::msgs::Marker &_msg); + const msgs::Marker &_msg); /// \brief Updates the markers. public: void Update(); /// \brief Callback that receives marker messages. /// \param[in] _req The marker message. - public: void OnMarkerMsg(const gz::msgs::Marker &_req); + public: void OnMarkerMsg(const msgs::Marker &_req); /// \brief Callback that receives multiple marker messages. /// \param[in] _req The vector of marker messages /// \param[in] _res Response data /// \return True if the request is received - public: bool OnMarkerMsgArray(const gz::msgs::Marker_V &_req, - gz::msgs::Boolean &_res); + public: bool OnMarkerMsgArray(const msgs::Marker_V &_req, + msgs::Boolean &_res); /// \brief Services callback that returns a list of markers. /// \param[out] _rep Service reply /// \return True on success. - public: bool OnList(gz::msgs::Marker_V &_rep); + public: bool OnList(msgs::Marker_V &_rep); /// \brief Sets Marker from marker message. /// \param[in] _msg The message data. /// \param[out] _markerPtr The message pointer to set. - public: void SetMarker(const gz::msgs::Marker &_msg, + public: void SetMarker(const msgs::Marker &_msg, const rendering::MarkerPtr &_markerPtr); /// \brief Sets Visual from marker message. /// \param[in] _msg The message data. /// \param[out] _visualPtr The visual pointer to set. - public: void SetVisual(const gz::msgs::Marker &_msg, + public: void SetVisual(const msgs::Marker &_msg, const rendering::VisualPtr &_visualPtr); /// \brief Sets sim time from time. @@ -108,22 +108,22 @@ class gz::sim::MarkerManagerPrivate /// \brief Map of visuals public: std::map> visuals; + std::map> visuals; /// \brief List of marker message to process. - public: std::list markerMsgs; + public: std::list markerMsgs; /// \brief Pointer to the scene public: rendering::ScenePtr scene; /// \brief Gazebo node - public: gz::transport::Node node; + public: transport::Node node; /// \brief Sim time according to UpdateInfo in RenderUtil public: std::chrono::steady_clock::duration simTime; /// \brief The last marker message received - public: gz::msgs::Marker msg; + public: msgs::Marker msg; /// \brief Topic name for the marker service public: std::string topicName = "/marker"; @@ -157,7 +157,7 @@ void MarkerManager::Update() } ///////////////////////////////////////////////// -bool MarkerManager::Init(const gz::rendering::ScenePtr &_scene) +bool MarkerManager::Init(const rendering::ScenePtr &_scene) { if (!_scene) { @@ -237,8 +237,8 @@ void MarkerManagerPrivate::Update() if (it->second->GeometryCount() == 0u) continue; - gz::rendering::MarkerPtr markerPtr = - std::dynamic_pointer_cast + rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast (it->second->GeometryByIndex(0u)); if (markerPtr != nullptr) { @@ -271,7 +271,7 @@ void MarkerManagerPrivate::SetSimTime( } ///////////////////////////////////////////////// -void MarkerManagerPrivate::SetVisual(const gz::msgs::Marker &_msg, +void MarkerManagerPrivate::SetVisual(const msgs::Marker &_msg, const rendering::VisualPtr &_visualPtr) { // Set Visual Scale @@ -310,7 +310,7 @@ void MarkerManagerPrivate::SetVisual(const gz::msgs::Marker &_msg, } ///////////////////////////////////////////////// -void MarkerManagerPrivate::SetMarker(const gz::msgs::Marker &_msg, +void MarkerManagerPrivate::SetMarker(const msgs::Marker &_msg, const rendering::MarkerPtr &_markerPtr) { _markerPtr->SetLayer(_msg.layer()); @@ -327,7 +327,7 @@ void MarkerManagerPrivate::SetMarker(const gz::msgs::Marker &_msg, _markerPtr->SetLifetime(std::chrono::seconds(0)); } // Set Marker Render Type - gz::rendering::MarkerType markerType = MsgToType(_msg); + rendering::MarkerType markerType = MsgToType(_msg); _markerPtr->SetType(markerType); // Set Marker Material @@ -365,49 +365,49 @@ void MarkerManagerPrivate::SetMarker(const gz::msgs::Marker &_msg, } ///////////////////////////////////////////////// -gz::rendering::MarkerType MarkerManagerPrivate::MsgToType( - const gz::msgs::Marker &_msg) +rendering::MarkerType MarkerManagerPrivate::MsgToType( + const msgs::Marker &_msg) { - gz::msgs::Marker_Type marker = this->msg.type(); - if (marker != _msg.type() && _msg.type() != gz::msgs::Marker::NONE) + msgs::Marker_Type marker = this->msg.type(); + if (marker != _msg.type() && _msg.type() != msgs::Marker::NONE) { marker = _msg.type(); this->msg.set_type(_msg.type()); } switch (marker) { - case gz::msgs::Marker::BOX: - return gz::rendering::MarkerType::MT_BOX; - case gz::msgs::Marker::CAPSULE: - return gz::rendering::MarkerType::MT_CAPSULE; - case gz::msgs::Marker::CYLINDER: - return gz::rendering::MarkerType::MT_CYLINDER; - case gz::msgs::Marker::LINE_STRIP: - return gz::rendering::MarkerType::MT_LINE_STRIP; - case gz::msgs::Marker::LINE_LIST: - return gz::rendering::MarkerType::MT_LINE_LIST; - case gz::msgs::Marker::POINTS: - return gz::rendering::MarkerType::MT_POINTS; - case gz::msgs::Marker::SPHERE: - return gz::rendering::MarkerType::MT_SPHERE; - case gz::msgs::Marker::TEXT: - return gz::rendering::MarkerType::MT_TEXT; - case gz::msgs::Marker::TRIANGLE_FAN: - return gz::rendering::MarkerType::MT_TRIANGLE_FAN; - case gz::msgs::Marker::TRIANGLE_LIST: - return gz::rendering::MarkerType::MT_TRIANGLE_LIST; - case gz::msgs::Marker::TRIANGLE_STRIP: - return gz::rendering::MarkerType::MT_TRIANGLE_STRIP; + case msgs::Marker::BOX: + return rendering::MarkerType::MT_BOX; + case msgs::Marker::CAPSULE: + return rendering::MarkerType::MT_CAPSULE; + case msgs::Marker::CYLINDER: + return rendering::MarkerType::MT_CYLINDER; + case msgs::Marker::LINE_STRIP: + return rendering::MarkerType::MT_LINE_STRIP; + case msgs::Marker::LINE_LIST: + return rendering::MarkerType::MT_LINE_LIST; + case msgs::Marker::POINTS: + return rendering::MarkerType::MT_POINTS; + case msgs::Marker::SPHERE: + return rendering::MarkerType::MT_SPHERE; + case msgs::Marker::TEXT: + return rendering::MarkerType::MT_TEXT; + case msgs::Marker::TRIANGLE_FAN: + return rendering::MarkerType::MT_TRIANGLE_FAN; + case msgs::Marker::TRIANGLE_LIST: + return rendering::MarkerType::MT_TRIANGLE_LIST; + case msgs::Marker::TRIANGLE_STRIP: + return rendering::MarkerType::MT_TRIANGLE_STRIP; default: gzerr << "Unable to create marker of type[" << _msg.type() << "]\n"; break; } - return gz::rendering::MarkerType::MT_NONE; + return rendering::MarkerType::MT_NONE; } ///////////////////////////////////////////////// rendering::MaterialPtr MarkerManagerPrivate::MsgToMaterial( - const gz::msgs::Marker &_msg) + const msgs::Marker &_msg) { rendering::MaterialPtr material = this->scene->CreateMaterial(); @@ -441,7 +441,7 @@ rendering::MaterialPtr MarkerManagerPrivate::MsgToMaterial( } ////////////////////////////////////////////////// -bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) +bool MarkerManagerPrivate::ProcessMarkerMsg(const msgs::Marker &_msg) { // Get the namespace, if it exists. Otherwise, use the global namespace std::string ns; @@ -461,14 +461,14 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) // Otherwise generate unique id else { - id = gz::math::Rand::IntUniform(0, gz::math::MAX_I32); + id = math::Rand::IntUniform(0, math::MAX_I32); // Make sure it's unique if namespace is given if (nsIter != this->visuals.end()) { while (nsIter->second.find(id) != nsIter->second.end()) - id = gz::math::Rand::IntUniform(gz::math::MIN_UI32, - gz::math::MAX_UI32); + id = math::Rand::IntUniform(math::MIN_UI32, + math::MAX_UI32); } } @@ -478,7 +478,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) visualIter = nsIter->second.find(id); // Add/modify a marker - if (_msg.action() == gz::msgs::Marker::ADD_MODIFY) + if (_msg.action() == msgs::Marker::ADD_MODIFY) { // Modify an existing marker, identified by namespace and id if (nsIter != this->visuals.end() && @@ -488,8 +488,8 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) { // TODO(anyone): Update so that multiple markers can // be attached to one visual - gz::rendering::MarkerPtr markerPtr = - std::dynamic_pointer_cast + rendering::MarkerPtr markerPtr = + std::dynamic_pointer_cast (visualIter->second->GeometryByIndex(0)); visualIter->second->RemoveGeometryByIndex(0); @@ -536,7 +536,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) } } // Remove a single marker - else if (_msg.action() == gz::msgs::Marker::DELETE_MARKER) + else if (_msg.action() == msgs::Marker::DELETE_MARKER) { // Remove the marker if it can be found. if (nsIter != this->visuals.end() && @@ -557,7 +557,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) } } // Remove all markers, or all markers in a namespace - else if (_msg.action() == gz::msgs::Marker::DELETE_ALL) + else if (_msg.action() == msgs::Marker::DELETE_ALL) { // If given namespace doesn't exist if (!ns.empty() && nsIter == this->visuals.end()) @@ -601,7 +601,7 @@ bool MarkerManagerPrivate::ProcessMarkerMsg(const gz::msgs::Marker &_msg) ///////////////////////////////////////////////// -bool MarkerManagerPrivate::OnList(gz::msgs::Marker_V &_rep) +bool MarkerManagerPrivate::OnList(msgs::Marker_V &_rep) { std::lock_guard lock(this->mutex); _rep.clear_marker(); @@ -611,7 +611,7 @@ bool MarkerManagerPrivate::OnList(gz::msgs::Marker_V &_rep) { for (auto iter : mIter.second) { - gz::msgs::Marker *markerMsg = _rep.add_marker(); + msgs::Marker *markerMsg = _rep.add_marker(); markerMsg->set_ns(mIter.first); markerMsg->set_id(iter.first); } @@ -621,7 +621,7 @@ bool MarkerManagerPrivate::OnList(gz::msgs::Marker_V &_rep) } ///////////////////////////////////////////////// -void MarkerManagerPrivate::OnMarkerMsg(const gz::msgs::Marker &_req) +void MarkerManagerPrivate::OnMarkerMsg(const msgs::Marker &_req) { std::lock_guard lock(this->mutex); this->markerMsgs.push_back(_req); @@ -629,7 +629,7 @@ void MarkerManagerPrivate::OnMarkerMsg(const gz::msgs::Marker &_req) ///////////////////////////////////////////////// bool MarkerManagerPrivate::OnMarkerMsgArray( - const gz::msgs::Marker_V&_req, gz::msgs::Boolean &_res) + const msgs::Marker_V&_req, msgs::Boolean &_res) { std::lock_guard lock(this->mutex); std::copy(_req.marker().begin(), _req.marker().end(), diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index db257ef9f9..62327f4d63 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -230,7 +230,7 @@ class gz::sim::RenderUtilPrivate public: MarkerManager markerManager; /// \brief Pointer to rendering engine. - public: gz::rendering::RenderEngine *engine{nullptr}; + public: rendering::RenderEngine *engine{nullptr}; /// \brief rendering scene to be managed by the scene manager and used to /// generate sensor data diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index cdd9879a51..156e724c5e 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -710,8 +710,8 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, descriptor.subMeshName = _geom.MeshShape()->Submesh(); descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - gz::common::MeshManager *meshManager = - gz::common::MeshManager::Instance(); + common::MeshManager *meshManager = + common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); @@ -1925,6 +1925,28 @@ void SceneManager::RemoveEntity(Entity _id) if (!this->dataPtr->scene) return; + { + auto it = this->dataPtr->actors.find(_id); + if (it != this->dataPtr->actors.end()) + { + this->dataPtr->actors.erase(it); + } + } + { + auto it = this->dataPtr->actorTrajectories.find(_id); + if (it != this->dataPtr->actorTrajectories.end()) + { + this->dataPtr->actorTrajectories.erase(it); + } + } + { + auto it = this->dataPtr->actorSkeletons.find(_id); + if (it != this->dataPtr->actorSkeletons.end()) + { + this->dataPtr->actorSkeletons.erase(it); + } + } + { auto it = this->dataPtr->visuals.find(_id); if (it != this->dataPtr->visuals.end()) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 7519061db2..f8e3d0d856 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -58,21 +58,21 @@ class gz::sim::systems::AckermannSteeringPrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const gz::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Gazebo communication node. public: transport::Node node; @@ -165,10 +165,10 @@ class gz::sim::systems::AckermannSteeringPrivate public: std::chrono::steady_clock::duration lastOdomTime{0}; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -260,8 +260,8 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->wheelRadius).first; // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique(); - this->dataPtr->limiterAng = std::make_unique(); + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. if (_sdf->HasElement("min_velocity")) @@ -375,8 +375,8 @@ void AckermannSteering::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void AckermannSteering::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void AckermannSteering::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("AckermannSteering::PreUpdate"); @@ -587,8 +587,8 @@ void AckermannSteering::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void AckermannSteeringPrivate::UpdateOdometry( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("AckermannSteering::UpdateOdometry"); // Initialize, if not already initialized. @@ -690,7 +690,7 @@ void AckermannSteeringPrivate::UpdateOdometry( // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - auto *tfMsgPose = tfMsg.add_pose(); + msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); @@ -702,8 +702,8 @@ void AckermannSteeringPrivate::UpdateOdometry( ////////////////////////////////////////////////// void AckermannSteeringPrivate::UpdateVelocity( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("AckermannSteering::UpdateVelocity"); @@ -788,13 +788,13 @@ void AckermannSteeringPrivate::OnCmdVel(const msgs::Twist &_msg) } GZ_ADD_PLUGIN(AckermannSteering, - gz::sim::System, + System, AckermannSteering::ISystemConfigure, AckermannSteering::ISystemPreUpdate, AckermannSteering::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(AckermannSteering, - "gz::sim::systems::AckermannSteering") + "gz::sim::systems::AckermannSteering") // TODO(CH3): Deprecated, remove on version 8 GZ_ADD_PLUGIN_ALIAS(AckermannSteering, diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 9efe66f4ed..ae4f29d9cc 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -261,7 +261,6 @@ void AltimeterPrivate::UpdateAltimeters(const EntityComponentManager &_ecm) auto it = this->entitySensorMap.find(_entity); if (it != this->entitySensorMap.end()) { - math::Vector3d linearVel; math::Pose3d worldPose = _worldPose->Data(); it->second->SetPosition(worldPose.Pos().Z()); it->second->SetVerticalVelocity(_worldLinearVel->Data().Z()); diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index b92a4c3c47..7715a69f12 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -36,7 +36,7 @@ class gz::sim::systems::ApplyJointForcePrivate { /// \brief Callback for joint force subscription /// \param[in] _msg Joint force message - public: void OnCmdForce(const gz::msgs::Double &_msg); + public: void OnCmdForce(const msgs::Double &_msg); /// \brief Gazebo communication node. public: transport::Node node; @@ -112,8 +112,8 @@ void ApplyJointForce::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void ApplyJointForce::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void ApplyJointForce::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("ApplyJointForce::PreUpdate"); @@ -165,7 +165,7 @@ void ApplyJointForcePrivate::OnCmdForce(const msgs::Double &_msg) } GZ_ADD_PLUGIN(ApplyJointForce, - gz::sim::System, + System, ApplyJointForce::ISystemConfigure, ApplyJointForce::ISystemPreUpdate) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 341ff7d025..382ce05ff3 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -63,11 +63,11 @@ class gz::sim::systems::LinearBatteryPluginPrivate /// \brief Callback executed to start recharging. /// \param[in] _req This value should be true. - public: void OnEnableRecharge(const gz::msgs::Boolean &_req); + public: void OnEnableRecharge(const msgs::Boolean &_req); /// \brief Callback executed to stop recharging. /// \param[in] _req This value should be true. - public: void OnDisableRecharge(const gz::msgs::Boolean &_req); + public: void OnDisableRecharge(const msgs::Boolean &_req); /// \brief Callback connected to additional topics that can start battery /// draining. @@ -411,7 +411,7 @@ double LinearBatteryPluginPrivate::StateOfCharge() const ////////////////////////////////////////////////// void LinearBatteryPluginPrivate::OnEnableRecharge( - const gz::msgs::Boolean &/*_req*/) + const msgs::Boolean &/*_req*/) { gzdbg << "Request for start charging received" << std::endl; this->startCharging = true; @@ -419,7 +419,7 @@ void LinearBatteryPluginPrivate::OnEnableRecharge( ////////////////////////////////////////////////// void LinearBatteryPluginPrivate::OnDisableRecharge( - const gz::msgs::Boolean &/*_req*/) + const msgs::Boolean &/*_req*/) { gzdbg << "Request for stop charging received" << std::endl; this->startCharging = false; @@ -434,8 +434,8 @@ void LinearBatteryPluginPrivate::OnBatteryDrainingMsg( ////////////////////////////////////////////////// void LinearBatteryPlugin::PreUpdate( - const gz::sim::UpdateInfo &/*_info*/, - gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) { GZ_PROFILE("LinearBatteryPlugin::PreUpdate"); @@ -670,7 +670,7 @@ double LinearBatteryPlugin::OnUpdateVoltage( } GZ_ADD_PLUGIN(LinearBatteryPlugin, - gz::sim::System, + System, LinearBatteryPlugin::ISystemConfigure, LinearBatteryPlugin::ISystemPreUpdate, LinearBatteryPlugin::ISystemUpdate, diff --git a/src/systems/breadcrumbs/Breadcrumbs.cc b/src/systems/breadcrumbs/Breadcrumbs.cc index 155c945470..84b5bcbbc4 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.cc +++ b/src/systems/breadcrumbs/Breadcrumbs.cc @@ -172,8 +172,8 @@ void Breadcrumbs::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void Breadcrumbs::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void Breadcrumbs::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("Breadcrumbs::PreUpdate"); @@ -434,7 +434,7 @@ void Breadcrumbs::OnDeploy(const msgs::Empty &) } GZ_ADD_PLUGIN(Breadcrumbs, - gz::sim::System, + System, Breadcrumbs::ISystemConfigure, Breadcrumbs::ISystemPreUpdate) diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index a018c1bf36..a2c21b9c5a 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -520,8 +520,8 @@ void Buoyancy::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void Buoyancy::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void Buoyancy::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("Buoyancy::PreUpdate"); this->dataPtr->CheckForNewEntities(_ecm); @@ -647,7 +647,7 @@ bool Buoyancy::IsEnabled(Entity _entity, } GZ_ADD_PLUGIN(Buoyancy, - gz::sim::System, + System, Buoyancy::ISystemConfigure, Buoyancy::ISystemPreUpdate, Buoyancy::ISystemPostUpdate) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 5bf1b91b81..e5a6e2008e 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -70,7 +70,7 @@ class gz::sim::systems::CameraVideoRecorderPrivate public: std::mutex updateMutex; /// \brief Connection to the post-render event. - public: gz::common::ConnectionPtr postRenderConn; + public: common::ConnectionPtr postRenderConn; /// \brief Pointer to the event manager public: EventManager *eventMgr = nullptr; @@ -355,7 +355,7 @@ void CameraVideoRecorderPrivate::OnPostRender() std::chrono::steady_clock::duration dt; dt = t - this->recordStartTime; int64_t sec, nsec; - std::tie(sec, nsec) = gz::math::durationToSecNsec(dt); + std::tie(sec, nsec) = math::durationToSecNsec(dt); msgs::Time msg; msg.set_sec(sec); msg.set_nsec(nsec); @@ -460,7 +460,7 @@ void CameraVideoRecorder::PostUpdate(const UpdateInfo &_info, } GZ_ADD_PLUGIN(CameraVideoRecorder, - gz::sim::System, + System, CameraVideoRecorder::ISystemConfigure, CameraVideoRecorder::ISystemPostUpdate) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index 9b40fff2d5..f19369cd13 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -51,7 +51,7 @@ namespace systems /// /// Video recorder bitrate (bps). The default value is /// 2070000 bps, and the supported type is unsigned int. - class CameraVideoRecorder: + class CameraVideoRecorder final: public System, public ISystemConfigure, public ISystemPostUpdate diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.cc b/src/systems/collada_world_exporter/ColladaWorldExporter.cc index b0dfc6d1ad..672b74e869 100644 --- a/src/systems/collada_world_exporter/ColladaWorldExporter.cc +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.cc @@ -85,7 +85,7 @@ class gz::sim::systems::ColladaWorldExporterPrivate components::Name, components::Geometry, components::Transparency>( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Visual *, const components::Name *_name, const components::Geometry *_geom, @@ -107,12 +107,12 @@ class gz::sim::systems::ColladaWorldExporterPrivate } mat->SetTransparency(_transparency->Data()); - const gz::common::Mesh *mesh; - std::weak_ptr subm; + const common::Mesh *mesh; + std::weak_ptr subm; math::Vector3d scale; math::Matrix4d matrix(worldPose); - gz::common::MeshManager *meshManager = - gz::common::MeshManager::Instance(); + common::MeshManager *meshManager = + common::MeshManager::Instance(); auto addSubmeshFunc = [&](int _matIndex) { diff --git a/src/systems/contact/Contact.cc b/src/systems/contact/Contact.cc index b0e0b116e1..b7f224d1ce 100644 --- a/src/systems/contact/Contact.cc +++ b/src/systems/contact/Contact.cc @@ -123,7 +123,7 @@ void ContactSensor::Load(const sdf::ElementPtr &_sdf, const std::string &_topic, } gzmsg << "Contact system publishing on " << this->topic << std::endl; - this->pub = this->node.Advertise(this->topic); + this->pub = this->node.Advertise(this->topic); } ////////////////////////////////////////////////// diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 8d39f1eca9..d78d777c44 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -114,8 +114,8 @@ void DetachableJoint::Configure(const Entity &_entity, ////////////////////////////////////////////////// void DetachableJoint::PreUpdate( - const gz::sim::UpdateInfo &/*_info*/, - gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) { GZ_PROFILE("DetachableJoint::PreUpdate"); if (this->validConfig && !this->initialized) @@ -190,7 +190,7 @@ void DetachableJoint::OnDetachRequest(const msgs::Empty &) } GZ_ADD_PLUGIN(DetachableJoint, - gz::sim::System, + System, DetachableJoint::ISystemConfigure, DetachableJoint::ISystemPreUpdate) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index a5eef5cbbe..cd72571dc6 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -59,25 +59,25 @@ class gz::sim::systems::DiffDrivePrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const gz::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for enable/disable subscription /// \param[in] _msg Boolean message - public: void OnEnable(const gz::msgs::Boolean &_msg); + public: void OnEnable(const msgs::Boolean &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Gazebo communication node. public: transport::Node node; @@ -128,10 +128,10 @@ class gz::sim::systems::DiffDrivePrivate public: transport::Node::Publisher tfPub; /// \brief Linear velocity limiter. - public: std::unique_ptr limiterLin; + public: std::unique_ptr limiterLin; /// \brief Angular velocity limiter. - public: std::unique_ptr limiterAng; + public: std::unique_ptr limiterAng; /// \brief Previous control command. public: Commands last0Cmd; @@ -206,8 +206,8 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius).first; // Instantiate the speed limiters. - this->dataPtr->limiterLin = std::make_unique(); - this->dataPtr->limiterAng = std::make_unique(); + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. @@ -386,8 +386,8 @@ void DiffDrive::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void DiffDrive::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void DiffDrive::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("DiffDrive::PreUpdate"); @@ -524,8 +524,8 @@ void DiffDrive::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateOdometry(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) +void DiffDrivePrivate::UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("DiffDrive::UpdateOdometry"); // Initialize, if not already initialized. @@ -608,7 +608,7 @@ void DiffDrivePrivate::UpdateOdometry(const gz::sim::UpdateInfo &_info, // Construct the Pose_V/tf message and publish it. msgs::Pose_V tfMsg; - gz::msgs::Pose *tfMsgPose = tfMsg.add_pose(); + msgs::Pose *tfMsgPose = tfMsg.add_pose(); tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); @@ -619,8 +619,8 @@ void DiffDrivePrivate::UpdateOdometry(const gz::sim::UpdateInfo &_info, } ////////////////////////////////////////////////// -void DiffDrivePrivate::UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &/*_ecm*/) +void DiffDrivePrivate::UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &/*_ecm*/) { GZ_PROFILE("DiffDrive::UpdateVelocity"); @@ -674,7 +674,7 @@ void DiffDrivePrivate::OnEnable(const msgs::Boolean &_msg) } GZ_ADD_PLUGIN(DiffDrive, - gz::sim::System, + System, DiffDrive::ISystemConfigure, DiffDrive::ISystemPreUpdate, DiffDrive::ISystemPostUpdate) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index dd8577f24d..ac0f699796 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -53,7 +53,7 @@ class gz::sim::systems::ImuPrivate { /// \brief A map of IMU entity to its IMU sensor. public: std::unordered_map> entitySensorMap; + std::unique_ptr> entitySensorMap; /// \brief gz-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 54a50a6d09..ce2d8640f6 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -39,7 +39,7 @@ class gz::sim::systems::JointControllerPrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const gz::msgs::Double &_msg); + public: void OnCmdVel(const msgs::Double &_msg); /// \brief Gazebo communication node. public: transport::Node node; @@ -61,7 +61,7 @@ class gz::sim::systems::JointControllerPrivate public: bool useForceCommands{false}; /// \brief Velocity PID controller. - public: gz::math::PID velPid; + public: math::PID velPid; }; ////////////////////////////////////////////////// @@ -173,8 +173,8 @@ void JointController::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void JointController::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void JointController::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("JointController::PreUpdate"); @@ -260,7 +260,7 @@ void JointControllerPrivate::OnCmdVel(const msgs::Double &_msg) } GZ_ADD_PLUGIN(JointController, - gz::sim::System, + System, JointController::ISystemConfigure, JointController::ISystemPreUpdate) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index c51f32ddb0..be75d7e9eb 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -40,7 +40,7 @@ class gz::sim::systems::JointPositionControllerPrivate { /// \brief Callback for position subscription /// \param[in] _msg Position message - public: void OnCmdPos(const gz::msgs::Double &_msg); + public: void OnCmdPos(const msgs::Double &_msg); /// \brief Gazebo communication node. public: transport::Node node; @@ -61,7 +61,7 @@ class gz::sim::systems::JointPositionControllerPrivate public: Model model{kNullEntity}; /// \brief Position PID controller. - public: gz::math::PID posPid; + public: math::PID posPid; /// \brief Joint index to be used. public: unsigned int jointIndex = 0u; @@ -218,8 +218,8 @@ void JointPositionController::Configure(const Entity &_entity, ////////////////////////////////////////////////// void JointPositionController::PreUpdate( - const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("JointPositionController::PreUpdate"); @@ -357,7 +357,7 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) } GZ_ADD_PLUGIN(JointPositionController, - gz::sim::System, + System, JointPositionController::ISystemConfigure, JointPositionController::ISystemPreUpdate) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index ca37844bcf..de11de8c2c 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -69,7 +69,7 @@ void JointStatePublisher::Configure( while (elem) { std::string jointName = elem->Get(); - sim::Entity jointEntity = this->model.JointByName(_ecm, jointName); + Entity jointEntity = this->model.JointByName(_ecm, jointName); if (jointEntity != kNullEntity) { this->CreateComponents(_ecm, jointEntity); @@ -105,7 +105,7 @@ void JointStatePublisher::Configure( ////////////////////////////////////////////////// void JointStatePublisher::CreateComponents(EntityComponentManager &_ecm, - sim::Entity _joint) + Entity _joint) { if (this->joints.find(_joint) != this->joints.end()) { @@ -313,7 +313,7 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, } GZ_ADD_PLUGIN(JointStatePublisher, - gz::sim::System, + System, JointStatePublisher::ISystemConfigure, JointStatePublisher::ISystemPostUpdate) diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index e318cbcc0c..f465e04789 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -110,12 +110,12 @@ class gz::sim::systems::LiftDragPrivate /// \brief Normally, this is taken as a direction parallel to the chord /// of the airfoil in zero angle of attack forward flight. - public: gz::math::Vector3d forward = math::Vector3d::UnitX; + public: math::Vector3d forward = math::Vector3d::UnitX; /// \brief A vector in the lift/drag plane, perpendicular to the forward /// vector. Inflow velocity orthogonal to forward and upward vectors /// is considered flow in the wing sweep direction. - public: gz::math::Vector3d upward = math::Vector3d::UnitZ; + public: math::Vector3d upward = math::Vector3d::UnitZ; /// \brief how much to change CL per radian of control surface joint /// value. @@ -154,15 +154,15 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, this->radialSymmetry).first; this->area = _sdf->Get("area", this->area).first; this->alpha0 = _sdf->Get("a0", this->alpha0).first; - this->cp = _sdf->Get("cp", this->cp).first; + this->cp = _sdf->Get("cp", this->cp).first; // blade forward (-drag) direction in link frame this->forward = - _sdf->Get("forward", this->forward).first; + _sdf->Get("forward", this->forward).first; this->forward.Normalize(); // blade upward (+lift) direction in link frame - this->upward = _sdf->Get( + this->upward = _sdf->Get( "upward", this->upward) .first; this->upward.Normalize(); @@ -281,12 +281,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // rotate forward and upward vectors into world frame const auto forwardI = pose.Rot().RotateVector(this->forward); - gz::math::Vector3d upwardI; + math::Vector3d upwardI; if (this->radialSymmetry) { // use inflow velocity to determine upward direction // which is the component of inflow perpendicular to forward direction. - gz::math::Vector3d tmp = forwardI.Cross(velI); + math::Vector3d tmp = forwardI.Cross(velI); upwardI = forwardI.Cross(tmp).Normalize(); } else @@ -300,7 +300,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const double minRatio = -1.0; const double maxRatio = 1.0; // check sweep (angle between velI and lift-drag-plane) - double sinSweepAngle = gz::math::clamp( + double sinSweepAngle = math::clamp( spanwiseI.Dot(velI), minRatio, maxRatio); // get cos from trig identity @@ -340,7 +340,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // given upwardI and liftI are both unit vectors, we can drop the denominator // cos(theta) = a.Dot(b) const double cosAlpha = - gz::math::clamp(liftI.Dot(upwardI), minRatio, maxRatio); + math::clamp(liftI.Dot(upwardI), minRatio, maxRatio); // Is alpha positive or negative? Test: // forwardI points toward zero alpha @@ -389,7 +389,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) } // compute lift force at cp - gz::math::Vector3d lift = cl * q * this->area * liftI; + math::Vector3d lift = cl * q * this->area * liftI; // compute cd at cp, check for stall, correct for sweep double cd; @@ -412,7 +412,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) cd = std::fabs(cd); // drag at cp - gz::math::Vector3d drag = cd * q * this->area * dragDirection; + math::Vector3d drag = cd * q * this->area * dragDirection; // compute cm at cp, check for stall, correct for sweep double cm; @@ -441,12 +441,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // compute moment (torque) at cp // spanwiseI used to be momentDirection - gz::math::Vector3d moment = cm * q * this->area * spanwiseI; + math::Vector3d moment = cm * q * this->area * spanwiseI; // force and torque about cg in world frame - gz::math::Vector3d force = lift + drag; - gz::math::Vector3d torque = moment; + math::Vector3d force = lift + drag; + math::Vector3d torque = moment; // Correct for nan or inf force.Correct(); this->cp.Correct(); @@ -558,7 +558,7 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } GZ_ADD_PLUGIN(LiftDrag, - gz::sim::System, + System, LiftDrag::ISystemConfigure, LiftDrag::ISystemPreUpdate) diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 3c498a65bb..a1afaab247 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -628,11 +628,11 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) } } -GZ_ADD_PLUGIN(gz::sim::systems::LogPlayback, - gz::sim::System, - LogPlayback::ISystemConfigure, - LogPlayback::ISystemReset, - LogPlayback::ISystemUpdate) +GZ_ADD_PLUGIN(LogPlayback, + System, + LogPlayback::ISystemConfigure, + LogPlayback::ISystemReset, + LogPlayback::ISystemUpdate) GZ_ADD_PLUGIN_ALIAS(LogPlayback, "gz::sim::systems::LogPlayback") diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 7ea230f9d7..940d5bc2f2 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -370,7 +370,7 @@ bool LogRecordPrivate::Start(const std::string &_logPath, // This calls Log::Open() and loads sql schema if (this->recorder.Start(dbPath) == - gz::transport::log::RecorderError::SUCCESS) + transport::log::RecorderError::SUCCESS) { this->instStarted = true; return true; @@ -735,11 +735,11 @@ void LogRecord::PostUpdate(const UpdateInfo &_info, this->dataPtr->LogModelResources(_ecm); } -GZ_ADD_PLUGIN(gz::sim::systems::LogRecord, - gz::sim::System, - LogRecord::ISystemConfigure, - LogRecord::ISystemPreUpdate, - LogRecord::ISystemPostUpdate) +GZ_ADD_PLUGIN(LogRecord, + System, + LogRecord::ISystemConfigure, + LogRecord::ISystemPreUpdate, + LogRecord::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(LogRecord, "gz::sim::systems::LogRecord") diff --git a/src/systems/log_video_recorder/LogVideoRecorder.cc b/src/systems/log_video_recorder/LogVideoRecorder.cc index ff5c95644a..71c68c7999 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.cc +++ b/src/systems/log_video_recorder/LogVideoRecorder.cc @@ -372,8 +372,8 @@ void LogVideoRecorder::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Rewind() { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error sending rewind request" << std::endl; @@ -399,14 +399,14 @@ void LogVideoRecorderPrivate::Play() ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Follow(const std::string &_entity) { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error sending follow request" << std::endl; }; - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_entity); if (this->node.Request(this->followService, req, cb)) { @@ -417,14 +417,14 @@ void LogVideoRecorderPrivate::Follow(const std::string &_entity) ////////////////////////////////////////////////// void LogVideoRecorderPrivate::Record(bool _record) { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error sending record request" << std::endl; }; - gz::msgs::VideoRecord req; + msgs::VideoRecord req; if (_record) { @@ -445,7 +445,7 @@ void LogVideoRecorderPrivate::Record(bool _record) } GZ_ADD_PLUGIN(LogVideoRecorder, - gz::sim::System, + System, LogVideoRecorder::ISystemConfigure, LogVideoRecorder::ISystemPostUpdate) diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index a1905ca56b..dd14a2d271 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -80,7 +80,7 @@ class gz::sim::systems::LogicalAudioSensorPluginPrivate const logical_audio::SourcePlayInfo &_sourcePlayInfo); /// \brief Node used to create publishers and services - public: gz::transport::Node node; + public: transport::Node node; /// \brief A flag used to initialize a source's playing information /// before starting simulation. @@ -99,7 +99,7 @@ class gz::sim::systems::LogicalAudioSensorPluginPrivate /// (an entity can have multiple microphones attached to it). /// The value is the microphone's detection publisher. public: std::unordered_map micEntities; + transport::Node::Publisher> micEntities; /// \brief A mutex used to ensure that the play source service call does /// not interfere with the source's state in the PreUpdate step. @@ -242,7 +242,7 @@ void LogicalAudioSensorPlugin::PostUpdate(const UpdateInfo &_info, // publish the source that the microphone heard, along with the // volume level the microphone detected. The detected source's // ID is embedded in the message's header - gz::msgs::Double msg; + msgs::Double msg; auto header = msg.mutable_header(); auto timeStamp = header->mutable_stamp(); timeStamp->set_sec(simSeconds.count()); @@ -286,7 +286,7 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( } _ids.insert(id); - gz::math::Pose3d pose; + math::Pose3d pose; if (!_elem->HasElement("pose")) { gzwarn << "Audio source is missing a pose. " @@ -295,7 +295,7 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( } else { - pose = _elem->Get("pose"); + pose = _elem->Get("pose"); } if (!_elem->HasElement("attenuation_function")) @@ -387,16 +387,16 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( components::LogicalAudioSourcePlayInfo(playInfo)); // create service callbacks that allow this source to be played/stopped - std::function playSrvCb = - [this, entity](gz::msgs::Boolean &_resp) + std::function playSrvCb = + [this, entity](msgs::Boolean &_resp) { std::lock_guard lock(this->playSourceMutex); this->sourceEntities[entity].first = true; _resp.set_data(true); return true; }; - std::function stopSrvCb = - [this, entity](gz::msgs::Boolean &_resp) + std::function stopSrvCb = + [this, entity](msgs::Boolean &_resp) { std::lock_guard lock(this->stopSourceMutex); this->sourceEntities[entity].second = true; @@ -456,7 +456,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( } _ids.insert(id); - gz::math::Pose3d pose; + math::Pose3d pose; if (!_elem->HasElement("pose")) { gzwarn << "Microphone is missing a pose. " @@ -465,7 +465,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( } else { - pose = _elem->Get("pose"); + pose = _elem->Get("pose"); } double volumeDetectionThreshold; @@ -503,7 +503,7 @@ void LogicalAudioSensorPluginPrivate::CreateMicrophone( components::Pose(pose)); // create the detection publisher for this microphone - auto pub = this->node.Advertise( + auto pub = this->node.Advertise( scopedName(entity, _ecm) + "/detection"); if (!pub) { @@ -528,7 +528,7 @@ bool LogicalAudioSensorPluginPrivate::DurationExceeded( } GZ_ADD_PLUGIN(LogicalAudioSensorPlugin, - gz::sim::System, + System, LogicalAudioSensorPlugin::ISystemConfigure, LogicalAudioSensorPlugin::ISystemPreUpdate, LogicalAudioSensorPlugin::ISystemPostUpdate) diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index abc4ad9c6a..2d349f7c07 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -52,7 +52,7 @@ class gz::sim::systems::MagnetometerPrivate { /// \brief A map of magnetometer entity to its sensor. public: std::unordered_map> entitySensorMap; + std::unique_ptr> entitySensorMap; /// \brief gz-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index 8880760e29..6f2f0338ad 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -335,8 +335,8 @@ math::Inertiald MulticopterVelocityControl::VehicleInertial( ////////////////////////////////////////////////// void MulticopterVelocityControl::PreUpdate( - const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("MulticopterVelocityControl::PreUpdate"); @@ -427,7 +427,7 @@ void MulticopterVelocityControl::OnEnable( ////////////////////////////////////////////////// void MulticopterVelocityControl::PublishRotorVelocities( - gz::sim::EntityComponentManager &_ecm, + EntityComponentManager &_ecm, const Eigen::VectorXd &_vels) { if (_vels.size() != this->rotorVelocitiesMsg.velocity_size()) @@ -464,7 +464,7 @@ void MulticopterVelocityControl::PublishRotorVelocities( } GZ_ADD_PLUGIN(MulticopterVelocityControl, - gz::sim::System, + System, MulticopterVelocityControl::ISystemConfigure, MulticopterVelocityControl::ISystemPreUpdate) diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index fec06f802d..f581f308c8 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -122,7 +122,7 @@ enum class MotorType { class gz::sim::systems::MulticopterMotorModelPrivate { /// \brief Callback for actuator commands. - public: void OnActuatorMsg(const gz::msgs::Actuators &_msg); + public: void OnActuatorMsg(const msgs::Actuators &_msg); /// \brief Apply link forces and moments based on propeller state. public: void UpdateForcesAndMoments(EntityComponentManager &_ecm); @@ -379,8 +379,8 @@ void MulticopterMotorModel::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void MulticopterMotorModel::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void MulticopterMotorModel::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("MulticopterMotorModel::PreUpdate"); @@ -478,7 +478,7 @@ void MulticopterMotorModel::PreUpdate(const gz::sim::UpdateInfo &_info, ////////////////////////////////////////////////// void MulticopterMotorModelPrivate::OnActuatorMsg( - const gz::msgs::Actuators &_msg) + const msgs::Actuators &_msg) { std::lock_guard lock(this->recvdActuatorsMsgMutex); this->recvdActuatorsMsg = _msg; @@ -568,8 +568,8 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( realMotorVelocity * realMotorVelocity * this->motorConstant; - using Pose = gz::math::Pose3d; - using Vector3 = gz::math::Vector3d; + using Pose = math::Pose3d; + using Vector3 = math::Vector3d; Link link(this->linkEntity); const auto worldPose = link.WorldPose(_ecm); @@ -681,7 +681,7 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( } GZ_ADD_PLUGIN(MulticopterMotorModel, - gz::sim::System, + System, MulticopterMotorModel::ISystemConfigure, MulticopterMotorModel::ISystemPreUpdate) diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc index f985e12ae4..23741b6428 100644 --- a/src/systems/performer_detector/PerformerDetector.cc +++ b/src/systems/performer_detector/PerformerDetector.cc @@ -130,8 +130,8 @@ void PerformerDetector::Configure(const Entity &_entity, ////////////////////////////////////////////////// void PerformerDetector::PostUpdate( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("PerformerDetector::PostUpdate"); @@ -260,7 +260,7 @@ void PerformerDetector::Publish( } GZ_ADD_PLUGIN(PerformerDetector, - gz::sim::System, + System, PerformerDetector::ISystemConfigure, PerformerDetector::ISystemPostUpdate) diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index 2a1b163bf1..948d83851a 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -64,7 +64,7 @@ class EntityFeatureMapFixture: public InternalFixture<::testing::Test> << "Failed to find plugin [" << pluginLib << "]"; // Load engine plugin - gz::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); ASSERT_FALSE(plugins.empty()) << "Unable to load the [" << pathToLib << "] library."; @@ -82,7 +82,7 @@ class EntityFeatureMapFixture: public InternalFixture<::testing::Test> << pathToLib << "]"; this->engine = - gz::physics::RequestEngine::From(plugin); ASSERT_NE(nullptr, this->engine); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0c5b0c6028..1a83e6a906 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -161,38 +161,38 @@ class gz::sim::systems::PhysicsPrivate /// implement to be supported by this system. /// New features can't be added to this list in minor / patch releases, in /// order to maintain backwards compatibility with downstream physics plugins. - public: struct MinimumFeatureList : gz::physics::FeatureList< - gz::physics::FindFreeGroupFeature, - gz::physics::SetFreeGroupWorldPose, - gz::physics::FreeGroupFrameSemantics, - gz::physics::LinkFrameSemantics, - gz::physics::ForwardStep, - gz::physics::RemoveModelFromWorld, - gz::physics::sdf::ConstructSdfModel, - gz::physics::sdf::ConstructSdfWorld, - gz::physics::GetLinkFromModel, - gz::physics::GetShapeFromLink + public: struct MinimumFeatureList : physics::FeatureList< + physics::FindFreeGroupFeature, + physics::SetFreeGroupWorldPose, + physics::FreeGroupFrameSemantics, + physics::LinkFrameSemantics, + physics::ForwardStep, + physics::RemoveModelFromWorld, + physics::sdf::ConstructSdfModel, + physics::sdf::ConstructSdfWorld, + physics::GetLinkFromModel, + physics::GetShapeFromLink >{}; /// \brief Engine type with just the minimum features. - public: using EnginePtrType = gz::physics::EnginePtr< - gz::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using EnginePtrType = physics::EnginePtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief World type with just the minimum features. - public: using WorldPtrType = gz::physics::WorldPtr< - gz::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using WorldPtrType = physics::WorldPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Model type with just the minimum features. - public: using ModelPtrType = gz::physics::ModelPtr< - gz::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using ModelPtrType = physics::ModelPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Link type with just the minimum features. - public: using LinkPtrType = gz::physics::LinkPtr< - gz::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using LinkPtrType = physics::LinkPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Free group type with just the minimum features. - public: using FreeGroupPtrType = gz::physics::FreeGroupPtr< - gz::physics::FeaturePolicy3d, MinimumFeatureList>; + public: using FreeGroupPtrType = physics::FreeGroupPtr< + physics::FeaturePolicy3d, MinimumFeatureList>; /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. @@ -319,7 +319,7 @@ class gz::sim::systems::PhysicsPrivate /// \param[in] _from An ancestor of the _to entity. /// \param[in] _to A descendant of the _from entity. /// \return Pose transform between the two entities - public: gz::math::Pose3d RelativePose(const Entity &_from, + public: math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; /// \brief Enable contact surface customization for the given world. @@ -458,18 +458,18 @@ class gz::sim::systems::PhysicsPrivate public: struct FrictionPyramidSlipComplianceFeatureList : physics::FeatureList< MinimumFeatureList, - gz::physics::GetShapeFrictionPyramidSlipCompliance, - gz::physics::SetShapeFrictionPyramidSlipCompliance>{}; + physics::GetShapeFrictionPyramidSlipCompliance, + physics::SetShapeFrictionPyramidSlipCompliance>{}; ////////////////////////////////////////////////// // Joints /// \brief Feature list to handle joints. - public: struct JointFeatureList : gz::physics::FeatureList< + public: struct JointFeatureList : physics::FeatureList< MinimumFeatureList, - gz::physics::GetJointFromModel, - gz::physics::GetBasicJointProperties, - gz::physics::GetBasicJointState, - gz::physics::SetBasicJointState>{}; + physics::GetJointFromModel, + physics::GetBasicJointProperties, + physics::GetBasicJointState, + physics::SetBasicJointState>{}; /// \brief Feature list to construct joints public: struct ConstructSdfJointFeatureList : gz::physics::FeatureList< @@ -496,50 +496,50 @@ class gz::sim::systems::PhysicsPrivate // Collisions /// \brief Feature list to handle collisions. - public: struct CollisionFeatureList : gz::physics::FeatureList< + public: struct CollisionFeatureList : physics::FeatureList< MinimumFeatureList, - gz::physics::sdf::ConstructSdfCollision>{}; + physics::sdf::ConstructSdfCollision>{}; /// \brief Feature list to handle contacts information. - public: struct ContactFeatureList : gz::physics::FeatureList< + public: struct ContactFeatureList : physics::FeatureList< CollisionFeatureList, - gz::physics::GetContactsFromLastStepFeature>{}; + physics::GetContactsFromLastStepFeature>{}; /// \brief Feature list to change contacts before they are applied to physics. public: struct SetContactPropertiesCallbackFeatureList : - gz::physics::FeatureList< + physics::FeatureList< ContactFeatureList, - gz::physics::SetContactPropertiesCallbackFeature>{}; + physics::SetContactPropertiesCallbackFeature>{}; /// \brief Collision type with collision features. - public: using ShapePtrType = gz::physics::ShapePtr< - gz::physics::FeaturePolicy3d, CollisionFeatureList>; + public: using ShapePtrType = physics::ShapePtr< + physics::FeaturePolicy3d, CollisionFeatureList>; /// \brief World type with just the minimum features. Non-pointer. - public: using WorldShapeType = gz::physics::World< - gz::physics::FeaturePolicy3d, ContactFeatureList>; + public: using WorldShapeType = physics::World< + physics::FeaturePolicy3d, ContactFeatureList>; ////////////////////////////////////////////////// // Collision filtering with bitmasks /// \brief Feature list to filter collisions with bitmasks. - public: struct CollisionMaskFeatureList : gz::physics::FeatureList< + public: struct CollisionMaskFeatureList : physics::FeatureList< CollisionFeatureList, - gz::physics::CollisionFilterMaskFeature>{}; + physics::CollisionFilterMaskFeature>{}; ////////////////////////////////////////////////// // Link force /// \brief Feature list for applying forces to links. - public: struct LinkForceFeatureList : gz::physics::FeatureList< - gz::physics::AddLinkExternalForceTorque>{}; + public: struct LinkForceFeatureList : physics::FeatureList< + physics::AddLinkExternalForceTorque>{}; ////////////////////////////////////////////////// // Bounding box /// \brief Feature list for model bounding box. - public: struct BoundingBoxFeatureList : gz::physics::FeatureList< + public: struct BoundingBoxFeatureList : physics::FeatureList< MinimumFeatureList, - gz::physics::GetModelBoundingBox>{}; + physics::GetModelBoundingBox>{}; ////////////////////////////////////////////////// @@ -573,8 +573,8 @@ class gz::sim::systems::PhysicsPrivate ////////////////////////////////////////////////// // World velocity command public: struct WorldVelocityCommandFeatureList : - gz::physics::FeatureList< - gz::physics::SetFreeGroupWorldVelocity>{}; + physics::FeatureList< + physics::SetFreeGroupWorldVelocity>{}; ////////////////////////////////////////////////// @@ -618,9 +618,9 @@ class gz::sim::systems::PhysicsPrivate // Nested Models /// \brief Feature list to construct nested models - public: struct NestedModelFeatureList : gz::physics::FeatureList< + public: struct NestedModelFeatureList : physics::FeatureList< MinimumFeatureList, - gz::physics::sdf::ConstructSdfNestedModel>{}; + physics::sdf::ConstructSdfNestedModel>{}; ////////////////////////////////////////////////// /// \brief World EntityFeatureMap @@ -830,7 +830,7 @@ void Physics::Configure(const Entity &_entity, } // Load engine plugin - gz::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); if (plugins.empty()) { @@ -860,8 +860,8 @@ void Physics::Configure(const Entity &_entity, continue; } - this->dataPtr->engine = gz::physics::RequestEngine< - gz::physics::FeaturePolicy3d, + this->dataPtr->engine = physics::RequestEngine< + physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::From(plugin); if (nullptr != this->dataPtr->engine) @@ -871,8 +871,8 @@ void Physics::Configure(const Entity &_entity, break; } - auto missingFeatures = gz::physics::RequestEngine< - gz::physics::FeaturePolicy3d, + auto missingFeatures = physics::RequestEngine< + physics::FeaturePolicy3d, PhysicsPrivate::MinimumFeatureList>::MissingFeatureNames(plugin); std::stringstream msg; @@ -1388,7 +1388,7 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, return true; } - auto &meshManager = *gz::common::MeshManager::Instance(); + auto &meshManager = *common::MeshManager::Instance(); auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); auto *mesh = meshManager.Load(fullPath); if (nullptr == mesh) @@ -2767,9 +2767,9 @@ gz::physics::ForwardStep::Output PhysicsPrivate::Step( const std::chrono::steady_clock::duration &_dt) { GZ_PROFILE("PhysicsPrivate::Step"); - gz::physics::ForwardStep::Input input; - gz::physics::ForwardStep::State state; - gz::physics::ForwardStep::Output output; + physics::ForwardStep::Input input; + physics::ForwardStep::State state; + physics::ForwardStep::Output output; input.Get() = _dt; @@ -2782,7 +2782,7 @@ gz::physics::ForwardStep::Output PhysicsPrivate::Step( } ////////////////////////////////////////////////// -gz::math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, +math::Pose3d PhysicsPrivate::RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const { math::Pose3d transform; @@ -3343,7 +3343,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - gz::math::Vector3d entityWorldAngularVel = + math::Vector3d entityWorldAngularVel = math::eigen3::convert(entityFrameData.angularVelocity); auto entityBodyAngularVel = @@ -3368,7 +3368,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); - gz::math::Vector3d entityWorldLinearAcc = + math::Vector3d entityWorldLinearAcc = math::eigen3::convert(entityFrameData.linearAcceleration); auto entityBodyLinearAcc = @@ -3798,7 +3798,7 @@ void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) } GZ_ADD_PLUGIN(Physics, - gz::sim::System, + System, Physics::ISystemConfigure, Physics::ISystemReset, Physics::ISystemUpdate) diff --git a/src/systems/pose_publisher/PosePublisher.cc b/src/systems/pose_publisher/PosePublisher.cc index 120a17fbd7..e75d5789a3 100644 --- a/src/systems/pose_publisher/PosePublisher.cc +++ b/src/systems/pose_publisher/PosePublisher.cc @@ -156,12 +156,12 @@ class gz::sim::systems::PosePublisherPrivate /// \brief A variable that gets populated with poses. This also here as a /// member variable to avoid repeated memory allocations and improve /// performance. - public: gz::msgs::Pose poseMsg; + public: msgs::Pose poseMsg; /// \brief A variable that gets populated with poses. This also here as a /// member variable to avoid repeated memory allocations and improve /// performance. - public: gz::msgs::Pose_V poseVMsg; + public: msgs::Pose_V poseVMsg; /// \brief True to publish a vector of poses. False to publish individual pose /// msgs. @@ -263,23 +263,23 @@ void PosePublisher::Configure(const Entity &_entity, if (this->dataPtr->usePoseV) { this->dataPtr->posePub = - this->dataPtr->node.Advertise(poseTopic); + this->dataPtr->node.Advertise(poseTopic); if (this->dataPtr->staticPosePublisher) { this->dataPtr->poseStaticPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( staticPoseTopic); } } else { this->dataPtr->posePub = - this->dataPtr->node.Advertise(poseTopic); + this->dataPtr->node.Advertise(poseTopic); if (this->dataPtr->staticPosePublisher) { this->dataPtr->poseStaticPub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( staticPoseTopic); } } @@ -530,7 +530,7 @@ void PosePublisherPrivate::PublishPoses( GZ_PROFILE("PosePublisher::PublishPoses"); // publish poses - gz::msgs::Pose *msg = nullptr; + msgs::Pose *msg = nullptr; if (this->usePoseV) this->poseVMsg.Clear(); diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 7beae440c8..ed61109e97 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -128,6 +128,9 @@ struct RadioState /// \brief Accumulation of bytes received in an epoch. uint64_t bytesReceivedThisEpoch = 0; + + /// \brief Name of the model associated with the radio. + std::string name; }; /// \brief Type for holding RF power as a Normally distributed random variable. @@ -234,7 +237,7 @@ RFPower RFComms::Implementation::LogNormalReceivedPower( const double kPL = this->rangeConfig.l0 + 10 * this->rangeConfig.fadingExponent * log10(kRange); - return {_txPower - kPL, this->rangeConfig.sigma}; + return {_txPower - kPL, pow(this->rangeConfig.sigma, 2.)}; } ///////////////////////////////////////////// @@ -245,7 +248,7 @@ std::tuple RFComms::Implementation::AttemptSend( // Maintain running window of bytes sent over the last epoch, e.g., 1s. while (!_txState.bytesSent.empty() && - _txState.bytesSent.front().first < now - this->epochDuration) + _txState.bytesSent.front().first <= now - this->epochDuration) { _txState.bytesSentThisEpoch -= _txState.bytesSent.front().second; _txState.bytesSent.pop_front(); @@ -263,8 +266,10 @@ std::tuple RFComms::Implementation::AttemptSend( // Check current epoch bitrate vs capacity and fail to send accordingly if (bitsSent > this->radioConfig.capacity * this->epochDuration) { - gzwarn << "Bitrate limited: " << bitsSent << "bits sent (limit: " - << this->radioConfig.capacity * this->epochDuration << std::endl; + gzwarn << "Bitrate limited: [" << _txState.name << "] " << bitsSent + << " bits sent (limit: " + << this->radioConfig.capacity * this->epochDuration << ")" + << std::endl; return std::make_tuple(false, std::numeric_limits::lowest()); } @@ -305,7 +310,7 @@ std::tuple RFComms::Implementation::AttemptSend( // Maintain running window of bytes received over the last epoch, e.g., 1s. while (!_rxState.bytesReceived.empty() && - _rxState.bytesReceived.front().first < now - this->epochDuration) + _rxState.bytesReceived.front().first <= now - this->epochDuration) { _rxState.bytesReceivedThisEpoch -= _rxState.bytesReceived.front().second; _rxState.bytesReceived.pop_front(); @@ -323,9 +328,10 @@ std::tuple RFComms::Implementation::AttemptSend( // Check current epoch bitrate vs capacity and fail to send accordingly. if (bitsReceived > this->radioConfig.capacity * this->epochDuration) { - // gzwarn << "Bitrate limited: " << bitsReceived - // << "bits received (limit: " - // << this->radioConfig.capacity * this->epochDuration << std::endl; + gzwarn << "Bitrate limited: [" << _rxState.name << "] " << bitsReceived + << " bits received (limit: " + << this->radioConfig.capacity * this->epochDuration << ")" + << std::endl; return std::make_tuple(false, std::numeric_limits::lowest()); } @@ -424,6 +430,7 @@ void RFComms::Step( this->dataPtr->radioStates[address].pose = kPose; this->dataPtr->radioStates[address].timeStamp = std::chrono::duration(_info.simTime).count(); + this->dataPtr->radioStates[address].name = content.modelName; } } @@ -445,11 +452,7 @@ void RFComms::Step( continue; auto [sendPacket, rssi] = this->dataPtr->AttemptSend( -#if GOOGLE_PROTOBUF_VERSION < 3004001 - itSrc->second, itDst->second, msg->ByteSize()); -#else - itSrc->second, itDst->second, msg->ByteSizeLong()); -#endif + itSrc->second, itDst->second, msg->data().size()); if (sendPacket) { diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 68b309d7a2..b174b7364c 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -89,21 +89,21 @@ class gz::sim::systems::SceneBroadcasterPrivate /// \brief Callback for scene info service. /// \param[out] _res Response containing the latest scene message. /// \return True if successful. - public: bool SceneInfoService(gz::msgs::Scene &_res); + public: bool SceneInfoService(msgs::Scene &_res); /// \brief Callback for scene graph service. /// \param[out] _res Response containing the the scene graph in DOT format. /// \return True if successful. - public: bool SceneGraphService(gz::msgs::StringMsg &_res); + public: bool SceneGraphService(msgs::StringMsg &_res); /// \brief Callback for state service. /// \param[out] _res Response containing the latest full state. /// \return True if successful. - public: bool StateService(gz::msgs::SerializedStepMap &_res); + public: bool StateService(msgs::SerializedStepMap &_res); /// \brief Callback for state service - non blocking. /// \param[out] _res Response containing the last available full state. - public: void StateAsyncService(const gz::msgs::StringMsg &_req); + public: void StateAsyncService(const msgs::StringMsg &_req); /// \brief Updates the scene graph when entities are added /// \param[in] _manager The entity component manager @@ -628,7 +628,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) // Scene info topic std::string sceneTopic{ns + "/scene/info"}; - this->scenePub = this->node->Advertise(sceneTopic); + this->scenePub = this->node->Advertise(sceneTopic); gzmsg << "Publishing scene information on [" << sceneTopic << "]" << std::endl; @@ -637,7 +637,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) std::string deletionTopic{ns + "/scene/deletion"}; this->deletionPub = - this->node->Advertise(deletionTopic); + this->node->Advertise(deletionTopic); gzmsg << "Publishing entity deletions on [" << deletionTopic << "]" << std::endl; @@ -646,7 +646,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) std::string stateTopic{ns + "/state"}; this->statePub = - this->node->Advertise(stateTopic); + this->node->Advertise(stateTopic); gzmsg << "Publishing state changes on [" << stateTopic << "]" << std::endl; @@ -675,7 +675,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) } ////////////////////////////////////////////////// -bool SceneBroadcasterPrivate::SceneInfoService(gz::msgs::Scene &_res) +bool SceneBroadcasterPrivate::SceneInfoService(msgs::Scene &_res) { std::lock_guard lock(this->graphMutex); @@ -695,7 +695,7 @@ bool SceneBroadcasterPrivate::SceneInfoService(gz::msgs::Scene &_res) ////////////////////////////////////////////////// void SceneBroadcasterPrivate::StateAsyncService( - const gz::msgs::StringMsg &_req) + const msgs::StringMsg &_req) { std::unique_lock lock(this->stateMutex); this->stateServiceRequest = true; @@ -704,7 +704,7 @@ void SceneBroadcasterPrivate::StateAsyncService( ////////////////////////////////////////////////// bool SceneBroadcasterPrivate::StateService( - gz::msgs::SerializedStepMap &_res) + msgs::SerializedStepMap &_res) { _res.Clear(); @@ -726,7 +726,7 @@ bool SceneBroadcasterPrivate::StateService( } ////////////////////////////////////////////////// -bool SceneBroadcasterPrivate::SceneGraphService(gz::msgs::StringMsg &_res) +bool SceneBroadcasterPrivate::SceneGraphService(msgs::StringMsg &_res) { std::lock_guard lock(this->graphMutex); @@ -996,22 +996,22 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( msgs::IMUSensor * imuMsg = sensorMsg->mutable_imu(); const auto * imu = imuComp->Data().ImuSensor(); - gz::sim::set( + set( imuMsg->mutable_linear_acceleration()->mutable_x_noise(), imu->LinearAccelerationXNoise()); - gz::sim::set( + set( imuMsg->mutable_linear_acceleration()->mutable_y_noise(), imu->LinearAccelerationYNoise()); - gz::sim::set( + set( imuMsg->mutable_linear_acceleration()->mutable_z_noise(), imu->LinearAccelerationZNoise()); - gz::sim::set( + set( imuMsg->mutable_angular_velocity()->mutable_x_noise(), imu->AngularVelocityXNoise()); - gz::sim::set( + set( imuMsg->mutable_angular_velocity()->mutable_y_noise(), imu->AngularVelocityYNoise()); - gz::sim::set( + set( imuMsg->mutable_angular_velocity()->mutable_z_noise(), imu->AngularVelocityZNoise()); } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 945d3b4ec2..23e7abec16 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -132,10 +132,10 @@ class gz::sim::systems::SensorsPrivate public: std::condition_variable renderCv; /// \brief Connection to events::Stop event, used to stop thread - public: gz::common::ConnectionPtr stopConn; + public: common::ConnectionPtr stopConn; /// \brief Connection to events::ForceRender event, used to force rendering - public: gz::common::ConnectionPtr forceRenderConn; + public: common::ConnectionPtr forceRenderConn; /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; diff --git a/src/systems/thermal/Thermal.cc b/src/systems/thermal/Thermal.cc index ee06b15b17..da4711b673 100644 --- a/src/systems/thermal/Thermal.cc +++ b/src/systems/thermal/Thermal.cc @@ -52,8 +52,8 @@ Thermal::~Thermal() = default; ////////////////////////////////////////////////// void Thermal::Configure(const Entity &_entity, const std::shared_ptr &_sdf, - sim::EntityComponentManager &_ecm, - sim::EventManager & /*_eventMgr*/) + EntityComponentManager &_ecm, + EventManager & /*_eventMgr*/) { const std::string temperatureTag = "temperature"; const std::string heatSignatureTag = "heat_signature"; diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index 041c3cec32..85090e560e 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -409,7 +409,7 @@ void TouchPlugin::PostUpdate(const UpdateInfo &_info, } GZ_ADD_PLUGIN(TouchPlugin, - gz::sim::System, + System, TouchPlugin::ISystemConfigure, TouchPlugin::ISystemPreUpdate, TouchPlugin::ISystemPostUpdate) diff --git a/src/systems/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc index 0fdcc8fc9a..4ca4041ddf 100644 --- a/src/systems/track_controller/TrackController.cc +++ b/src/systems/track_controller/TrackController.cc @@ -91,8 +91,8 @@ class gz::sim::systems::TrackControllerPrivate /// \param[in] _frictionDirection First friction direction (in world coords). /// \return The computed contact surface speed. public: double ComputeSurfaceMotion( - double _beltSpeed, const gz::math::Vector3d &_beltDirection, - const gz::math::Vector3d &_frictionDirection); + double _beltSpeed, const math::Vector3d &_beltDirection, + const math::Vector3d &_frictionDirection); /// \brief Compute the first friction direction of the contact surface. /// \param[in] _centerOfRotation The point around which the track circles ( @@ -100,11 +100,11 @@ class gz::sim::systems::TrackControllerPrivate /// \param[in] _contactWorldPosition Position of the contact point. /// \param[in] _contactNormal Normal of the contact surface (in world coords). /// \param[in] _beltDirection Direction of the belt (in world coords). - public: gz::math::Vector3d ComputeFrictionDirection( - const gz::math::Vector3d &_centerOfRotation, - const gz::math::Vector3d &_contactWorldPosition, - const gz::math::Vector3d &_contactNormal, - const gz::math::Vector3d &_beltDirection); + public: math::Vector3d ComputeFrictionDirection( + const math::Vector3d &_centerOfRotation, + const math::Vector3d &_contactWorldPosition, + const math::Vector3d &_contactNormal, + const math::Vector3d &_beltDirection); /// \brief Name of the link to which the track is attached. public: std::string linkName; @@ -342,24 +342,24 @@ void TrackController::Configure(const Entity &_entity, if (this->dataPtr->debug) { this->dataPtr->debugMarker.set_ns(this->dataPtr->linkName + "/friction"); - this->dataPtr->debugMarker.set_action(gz::msgs::Marker::ADD_MODIFY); - this->dataPtr->debugMarker.set_type(gz::msgs::Marker::BOX); - this->dataPtr->debugMarker.set_visibility(gz::msgs::Marker::GUI); + this->dataPtr->debugMarker.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(msgs::Marker::BOX); + this->dataPtr->debugMarker.set_visibility(msgs::Marker::GUI); this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); // Set material properties - gz::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), - gz::math::Color(0, 0, 1, 1)); - gz::msgs::Set( + math::Color(0, 0, 1, 1)); + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), - gz::math::Color(0, 0, 1, 1)); + math::Color(0, 0, 1, 1)); // Set marker scale - gz::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_scale(), - gz::math::Vector3d(0.3, 0.03, 0.03)); + math::Vector3d(0.3, 0.03, 0.03)); } } @@ -530,7 +530,7 @@ void TrackControllerPrivate::ComputeSurfaceProperties( p += rot.RotateVector( math::Vector3d::UnitX * this->debugMarker.scale().x() / 2); - gz::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( p.X(), p.Y(), p.Z(), rot.Roll(), rot.Pitch(), rot.Yaw())); this->debugMarker.mutable_material()->mutable_diffuse()->set_r( surfaceMotion >= 0 ? 0.0f : 1.0f); @@ -541,8 +541,8 @@ void TrackControllerPrivate::ComputeSurfaceProperties( ////////////////////////////////////////////////// double TrackControllerPrivate::ComputeSurfaceMotion( - const double _beltSpeed, const gz::math::Vector3d &_beltDirection, - const gz::math::Vector3d &_frictionDirection) + const double _beltSpeed, const math::Vector3d &_beltDirection, + const math::Vector3d &_frictionDirection) { // the dot product is the cosine of the angle they // form (because both are unit vectors) @@ -553,11 +553,11 @@ double TrackControllerPrivate::ComputeSurfaceMotion( } ////////////////////////////////////////////////// -gz::math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( - const gz::math::Vector3d &_centerOfRotation, - const gz::math::Vector3d &_contactWorldPosition, - const gz::math::Vector3d &_contactNormal, - const gz::math::Vector3d &_beltDirection) +math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( + const math::Vector3d &_centerOfRotation, + const math::Vector3d &_contactWorldPosition, + const math::Vector3d &_contactNormal, + const math::Vector3d &_beltDirection) { if (_centerOfRotation.IsFinite()) { @@ -614,7 +614,7 @@ void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) } GZ_ADD_PLUGIN(TrackController, - gz::sim::System, + System, TrackController::ISystemConfigure, TrackController::ISystemPreUpdate) diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc index 1c952e6b17..094b7ebab1 100644 --- a/src/systems/tracked_vehicle/TrackedVehicle.cc +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -60,25 +60,25 @@ class gz::sim::systems::TrackedVehiclePrivate { /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const gz::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for steering efficiency subscription /// \param[in] _msg Steering efficiency message - public: void OnSteeringEfficiency(const gz::msgs::Double &_msg); + public: void OnSteeringEfficiency(const msgs::Double &_msg); /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateOdometry(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateOdometry(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Gazebo communication node. public: transport::Node node; @@ -423,31 +423,31 @@ void TrackedVehicle::Configure(const Entity &_entity, { this->dataPtr->debugMarker.set_ns( this->dataPtr->model.Name(_ecm) + "/cor"); - this->dataPtr->debugMarker.set_action(gz::msgs::Marker::ADD_MODIFY); - this->dataPtr->debugMarker.set_type(gz::msgs::Marker::SPHERE); - this->dataPtr->debugMarker.set_visibility(gz::msgs::Marker::GUI); + this->dataPtr->debugMarker.set_action(msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(msgs::Marker::SPHERE); + this->dataPtr->debugMarker.set_visibility(msgs::Marker::GUI); this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); this->dataPtr->debugMarker.set_id(1); // Set material properties - gz::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), - gz::math::Color(0, 0, 1, 1)); - gz::msgs::Set( + math::Color(0, 0, 1, 1)); + msgs::Set( this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), - gz::math::Color(0, 0, 1, 1)); + math::Color(0, 0, 1, 1)); // Set marker scale - gz::msgs::Set( + msgs::Set( this->dataPtr->debugMarker.mutable_scale(), - gz::math::Vector3d(0.1, 0.1, 0.1)); + math::Vector3d(0.1, 0.1, 0.1)); } } ////////////////////////////////////////////////// -void TrackedVehicle::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void TrackedVehicle::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("TrackedVehicle::PreUpdate"); @@ -546,8 +546,8 @@ void TrackedVehicle::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void TrackedVehiclePrivate::UpdateOdometry( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("TrackedVehicle::UpdateOdometry"); // Initialize, if not already initialized. @@ -629,8 +629,8 @@ void TrackedVehiclePrivate::UpdateOdometry( ////////////////////////////////////////////////// void TrackedVehiclePrivate::UpdateVelocity( - const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { GZ_PROFILE("TrackedVehicle::UpdateVelocity"); @@ -702,7 +702,7 @@ void TrackedVehiclePrivate::UpdateVelocity( const auto bodyPose = worldPose(this->bodyLink, _ecm); const auto bodyYAxisGlobal = - bodyPose.Rot().RotateVector(gz::math::Vector3d(0, 1, 0)); + bodyPose.Rot().RotateVector(math::Vector3d(0, 1, 0)); // centerOfRotation may be +inf this->centerOfRotation = (bodyYAxisGlobal * desiredRotationRadiusSigned) + bodyPose.Pos(); @@ -742,7 +742,7 @@ void TrackedVehiclePrivate::UpdateVelocity( << ", right v=" << this->rightSpeed << (sendCommandsToTracks ? " (sent to tracks)" : "") << std::endl; - gz::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( this->centerOfRotation.X(), this->centerOfRotation.Y(), this->centerOfRotation.Z(), @@ -761,7 +761,7 @@ void TrackedVehiclePrivate::OnCmdVel(const msgs::Twist &_msg) ////////////////////////////////////////////////// void TrackedVehiclePrivate::OnSteeringEfficiency( - const gz::msgs::Double& _msg) + const msgs::Double& _msg) { std::lock_guard lock(this->mutex); this->steeringEfficiency = _msg.data(); @@ -769,7 +769,7 @@ void TrackedVehiclePrivate::OnSteeringEfficiency( } GZ_ADD_PLUGIN(TrackedVehicle, - gz::sim::System, + System, TrackedVehicle::ISystemConfigure, TrackedVehicle::ISystemPreUpdate, TrackedVehicle::ISystemPostUpdate) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 7f38ec03df..54b3756d4b 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -844,7 +844,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) } GZ_ADD_PLUGIN(TriggeredPublisher, - gz::sim::System, + System, TriggeredPublisher::ISystemConfigure, TriggeredPublisher::ISystemPreUpdate) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 45de3a7335..c50f0a57ce 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -44,26 +44,26 @@ class gz::sim::systems::VelocityControlPrivate { /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const gz::msgs::Twist &_msg); + public: void OnCmdVel(const msgs::Twist &_msg); /// \brief Callback for link velocity subscription /// \param[in] _msg Velocity message - public: void OnLinkCmdVel(const gz::msgs::Twist &_msg, - const gz::transport::MessageInfo &_info); + public: void OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Update link velocity. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateLinkVelocity(const gz::sim::UpdateInfo &_info, - const gz::sim::EntityComponentManager &_ecm); + public: void UpdateLinkVelocity(const UpdateInfo &_info, + const EntityComponentManager &_ecm); /// \brief Gazebo communication node. public: transport::Node node; @@ -183,8 +183,8 @@ void VelocityControl::Configure(const Entity &_entity, } ////////////////////////////////////////////////// -void VelocityControl::PreUpdate(const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) +void VelocityControl::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) { GZ_PROFILE("VelocityControl::PreUpdate"); @@ -327,8 +327,8 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateVelocity( - const gz::sim::UpdateInfo &/*_info*/, - const gz::sim::EntityComponentManager &/*_ecm*/) + const UpdateInfo &/*_info*/, + const EntityComponentManager &/*_ecm*/) { GZ_PROFILE("VeocityControl::UpdateVelocity"); @@ -339,8 +339,8 @@ void VelocityControlPrivate::UpdateVelocity( ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateLinkVelocity( - const gz::sim::UpdateInfo &/*_info*/, - const gz::sim::EntityComponentManager &/*_ecm*/) + const UpdateInfo &/*_info*/, + const EntityComponentManager &/*_ecm*/) { GZ_PROFILE("VelocityControl::UpdateLinkVelocity"); @@ -378,7 +378,7 @@ void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, } GZ_ADD_PLUGIN(VelocityControl, - gz::sim::System, + System, VelocityControl::ISystemConfigure, VelocityControl::ISystemPreUpdate, VelocityControl::ISystemPostUpdate) diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index fcd3ef8562..b78451e0f0 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -386,7 +386,7 @@ void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } GZ_ADD_PLUGIN(WheelSlip, - gz::sim::System, + System, WheelSlip::ISystemConfigure, WheelSlip::ISystemPreUpdate) diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index c3fb243da9..92a5f522fd 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -549,7 +549,7 @@ void WindEffectsPrivate::UpdateWindVelocity(const UpdateInfo &_info, direction = this->noiseDirection->Apply(direction); // Apply wind velocity - gz::math::Vector3d windVel; + math::Vector3d windVel; windVel.X(magnitude * std::cos(GZ_DTOR(direction))); windVel.Y(magnitude * std::sin(GZ_DTOR(direction))); diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index 886cc43c3d..75e6488527 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -64,8 +64,8 @@ class AckermannSteeringTest test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -125,8 +125,8 @@ class AckermannSteeringTest const double desiredLinVel = 10.5; const double desiredAngVel = 0.1; velocityRamp.OnPreUpdate( - [&](const sim::UpdateInfo &/*_info*/, - const sim::EntityComponentManager &) + [&](const UpdateInfo &/*_info*/, + const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVel, 0, 0)); @@ -232,8 +232,8 @@ TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -330,8 +330,8 @@ TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/air_pressure_system.cc b/test/integration/air_pressure_system.cc index 3337eb3af0..dc1c4afbce 100644 --- a/test/integration/air_pressure_system.cc +++ b/test/integration/air_pressure_system.cc @@ -64,11 +64,11 @@ TEST_F(AirPressureTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirPressure)) // Create a system that checks sensor topic test::Relay testSystem; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::AirPressureSensor *, const components::Name *_name) -> bool { diff --git a/test/integration/altimeter_system.cc b/test/integration/altimeter_system.cc index 0b94217e1f..deb0e815eb 100644 --- a/test/integration/altimeter_system.cc +++ b/test/integration/altimeter_system.cc @@ -83,13 +83,13 @@ TEST_F(AltimeterTest, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(ModelFalling)) test::Relay testSystem; std::vector poses; std::vector velocities; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Altimeter *, const components::Name *_name, const components::WorldPose *_worldPose, diff --git a/test/integration/apply_joint_force_system.cc b/test/integration/apply_joint_force_system.cc index 7539b9fc99..824213686d 100644 --- a/test/integration/apply_joint_force_system.cc +++ b/test/integration/apply_joint_force_system.cc @@ -72,7 +72,7 @@ TEST_F(ApplyJointForceTestFixture, test::Relay testSystem; std::vector jointForceCmd; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 798c62c4f0..2d54f47e1d 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -60,15 +60,15 @@ class BatteryPluginTest : public InternalFixture<::testing::Test> EXPECT_TRUE(plugin.has_value()); this->systemPtr = plugin.value(); - this->mockSystem = static_cast( - systemPtr->QueryInterface()); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); EXPECT_NE(nullptr, this->mockSystem); } - public: gz::sim::SystemPluginPtr systemPtr; - public: sim::MockSystem *mockSystem; + public: SystemPluginPtr systemPtr; + public: MockSystem *mockSystem; - private: sim::SystemLoader sm; + private: SystemLoader sm; }; @@ -87,9 +87,9 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBattery)) serverConfig.SetSdfFile(sdfPath); // A pointer to the ecm. This will be valid once we run the mock system - sim::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; this->mockSystem->preUpdateCallback = - [&ecm](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&ecm](const UpdateInfo &, EntityComponentManager &_ecm) { ecm = &_ecm; diff --git a/test/integration/breadcrumbs.cc b/test/integration/breadcrumbs.cc index c846289377..d7b2827a54 100644 --- a/test/integration/breadcrumbs.cc +++ b/test/integration/breadcrumbs.cc @@ -147,8 +147,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(DeployAtOffset)) node.Advertise("/model/vehicle_blue/breadcrumbs/B1/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // After 1000 iterations, stop the vehicle, spawn a breadcrumb @@ -212,8 +212,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(MaxDeployments)) node.Advertise("/model/vehicle_blue/breadcrumbs/B1/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // Every 1000 iterations, deploy @@ -270,8 +270,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(FuelDeploy)) const std::size_t nIters = iterTestStart + 2500; const std::size_t maxDeployments = 5; std::size_t deployCount = 0; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // Every 500 iterations, deploy @@ -324,8 +324,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Performer)) const std::size_t nIters = iterTestStart + 10000; std::optional initialPose; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Deploy a performer breadcrumb on a tile that's on a level, and ensure // that it keeps the tile from being unloaded. @@ -396,8 +396,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PerformerSetVolume)) const std::size_t iterTestStart = 1000; const std::size_t nIters = iterTestStart + 2000; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Deploy a performer breadcrumb on a tile that's on the default a level, // and check that it causes tile_1 to be loaded since the performer's volume @@ -449,8 +449,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(DeployDisablePhysics)) node.Advertise("/model/vehicle_blue/breadcrumbs/B2/deploy"); std::size_t iterTestStart = 1000; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Start moving the vehicle // After 1000 iterations, stop the vehicle, spawn a breadcrumb @@ -550,7 +550,7 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AllowRenaming)) ///////////////////////////////////////////////// /// Return a list of model entities whose names match the given regex std::vector ModelsByNameRegex( - const sim::EntityComponentManager &_ecm, const std::regex &_re) + const EntityComponentManager &_ecm, const std::regex &_re) { std::vector entities; _ecm.Each( @@ -586,8 +586,8 @@ TEST_F(BreadcrumbsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelLoadUnload)) std::regex reTile1{"tile_1"}; std::regex reBreadcrumb{"B1_.*"}; testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Ensure that tile_1 is loaded at the start, deploy a breadcrumb if (_info.iterations == iterTestStart) diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 1cca53bdcc..b004b1bf20 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -181,8 +181,8 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement)) bool finished = false; test::Relay testSystem; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Check pose Entity submarine = _ecm.EntityByComponents( @@ -217,7 +217,7 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement)) auto submarineCenterOfVolume = _ecm.Component(submarineLink); ASSERT_NE(submarineCenterOfVolume, nullptr); - EXPECT_EQ(gz::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineCenterOfVolume->Data()); // Get the submarine buoyant link @@ -235,7 +235,7 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement)) auto submarineBuoyantCenterOfVolume = _ecm.Component(submarineBuoyantLink); ASSERT_NE(submarineBuoyantCenterOfVolume, nullptr); - EXPECT_EQ(gz::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineBuoyantCenterOfVolume->Data()); // Get the submarine sinking link @@ -253,7 +253,7 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement)) auto submarineSinkingCenterOfVolume = _ecm.Component(submarineSinkingLink); ASSERT_NE(submarineSinkingCenterOfVolume, nullptr); - EXPECT_EQ(gz::math::Vector3d(0, 0, 0), + EXPECT_EQ(math::Vector3d(0, 0, 0), submarineSinkingCenterOfVolume->Data()); // Get the duck link @@ -269,7 +269,7 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(UniformWorldMovement)) auto duckCenterOfVolume = _ecm.Component(duckLink); ASSERT_NE(duckCenterOfVolume, nullptr); - EXPECT_EQ(gz::math::Vector3d(0, 0, -0.4), + EXPECT_EQ(math::Vector3d(0, 0, -0.4), duckCenterOfVolume->Data()); auto submarinePose = _ecm.Component(submarine); @@ -464,8 +464,8 @@ TEST_F(BuoyancyTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(OffsetAndRotation)) std::size_t iterations{0}; fixture.OnPostUpdate([&]( - const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + const UpdateInfo &, + const EntityComponentManager &_ecm) { // Get links auto noOffsets = entitiesFromScopedName("no_offset::link", _ecm); diff --git a/test/integration/camera_video_record_system.cc b/test/integration/camera_video_record_system.cc index 6889e1bb45..97380db6d7 100644 --- a/test/integration/camera_video_record_system.cc +++ b/test/integration/camera_video_record_system.cc @@ -55,7 +55,7 @@ TEST_F(CameraVideoRecorderTest, GZ_UTILS_TEST_DISABLED_ON_MAC(RecordVideo)) // Run server server.Run(true, 1, false); - gz::transport::Node node; + transport::Node node; std::vector services; bool hasService = false; @@ -79,8 +79,8 @@ TEST_F(CameraVideoRecorderTest, GZ_UTILS_TEST_DISABLED_ON_MAC(RecordVideo)) } EXPECT_TRUE(hasService); - gz::msgs::VideoRecord videoRecordMsg; - gz::msgs::Boolean res; + msgs::VideoRecord videoRecordMsg; + msgs::Boolean res; bool result = false; unsigned int timeout = 5000; diff --git a/test/integration/components.cc b/test/integration/components.cc index a66f2c4de1..475993b092 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -120,7 +120,7 @@ TEST_F(ComponentsTest, Actor) comp3.Deserialize(istr); EXPECT_EQ("abc", comp3.Data().Name()); EXPECT_EQ("def", comp3.Data().SkinFilename()); - EXPECT_EQ(gz::math::Pose3d(3, 2, 1, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(3, 2, 1, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -183,7 +183,7 @@ TEST_F(ComponentsTest, AirPressureSensor) sdf::Sensor data1; data1.SetName("abc"); data1.SetType(sdf::SensorType::AIR_PRESSURE); - data1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::AirPressure airPressure1; data1.SetAirPressureSensor(airPressure1); @@ -211,7 +211,7 @@ TEST_F(ComponentsTest, AirPressureSensor) comp3.Deserialize(istr); EXPECT_EQ("abc", comp3.Data().Name()); EXPECT_EQ(sdf::SensorType::AIR_PRESSURE, comp3.Data().Type()); - EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -474,7 +474,7 @@ TEST_F(ComponentsTest, Imu) data1.SetType(sdf::SensorType::IMU); data1.SetUpdateRate(100); data1.SetTopic("imu_data"); - data1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Imu imu1; data1.SetImuSensor(imu1); @@ -505,7 +505,7 @@ TEST_F(ComponentsTest, Imu) EXPECT_EQ(sdf::SensorType::IMU, comp3.Data().Type()); EXPECT_EQ("imu_data", comp3.Data().Topic()); EXPECT_DOUBLE_EQ(100, comp3.Data().UpdateRate()); - EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// @@ -1100,7 +1100,7 @@ TEST_F(ComponentsTest, Magnetometer) data1.SetType(sdf::SensorType::MAGNETOMETER); data1.SetUpdateRate(12.4); data1.SetTopic("grape"); - data1.SetRawPose(gz::math::Pose3d(1, 2, 3, 0, 0, 0)); + data1.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); sdf::Magnetometer mag1; data1.SetMagnetometerSensor(mag1); @@ -1130,7 +1130,7 @@ TEST_F(ComponentsTest, Magnetometer) EXPECT_EQ(sdf::SensorType::MAGNETOMETER, comp3.Data().Type()); EXPECT_EQ("grape", comp3.Data().Topic()); EXPECT_DOUBLE_EQ(12.4, comp3.Data().UpdateRate()); - EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), comp3.Data().RawPose()); } ///////////////////////////////////////////////// diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index 0b41900cfa..5df8b289b5 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -104,9 +104,9 @@ TEST_F(DepthCameraTest, GZ_UTILS_TEST_DISABLED_ON_MAC(DepthCameraBox)) // Lock access to buffer and don't release it mutex.lock(); - EXPECT_DOUBLE_EQ(depthBuffer[left], gz::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[left], math::INF_D); EXPECT_NEAR(depthBuffer[mid], expectedRangeAtMidPointBox1, DEPTH_TOL); - EXPECT_DOUBLE_EQ(depthBuffer[right], gz::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[right], math::INF_D); delete[] depthBuffer; } diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 7b74a709fb..8a7223676e 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -71,8 +71,8 @@ TEST_F(DetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StartConnected)) auto poseRecorder = [](const std::string &_modelName, std::vector &_poses) { - return [&, _modelName](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + return [&, _modelName](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &_entity, const components::Model *, @@ -149,8 +149,8 @@ TEST_F(DetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) auto poseRecorder = [](const std::string &_linkName, std::vector &_poses) { - return [&, _linkName](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + return [&, _linkName](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &_entity, const components::Link *, diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 35ce6d2703..c8452b84f8 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -65,8 +65,8 @@ class DiffDriveTest : public InternalFixture<::testing::TestWithParam> test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -128,8 +128,8 @@ class DiffDriveTest : public InternalFixture<::testing::TestWithParam> double desiredLinVel = movementDirection * 10.5; double desiredAngVel = 0.2; velocityRamp.OnPreUpdate( - [&](const sim::UpdateInfo &/*_info*/, - const sim::EntityComponentManager &) + [&](const UpdateInfo &/*_info*/, + const EntityComponentManager &) { msgs::Set(msg.mutable_linear(), math::Vector3d(desiredLinVel, 0, 0)); @@ -249,8 +249,8 @@ TEST_P(DiffDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SkidPublishCmd)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -351,8 +351,8 @@ TEST_P(DiffDriveTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(EnableDisableCmd)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/elevator_system.cc b/test/integration/elevator_system.cc index 0f643f0565..5207b57f62 100644 --- a/test/integration/elevator_system.cc +++ b/test/integration/elevator_system.cc @@ -40,7 +40,7 @@ class ElevatorTestFixture : public ::testing::Test // Documentation inherited protected: void SetUp() override { - gz::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); } diff --git a/test/integration/entity_erase.cc b/test/integration/entity_erase.cc index e3e7e7b77d..8189f141d3 100644 --- a/test/integration/entity_erase.cc +++ b/test/integration/entity_erase.cc @@ -38,12 +38,12 @@ class PhysicsSystemFixture : public InternalFixture<::testing::Test> TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CreatePhysicsWorld)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); - sim::Server server(serverConfig); + Server server(serverConfig); EXPECT_TRUE(server.HasEntity("box")); EXPECT_TRUE(server.HasEntity("capsule")); EXPECT_TRUE(server.HasEntity("cylinder")); diff --git a/test/integration/gpu_lidar.cc b/test/integration/gpu_lidar.cc index 5e58fc0f03..25b54ce071 100644 --- a/test/integration/gpu_lidar.cc +++ b/test/integration/gpu_lidar.cc @@ -100,9 +100,9 @@ TEST_F(GpuLidarTest, GZ_UTILS_TEST_DISABLED_ON_MAC(GpuLidarBox)) double expectedRangeAtMidPointBox1 = 0.45; // Sensor 1 should see TestBox1 - EXPECT_DOUBLE_EQ(lastMsg.ranges(0), gz::math::INF_D); + EXPECT_DOUBLE_EQ(lastMsg.ranges(0), math::INF_D); EXPECT_NEAR(lastMsg.ranges(mid), expectedRangeAtMidPointBox1, LASER_TOL); - EXPECT_DOUBLE_EQ(lastMsg.ranges(last), gz::math::INF_D); + EXPECT_DOUBLE_EQ(lastMsg.ranges(last), math::INF_D); EXPECT_EQ("gpu_lidar::gpu_lidar_link::gpu_lidar", lastMsg.frame()); } diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc index 54ebf0cb7f..bcfb53b072 100644 --- a/test/integration/hydrodynamics.cc +++ b/test/integration/hydrodynamics.cc @@ -120,6 +120,8 @@ TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil)) auto sphere1Vels = this->TestWorld(world, "sphere1"); auto sphere2Vels = this->TestWorld(world, "sphere2"); + auto whenSphere1ExceedsSphere2Vel = 2000; + for (unsigned int i = 0; i < 1000; ++i) { // Sanity check @@ -128,19 +130,23 @@ TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil)) EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].X()); EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].Y()); - // Wait a couple of iterations for the body to move - if(i > 4) + // Expect sphere1 to fall faster than sphere 2 as no hydro + // drag is applied to it. + EXPECT_LE(sphere1Vels[i].Z(), sphere2Vels[i].Z()); + if(sphere1Vels[i].Z() < sphere2Vels[i].Z() + && whenSphere1ExceedsSphere2Vel > 1000) + { + // Mark this as the time when velocity of sphere1 exceeds sphere 2 + whenSphere1ExceedsSphere2Vel = i; + } + if (i > 900) { - EXPECT_LT(sphere1Vels[i].Z(), sphere2Vels[i].Z()); - - if (i > 900) - { - // Expect for the velocity to stabilize - EXPECT_NEAR(sphere1Vels[i-1].Z(), sphere1Vels[i].Z(), 1e-6); - EXPECT_NEAR(sphere2Vels[i-1].Z(), sphere2Vels[i].Z(), 1e-6); - } + // Expect for the velocity to stabilize + EXPECT_NEAR(sphere1Vels[i-1].Z(), sphere1Vels[i].Z(), 1e-6); + EXPECT_NEAR(sphere2Vels[i-1].Z(), sphere2Vels[i].Z(), 1e-6); } } + EXPECT_LT(whenSphere1ExceedsSphere2Vel, 500); } ///////////////////////////////////////////////// diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 42512cc0a7..dc04f9fa13 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -136,15 +136,15 @@ TEST_F(ImuTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) std::vector poses; std::vector accelerations; std::vector angularVelocities; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Imu *, const components::Name *_name, const components::WorldPose *_worldPose, @@ -175,7 +175,7 @@ TEST_F(ImuTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelFalling)) }); _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Gravity *_gravity) -> bool { // gtest is having a hard time with ASSERTs inside nested lambdas diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc index 0ae978c0dc..2ed5addbcd 100644 --- a/test/integration/joint_controller_system.cc +++ b/test/integration/joint_controller_system.cc @@ -70,7 +70,7 @@ TEST_F(JointControllerTestFixture, test::Relay testSystem; std::vector angularVelocities; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto link = _ecm.EntityByComponents(components::Link(), components::Name(linkName)); @@ -82,12 +82,12 @@ TEST_F(JointControllerTestFixture, } }); - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::AngularVelocity *_angularVel) -> bool @@ -168,7 +168,7 @@ TEST_F(JointControllerTestFixture, test::Relay testSystem; math::Vector3d angularVelocity; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto link = _ecm.EntityByComponents(components::Link(), components::Name(linkName)); @@ -180,12 +180,12 @@ TEST_F(JointControllerTestFixture, } }); - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::AngularVelocity *_angularVel) -> bool diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 94c2818a9d..e80b6d3e4f 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -71,7 +71,7 @@ TEST_F(JointPositionControllerTestFixture, test::Relay testSystem; std::vector currentPosition; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); @@ -83,12 +83,12 @@ TEST_F(JointPositionControllerTestFixture, } }); - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_position) -> bool diff --git a/test/integration/level_manager.cc b/test/integration/level_manager.cc index 0031a95239..b42194816d 100644 --- a/test/integration/level_manager.cc +++ b/test/integration/level_manager.cc @@ -73,15 +73,15 @@ class ModelMover: public test::Relay poseCmd = std::move(_pose); } - public: sim::Entity Id() const + public: Entity Id() const { return entity; } /// \brief Sets the pose component of the entity to the commanded pose. This /// function meant to be called in the preupdate phase - private: void MoveModel(const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + private: void MoveModel(const UpdateInfo &, + EntityComponentManager &_ecm) { if (this->poseCmd) { @@ -93,7 +93,7 @@ class ModelMover: public test::Relay /// \brief Entity to move - private: sim::Entity entity; + private: Entity entity; /// \brief Pose command private: std::optional poseCmd; }; @@ -106,7 +106,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> { InternalFixture::SetUp(); - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; // Except tile_0, which is on the default level, every tile belongs to a // level. The name of the level corresponds to the tile in its suffix, i.e., diff --git a/test/integration/level_manager_runtime_performers.cc b/test/integration/level_manager_runtime_performers.cc index b5f6772c79..f176dd3e0c 100644 --- a/test/integration/level_manager_runtime_performers.cc +++ b/test/integration/level_manager_runtime_performers.cc @@ -70,15 +70,15 @@ class ModelMover: public test::Relay poseCmd = std::move(_pose); } - public: sim::Entity Id() const + public: Entity Id() const { return entity; } /// \brief Sets the pose component of the entity to the commanded pose. This /// function meant to be called in the preupdate phase - private: void MoveModel(const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + private: void MoveModel(const UpdateInfo &, + EntityComponentManager &_ecm) { if (this->poseCmd) { @@ -90,7 +90,7 @@ class ModelMover: public test::Relay /// \brief Entity to move - private: sim::Entity entity; + private: Entity entity; /// \brief Pose command private: std::optional poseCmd; }; @@ -103,7 +103,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> { InternalFixture::SetUp(); - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; // Except tile_0, which is on the default level, every tile belongs to a // level. The name of the level corresponds to the tile in its suffix, i.e., @@ -112,7 +112,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> "/test/worlds/levels_no_performers.sdf"); serverConfig.SetUseLevels(true); - server = std::make_unique(serverConfig); + server = std::make_unique(serverConfig); // Add in the "box" performer using a service call transport::Node node; @@ -145,8 +145,8 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> test::Relay testSystem; // Check entities loaded on the default level - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { Entity sphere = _ecm.EntityByComponents(components::Name("sphere")); EXPECT_EQ(1u, @@ -198,7 +198,7 @@ class LevelManagerFixture : public InternalFixture<::testing::Test> this->server->Run(true, 1, false); } - public: std::unique_ptr server; + public: std::unique_ptr server; public: std::vector loadedModels; public: std::vector unloadedModels; public: std::vector loadedLights; @@ -214,8 +214,8 @@ TEST_F(LevelManagerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(DefaultLevel)) test::Relay recorder; // Check entities loaded on the default level - recorder.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + recorder.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( [&](const Entity &, const components::DefaultLevel *, diff --git a/test/integration/lift_drag_system.cc b/test/integration/lift_drag_system.cc index 68092642d1..91970f6c42 100644 --- a/test/integration/lift_drag_system.cc +++ b/test/integration/lift_drag_system.cc @@ -110,7 +110,7 @@ TEST_P(VerticalForceParamFixture, std::vector linearVelocities; std::vector forces; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { // Create velocity and acceleration components if they dont't exist. // This signals physics system to populate the component @@ -130,7 +130,7 @@ TEST_P(VerticalForceParamFixture, const double kp = 100.0; // Set a constant velocity to the prismatic joint testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); @@ -161,8 +161,8 @@ TEST_P(VerticalForceParamFixture, // drag system. This is needed to capture the wrench set by the lift drag // system. This assumption may not hold when systems are run in parallel. test::Relay wrenchRecorder; - wrenchRecorder.OnPreUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + wrenchRecorder.OnPreUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto bladeLink = firstEntityFromScopedName(bladeName, _ecm); auto bodyLink = firstEntityFromScopedName(bodyName, _ecm); diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index ef91066f2c..ae81d272ec 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -329,7 +329,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogDefaults)) std::string homeOrig; common::env(GZ_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeFake.c_str())); // Test case 1: // No path specified on command line. This does not go through @@ -416,7 +416,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogDefaults)) #endif // Revert environment variable after test is done - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeOrig.c_str())); } ///////////////////////////////////////////////// @@ -472,7 +472,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogPaths)) std::string homeOrig; common::env(GZ_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeFake.c_str())); // Store number of files before running auto logPath = common::joinPaths(homeFake.c_str(), ".gz", "sim", @@ -685,7 +685,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogPaths)) #endif // Revert environment variable after test is done - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeOrig.c_str())); this->RemoveLogsDir(); } @@ -1497,7 +1497,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogResources)) std::string homeOrig; common::env(GZ_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeFake.c_str())); const std::string recordPath = this->logDir; std::string statePath = common::joinPaths(recordPath, "state.tlog"); @@ -1584,7 +1584,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogTopics)) std::string homeOrig; common::env(GZ_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeFake.c_str())); const std::string recordPath = this->logDir; std::string statePath = common::joinPaths(recordPath, "state.tlog"); diff --git a/test/integration/logical_audio_sensor_plugin.cc b/test/integration/logical_audio_sensor_plugin.cc index 20f4d446c6..d462950570 100644 --- a/test/integration/logical_audio_sensor_plugin.cc +++ b/test/integration/logical_audio_sensor_plugin.cc @@ -67,10 +67,10 @@ TEST_F(LogicalAudioTest, EXPECT_FALSE(*server.Running(0)); // helper variables for checking the validity of the ECM - const gz::math::Pose3d sourcePose(0, 0, 0, 0, 0, 0); + const math::Pose3d sourcePose(0, 0, 0, 0, 0, 0); const auto zeroSeconds = std::chrono::seconds(0); - const gz::math::Pose3d micClosePose(0.5, 0, 0, 0, 0, 0); - const gz::math::Pose3d micFarPose(0, 0, 0, 0, 0, 0); + const math::Pose3d micClosePose(0.5, 0, 0, 0, 0, 0); + const math::Pose3d micFarPose(0, 0, 0, 0, 0, 0); std::chrono::steady_clock::duration sourceStartTime; bool firstTime{true}; diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index d1db0494cf..a7f2abddcb 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -93,11 +93,11 @@ TEST_F(LogicalCameraTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox)) // Create a system that checks sensor topics test::Relay testSystem; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::LogicalCamera *, const components::Name *_name) -> bool { @@ -177,22 +177,22 @@ TEST_F(LogicalCameraTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogicalCameraBox)) math::Pose3d boxPose(1, 0, 0.5, 0, 0, 0); math::Pose3d sensor1Pose(0.05, 0.05, 0.55, 0, 0, 0); mutex.lock(); - gz::msgs::LogicalCameraImage img1 = logicalCamera1Msgs.back(); - EXPECT_EQ(sensor1Pose, gz::msgs::Convert(img1.pose())); + msgs::LogicalCameraImage img1 = logicalCamera1Msgs.back(); + EXPECT_EQ(sensor1Pose, msgs::Convert(img1.pose())); EXPECT_EQ(1, img1.model().size()); EXPECT_EQ(boxName, img1.model(0).name()); - gz::math::Pose3d boxPoseCamera1Frame = sensor1Pose.Inverse() * boxPose; - EXPECT_EQ(boxPoseCamera1Frame, gz::msgs::Convert(img1.model(0).pose())); + math::Pose3d boxPoseCamera1Frame = sensor1Pose.Inverse() * boxPose; + EXPECT_EQ(boxPoseCamera1Frame, msgs::Convert(img1.model(0).pose())); mutex.unlock(); // Sensor 2 should see box too - note different sensor pose. math::Pose3d sensor2Pose(0.05, -0.45, 0.55, 0, 0, 0); mutex.lock(); - gz::msgs::LogicalCameraImage img2 = logicalCamera2Msgs.back(); - EXPECT_EQ(sensor2Pose, gz::msgs::Convert(img2.pose())); + msgs::LogicalCameraImage img2 = logicalCamera2Msgs.back(); + EXPECT_EQ(sensor2Pose, msgs::Convert(img2.pose())); EXPECT_EQ(1, img2.model().size()); EXPECT_EQ(boxName, img2.model(0).name()); - gz::math::Pose3d boxPoseCamera2Frame = sensor2Pose.Inverse() * boxPose; - EXPECT_EQ(boxPoseCamera2Frame, gz::msgs::Convert(img2.model(0).pose())); + math::Pose3d boxPoseCamera2Frame = sensor2Pose.Inverse() * boxPose; + EXPECT_EQ(boxPoseCamera2Frame, msgs::Convert(img2.model(0).pose())); mutex.unlock(); } diff --git a/test/integration/magnetometer_system.cc b/test/integration/magnetometer_system.cc index 798f69cac0..38daa6f972 100644 --- a/test/integration/magnetometer_system.cc +++ b/test/integration/magnetometer_system.cc @@ -83,13 +83,13 @@ TEST_F(MagnetometerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RotatedMagnetometer)) test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Magnetometer *, const components::Name *_name, const components::WorldPose *_worldPose) -> bool @@ -130,7 +130,7 @@ TEST_F(MagnetometerTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RotatedMagnetometer)) // Hardcoded SDF values math::Vector3d worldMagneticField(0.94, 0.76, -0.12); - gz::math::Vector3d field = poses.back().Rot().Inverse().RotateVector( + math::Vector3d field = poses.back().Rot().Inverse().RotateVector( worldMagneticField); mutex.lock(); EXPECT_NEAR(magnetometerMsgs.back().mutable_field_tesla()->x(), diff --git a/test/integration/multicopter.cc b/test/integration/multicopter.cc index 764c003cfb..32ba32913a 100644 --- a/test/integration/multicopter.cc +++ b/test/integration/multicopter.cc @@ -84,7 +84,7 @@ TEST_F(MulticopterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CommandedMotorSpeed)) const std::size_t iterTestStart{100}; const std::size_t nIters{500}; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -100,8 +100,8 @@ TEST_F(MulticopterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CommandedMotorSpeed)) }); testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // Command a motor speed // After nIters iterations, check angular velocity of each of the rotors @@ -150,7 +150,7 @@ TEST_F(MulticopterTest, const std::size_t nIters{2000}; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -188,8 +188,8 @@ TEST_F(MulticopterTest, }; testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) { if (!iterTestStart.has_value()) { @@ -258,7 +258,7 @@ TEST_F(MulticopterTest, auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &_info, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) { // Create components, if the don't exist, on the first iteration if (_info.iterations == 1) @@ -292,8 +292,8 @@ TEST_F(MulticopterTest, node.Advertise("/X3/gazebo/command/motor_speed"); testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) { // Publish a motor speed command { diff --git a/test/integration/network_handshake.cc b/test/integration/network_handshake.cc index b213fe96d8..b08cec03f8 100644 --- a/test/integration/network_handshake.cc +++ b/test/integration/network_handshake.cc @@ -47,8 +47,8 @@ uint64_t testPaused(bool _paused) bool paused = !_paused; uint64_t iterations = 0; - std::function cb = - [&](const gz::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); paused = _msg.paused(); @@ -180,8 +180,8 @@ TEST_F(NetworkHandshake, GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Updates)) // Subscribe to pose updates, which should come from the primary transport::Node node; std::vector zPos; - std::function cb = - [&](const gz::msgs::Pose_V &_msg) + std::function cb = + [&](const msgs::Pose_V &_msg) { for (int i = 0; i < _msg.pose().size(); ++i) { diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc index d79a4fb9ad..67cc1986ef 100644 --- a/test/integration/performer_detector.cc +++ b/test/integration/performer_detector.cc @@ -201,8 +201,8 @@ TEST_F(PerformerDetectorTest, auto server = this->StartServer("/test/worlds/performer_detector.sdf", true); test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &_info, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &_info, + EntityComponentManager &_ecm) { Entity vehicle = _ecm.EntityByComponents( components::Model(), components::Name("vehicle_blue")); diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 0511fc4d36..bee99946f1 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -100,12 +100,12 @@ class PhysicsSystemFixtureWithDart6_10 : public PhysicsSystemFixture ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); @@ -122,7 +122,7 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) // See https://github.com/gazebosim/gz-sim/issues/1175 TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(FallingObject)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/falling.sdf"; @@ -133,22 +133,22 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(FallingObject)) const sdf::World *world = root.WorldByIndex(0); const sdf::Model *model = world->ModelByIndex(0); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); const std::string modelName = "sphere"; - std::vector spherePoses; + std::vector spherePoses; // Create a system that records the poses of the sphere test::Relay testSystem; testSystem.OnPostUpdate( - [modelName, &spherePoses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [modelName, &spherePoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (_name->Data() == modelName) { @@ -191,7 +191,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(FallingObject)) // must be correct. TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanonicalLink)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/canonical.sdf"; @@ -203,7 +203,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanonicalLink)) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); @@ -212,7 +212,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanonicalLink)) const sdf::Model *model = world->ModelByIndex(0); - std::unordered_map expectedLinPoses; + std::unordered_map expectedLinPoses; for (auto &linkName : linksToCheck) expectedLinPoses[linkName] = model->LinkByName(linkName)->RawPose(); ASSERT_EQ(3u, expectedLinPoses.size()); @@ -220,14 +220,14 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanonicalLink)) // Create a system that records the poses of the links after physics test::Relay testSystem; - std::unordered_map postUpLinkPoses; + std::unordered_map postUpLinkPoses; testSystem.OnPostUpdate( - [&modelName, &postUpLinkPoses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&modelName, &postUpLinkPoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose, const components::ParentEntity *_parent)->bool { @@ -263,7 +263,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanonicalLink)) TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(NonDefaultCanonicalLink)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/nondefault_canonical.sdf"; @@ -275,7 +275,7 @@ TEST_F(PhysicsSystemFixture, serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); @@ -284,13 +284,13 @@ TEST_F(PhysicsSystemFixture, // Create a system that records the pose of the model. test::Relay testSystem; - std::vector modelPoses; + std::vector modelPoses; testSystem.OnPostUpdate( - [&modelName, &modelPoses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&modelName, &modelPoses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (_name->Data() == modelName) @@ -318,7 +318,7 @@ TEST_F(PhysicsSystemFixture, // Test physics integration with revolute joints TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RevoluteJoint)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -330,7 +330,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RevoluteJoint)) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); @@ -347,11 +347,11 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RevoluteJoint)) // arm is in its initial position. The minimum distance is when the arm is in // line with the support arm. testSystem.OnPostUpdate( - [&rotatingLinkName, &armDistances](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&rotatingLinkName, &armDistances](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose)->bool { if (rotatingLinkName == _name->Data()) @@ -397,8 +397,8 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RevoluteJoint)) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CreateRuntime)) { - gz::sim::ServerConfig serverConfig; - sim::Server server(serverConfig); + ServerConfig serverConfig; + Server server(serverConfig); server.SetPaused(false); // Create a system just to get the ECM @@ -408,8 +408,8 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CreateRuntime)) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -481,29 +481,29 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(CreateRuntime)) TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(SetFrictionCoefficient)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/friction.sdf"; serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); std::map boxParams{ {"box1", 0.01}, {"box2", 0.1}, {"box3", 1.0}}; - std::map> poses; + std::map> poses; // Create a system that records the poses of the 3 boxes test::Relay testSystem; testSystem.OnPostUpdate( - [&boxParams, &poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&boxParams, &poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { if (boxParams.find(_name->Data()) != boxParams.end()) { @@ -567,23 +567,23 @@ TEST_F(PhysicsSystemFixture, TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(MultiAxisJointPosition)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/demo_joint_types.sdf"; serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(0ns); test::Relay testSystem; // Create JointPosition components if they don't already exist testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, components::Joint *) -> bool { auto posComp = _ecm.Component(_entity); @@ -598,10 +598,10 @@ TEST_F(PhysicsSystemFixture, std::map jointPosDof; testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_jointPos) -> bool @@ -647,7 +647,7 @@ TEST_F(PhysicsSystemFixture, TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResetPositionComponent)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -659,7 +659,7 @@ TEST_F(PhysicsSystemFixture, serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -673,10 +673,10 @@ TEST_F(PhysicsSystemFixture, bool firstRun = true; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { EXPECT_NE(nullptr, _name); @@ -711,11 +711,11 @@ TEST_F(PhysicsSystemFixture, std::vector positions; testSystem.OnPostUpdate([&]( - const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -747,7 +747,7 @@ TEST_F(PhysicsSystemFixture, TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResetVelocityComponent)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -759,7 +759,7 @@ TEST_F(PhysicsSystemFixture, serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -773,10 +773,10 @@ TEST_F(PhysicsSystemFixture, bool firstRun = true; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -809,12 +809,12 @@ TEST_F(PhysicsSystemFixture, std::vector velocities; testSystem.OnPostUpdate([&]( - const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointVelocity *_vel) @@ -845,7 +845,7 @@ TEST_F(PhysicsSystemFixture, /// Test joint position limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -857,7 +857,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -878,10 +878,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) // commands do not break the positional limit. testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -934,12 +934,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) std::vector positions; testSystem.OnPostUpdate([&]( - const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -968,7 +968,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) /// Test joint velocity limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint.sdf"; @@ -980,7 +980,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -1001,10 +1001,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) // commands do not break the velocity limit. testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -1057,12 +1057,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) std::vector velocities; testSystem.OnPostUpdate([&]( - const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointVelocity *_vel) @@ -1092,7 +1092,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) /// Test joint effort limit command component TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/revolute_joint_equilibrium.sdf"; @@ -1104,7 +1104,7 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ms); @@ -1125,10 +1125,10 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) // commands do not break the effort limit. testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, + [&](const Entity &_entity, const components::Joint *, components::Name *_name) -> bool { if (_name->Data() == rotatingJointName) @@ -1186,12 +1186,12 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) std::vector positions; testSystem.OnPostUpdate([&]( - const sim::UpdateInfo &, const sim::EntityComponentManager &_ecm) + const UpdateInfo &, const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_pos) @@ -1219,28 +1219,28 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/contact.sdf"; serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); // a map of model name to its axis aligned box - std::map bbox; + std::map bbox; // Create a system that records the bounding box of a model test::Relay testSystem; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, const components::Model *, + [&](const Entity &_entity, const components::Model *, const components::Name *_name, const components::Static *)->bool { // create axis aligned box to be filled by physics @@ -1258,13 +1258,13 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) }); testSystem.OnPostUpdate( - [&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) { // store models that have axis aligned box computed _ecm.Each( - [&](const gz::sim::Entity &, const components::Model *, + [&](const Entity &, const components::Model *, const components::Name *_name, const components::Static *, const components::AxisAlignedBox *_aabb)->bool { @@ -1279,9 +1279,9 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) EXPECT_EQ(1u, bbox.size()); EXPECT_EQ("box1", bbox.begin()->first); - EXPECT_EQ(gz::math::AxisAlignedBox( - gz::math::Vector3d(-1.25, -2, 0), - gz::math::Vector3d(-0.25, 2, 1)), + EXPECT_EQ(math::AxisAlignedBox( + math::Vector3d(-1.25, -2, 0), + math::Vector3d(-0.25, 2, 1)), bbox.begin()->second); } @@ -1290,7 +1290,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) // This tests whether nested models can be loaded correctly TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(NestedModel)) { - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + "/test/worlds/nested_model.sdf"; @@ -1302,22 +1302,22 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(NestedModel)) serverConfig.SetSdfFile(sdfFile); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1us); // Create a system that records the poses of the links after physics test::Relay testSystem; - std::unordered_map postUpModelPoses; - std::unordered_map postUpLinkPoses; + std::unordered_map postUpModelPoses; + std::unordered_map postUpLinkPoses; std::unordered_map parents; testSystem.OnPostUpdate( - [&postUpModelPoses, &postUpLinkPoses, &parents](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [&postUpModelPoses, &postUpLinkPoses, &parents](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_entity, const components::Model *, + [&](const Entity &_entity, const components::Model *, const components::Name *_name, const components::Pose *_pose)->bool { // store model pose @@ -1336,7 +1336,7 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(NestedModel)) _ecm.Each( - [&](const gz::sim::Entity &, const components::Link *, + [&](const Entity &, const components::Link *, const components::Name *_name, const components::Pose *_pose, const components::ParentEntity *_parent)->bool { diff --git a/test/integration/play_pause.cc b/test/integration/play_pause.cc index e165c270dd..9774791ab7 100644 --- a/test/integration/play_pause.cc +++ b/test/integration/play_pause.cc @@ -39,13 +39,13 @@ uint64_t kIterations; // Send a world control message. void worldControl(bool _paused, uint64_t _steps) { - std::function cb = - [&](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [&](const msgs::Boolean &/*_rep*/, const bool _result) { EXPECT_TRUE(_result); }; - gz::msgs::WorldControl req; + msgs::WorldControl req; req.set_pause(_paused); req.set_multi_step(_steps); transport::Node node; @@ -61,8 +61,8 @@ void testPaused(bool _paused) transport::Node node; bool paused = !_paused; - std::function cb = - [&](const gz::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); paused = _msg.paused(); @@ -84,8 +84,8 @@ uint64_t iterations() transport::Node node; uint64_t iterations = 0; - std::function cb = - [&](const gz::msgs::WorldStatistics &_msg) + std::function cb = + [&](const msgs::WorldStatistics &_msg) { std::unique_lock lock(mutex); iterations = _msg.iterations(); diff --git a/test/integration/pose_publisher_system.cc b/test/integration/pose_publisher_system.cc index a36d2c64e2..9c65c42fdf 100644 --- a/test/integration/pose_publisher_system.cc +++ b/test/integration/pose_publisher_system.cc @@ -143,8 +143,8 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) [&modelName, &baseName, &lowerLinkName, &upperLinkName, &sensorName, &poses, &basePoses, &lowerLinkPoses, &upperLinkPoses, &sensorPoses, ×tamps]( - const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // get our double pendulum model auto id = _ecm.EntityByComponents( @@ -191,7 +191,7 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) // timestamps auto simTimeSecNsec = - gz::math::durationToSecNsec(_info.simTime); + math::durationToSecNsec(_info.simTime); timestamps.push_back( math::secNsecToTimePoint( simTimeSecNsec.first, @@ -240,7 +240,7 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PublishCmd)) // sort the pose msgs according to timestamp std::sort(poseMsgs.begin(), poseMsgs.end(), []( - const gz::msgs::Pose &_l, const gz::msgs::Pose &_r) + const msgs::Pose &_l, const msgs::Pose &_r) { std::chrono::steady_clock::time_point lt = math::secNsecToTimePoint( @@ -440,8 +440,8 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StaticPosePublisher)) [&modelName, &baseName, &lowerLinkName, &upperLinkName, &sensorName, &poses, &basePoses, &lowerLinkPoses, &upperLinkPoses, &sensorPoses, ×tamps, &staticPoseTimestamps]( - const sim::UpdateInfo &_info, - const sim::EntityComponentManager &_ecm) + const UpdateInfo &_info, + const EntityComponentManager &_ecm) { // get our double pendulum model auto id = _ecm.EntityByComponents( @@ -488,7 +488,7 @@ TEST_F(PosePublisherTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StaticPosePublisher)) // timestamps auto simTimeSecNsec = - gz::math::durationToSecNsec(_info.simTime); + math::durationToSecNsec(_info.simTime); timestamps.push_back( math::secNsecToTimePoint( simTimeSecNsec.first, simTimeSecNsec.second)); diff --git a/test/integration/rgbd_camera.cc b/test/integration/rgbd_camera.cc index 5a0433554a..a00d31b66e 100644 --- a/test/integration/rgbd_camera.cc +++ b/test/integration/rgbd_camera.cc @@ -104,9 +104,9 @@ TEST_F(RgbdCameraTest, GZ_UTILS_TEST_DISABLED_ON_MAC(RgbdCameraBox)) // Lock access to buffer and don't release it mutex.lock(); - EXPECT_DOUBLE_EQ(depthBuffer[left], gz::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[left], math::INF_D); EXPECT_NEAR(depthBuffer[mid], expectedRangeAtMidPointBox1, DEPTH_TOL); - EXPECT_DOUBLE_EQ(depthBuffer[right], gz::math::INF_D); + EXPECT_DOUBLE_EQ(depthBuffer[right], math::INF_D); delete[] depthBuffer; } diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index bc080ce69b..75ed9ba74d 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -57,7 +57,7 @@ class SceneBroadcasterTest TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PoseInfo)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -107,7 +107,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PoseInfo)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfo)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -124,7 +124,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfo)) bool result{false}; unsigned int timeout{5000}; - gz::msgs::Scene res; + msgs::Scene res; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res, result)); EXPECT_TRUE(result); @@ -142,7 +142,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfo)) } // Repeat the request to make sure the same information is returned - gz::msgs::Scene res2; + msgs::Scene res2; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res2, result)); EXPECT_TRUE(result); @@ -153,7 +153,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfo)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneGraph)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -170,7 +170,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneGraph)) bool result{false}; unsigned int timeout{5000}; - gz::msgs::StringMsg res; + msgs::StringMsg res; EXPECT_TRUE(node.Request("/world/default/scene/graph", timeout, res, result)); EXPECT_TRUE(result); @@ -193,7 +193,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneGraph)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneTopic)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -225,7 +225,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneTopic)) bool result{false}; unsigned int timeout{5000}; - gz::msgs::Scene msg; + msgs::Scene msg; EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, msg, result)); EXPECT_TRUE(result); @@ -238,7 +238,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneTopicSensors)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/altimeter_with_pose.sdf"); @@ -270,7 +270,7 @@ TEST_P(SceneBroadcasterTest, bool result{false}; unsigned int timeout{5000}; - gz::msgs::Scene msg; + msgs::Scene msg; EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info", timeout, msg, result)); @@ -289,7 +289,7 @@ TEST_P(SceneBroadcasterTest, TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(DeletedTopic)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -350,7 +350,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(DeletedTopic)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnedModel)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -396,8 +396,8 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnedModel)) // Check that the model is in the scene/infor response { - gz::msgs::Empty req; - gz::msgs::Scene rep; + msgs::Empty req; + msgs::Scene rep; bool result; unsigned int timeout = 2000; EXPECT_TRUE(node.Request("/world/default/scene/info", req, timeout, @@ -420,7 +420,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SpawnedModel)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(State)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/shapes.sdf"); @@ -511,7 +511,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(State)) std::string reqSrv = "/state_async_callback_test"; node.Advertise(reqSrv, cbAsync); - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(reqSrv); node.Request("/world/default/state_async", req); @@ -531,7 +531,7 @@ TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(State)) TEST_P(SceneBroadcasterTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(StateStatic)) { // Start server - gz::sim::ServerConfig serverConfig; + sim::ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/empty.sdf"); diff --git a/test/integration/sdf_frame_semantics.cc b/test/integration/sdf_frame_semantics.cc index 77193b32f7..796caa2670 100644 --- a/test/integration/sdf_frame_semantics.cc +++ b/test/integration/sdf_frame_semantics.cc @@ -64,7 +64,7 @@ class SdfFrameSemanticsTest : public InternalFixture<::testing::Test> EXPECT_FALSE(*this->server->Running(0)); // A pointer to the ecm. This will be valid once we run the mock system relay->OnPreUpdate( - [this](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [this](const UpdateInfo &, EntityComponentManager &_ecm) { this->ecm = &_ecm; }); @@ -97,9 +97,9 @@ class SdfFrameSemanticsTest : public InternalFixture<::testing::Test> this->creator->SetParent(modelEntity, worldEntity); } - public: sim::Model GetModel(const std::string &_name) + public: Model GetModel(const std::string &_name) { - return sim::Model(this->ecm->EntityByComponents( + return Model(this->ecm->EntityByComponents( components::Model(), components::Name(_name))); } @@ -164,8 +164,8 @@ TEST_F(SdfFrameSemanticsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LinkRelativeTo)) EXPECT_NE(link2, kNullEntity); // Expect the pose of L2 relative to model to be 0 0 1 0 0 pi - gz::math::Pose3d expRelPose(0, 0, 1, 0, 0, GZ_PI); - gz::math::Pose3d expWorldPose(0, 0, 3, 0, 0, GZ_PI); + math::Pose3d expRelPose(0, 0, 1, 0, 0, GZ_PI); + math::Pose3d expWorldPose(0, 0, 3, 0, 0, GZ_PI); EXPECT_EQ(expRelPose, this->GetPose(link2)); @@ -231,7 +231,7 @@ TEST_F(SdfFrameSemanticsTest, JointRelativeTo) // Expect the pose of J1 relative to model to be the same as L2 (default // behavior) - gz::math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); + math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); // Expect the pose of J2 relative to model to be the same as L2 (non default // behavior due to "relative_to='L2'") @@ -286,7 +286,7 @@ TEST_F(SdfFrameSemanticsTest, VisualCollisionRelativeTo) // Expect the pose of v1 and relative to L2 (their parent link) to be the same // as the pose of L1 relative to L2 - gz::math::Pose3d expPose(0, 0, -1, 0, 0, 0); + math::Pose3d expPose(0, 0, -1, 0, 0, 0); EXPECT_EQ(expPose, this->GetPose(visual)); EXPECT_EQ(expPose, this->GetPose(collision)); @@ -334,13 +334,13 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithLinks) // Expect the pose of L1 and relative to M to be the same // as the pose of F1 relative to M - gz::math::Pose3d link1ExpRelativePose(0, 0, 1, 0, 0, 0); + math::Pose3d link1ExpRelativePose(0, 0, 1, 0, 0, 0); EXPECT_EQ(link1ExpRelativePose, this->GetPose(link1)); // Expect the pose of L2 and relative to M to be the same // as the pose of F2, which is at the origin of M - gz::math::Pose3d link2ExpRelativePose = gz::math::Pose3d::Zero; + math::Pose3d link2ExpRelativePose = math::Pose3d::Zero; EXPECT_EQ(link2ExpRelativePose, this->GetPose(link2)); @@ -389,7 +389,7 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithJoints) this->server->Run(true, 1, false); // Expect the pose of J1 relative to model to be the same as F1 in world - gz::math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); + math::Pose3d expWorldPose(1, 0, 2, 0, 0, 0); // TODO(anyone) Enable the following expectation when a joint's WorldPose // can be computed by gz-physics. // EXPECT_EQ(expWorldPose, this->GetWorldPose(joint1)); @@ -440,7 +440,7 @@ TEST_F(SdfFrameSemanticsTest, ExplicitFramesWithVisualAndCollision) // Expect the pose of v1 and relative to L1 (their parent link) to be the same // as the pose of F1 relative to L1 - gz::math::Pose3d expPose(0, 0, 1, 0, 0, 0); + math::Pose3d expPose(0, 0, 1, 0, 0, 0); EXPECT_EQ(expPose, this->GetPose(visual)); EXPECT_EQ(expPose, this->GetPose(collision)); diff --git a/test/integration/sdf_include.cc b/test/integration/sdf_include.cc index 9db6b2ac84..f10c93b4d7 100644 --- a/test/integration/sdf_include.cc +++ b/test/integration/sdf_include.cc @@ -39,13 +39,13 @@ TEST_F(SdfInclude, GZ_UTILS_TEST_DISABLED_ON_WIN32(DownloadFromFuel)) std::string path = common::cwd() + "/test_cache"; // Configure the gazebo server, which will cause a model to be downloaded. - sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetResourceCache(path); serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/include.sdf"); - sim::Server server(serverConfig); + Server server(serverConfig); EXPECT_TRUE(common::exists(path + - "/fuel.ignitionrobotics.org/openrobotics/models/ground plane" + + "/fuel.gazebosim.org/openrobotics/models/ground plane" + "/1/model.sdf")); } diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 8244f781fa..1e9155514c 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -67,11 +67,11 @@ TEST_F(ThermalTest, GZ_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) std::map entityTempRange; std::map heatSignatures; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &_id, + [&](const Entity &_id, const components::Temperature *_temp, const components::Name *_name) -> bool { diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc index bc37896c78..64b4fbc0f9 100644 --- a/test/integration/tracked_vehicle_system.cc +++ b/test/integration/tracked_vehicle_system.cc @@ -90,7 +90,7 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> << "Failed to find plugin [" << *pluginLib << "]"; // Load engine plugin - gz::plugin::Loader pluginLoader; + plugin::Loader pluginLoader; auto plugins = pluginLoader.LoadLib(pathToLib); ASSERT_FALSE(plugins.empty()) << "Unable to load the [" << pathToLib << "] library"; @@ -133,8 +133,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; - ecmGetterSystem.OnPreUpdate([&ecm](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + ecmGetterSystem.OnPreUpdate([&ecm](const UpdateInfo &, + EntityComponentManager &_ecm) { if (ecm == nullptr) ecm = &_ecm; @@ -156,8 +156,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> test::Relay testSystem; Entity modelEntity {kNullEntity}; std::vector poses; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { modelEntity = _ecm.EntityByComponents( components::Model(), @@ -253,7 +253,7 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> poses.clear(); - sim::Model model(modelEntity); + Model model(modelEntity); // Move the robot somewhere to free space without obstacles. model.SetWorldPoseCmd(*ecm, math::Pose3d(10, 10, 0.1, 0, 0, 0)); @@ -455,8 +455,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; - ecmGetterSystem.OnPreUpdate([&ecm](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + ecmGetterSystem.OnPreUpdate([&ecm](const UpdateInfo &, + EntityComponentManager &_ecm) { if (ecm == nullptr) ecm = &_ecm; @@ -478,8 +478,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> test::Relay testSystem; Entity boxEntity {kNullEntity}; std::vector poses; - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { boxEntity = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index d7e6c9e6ad..c9b4f6a803 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -75,8 +75,8 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Create)) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -354,8 +354,8 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Remove)) // shared pointer owned by the SimulationRunner. EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -538,8 +538,8 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Pose)) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -712,8 +712,8 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PoseVector)) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -1057,8 +1057,8 @@ TEST_F(UserCommandsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Physics)) // Create a system just to get the ECM EntityComponentManager *ecm{nullptr}; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index c355d4ba4d..9e82087ad7 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -58,8 +58,8 @@ class VelocityControlTest test::Relay testSystem; std::vector poses; - testSystem.OnPostUpdate([&poses](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto id = _ecm.EntityByComponents( components::Model(), @@ -145,8 +145,8 @@ class VelocityControlTest std::vector modelPoses; std::vector linkPoses; testSystem.OnPostUpdate([&linkPoses, &modelPoses]( - const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + const UpdateInfo &, + const EntityComponentManager &_ecm) { auto modelId = _ecm.EntityByComponents( components::Model(), diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index b05ff2b1f9..08453c8933 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -82,7 +82,7 @@ class WheelSlipTest : public InternalFixture<::testing::Test> public: double drumSpeed = 0.0; /// \brief Steer angle to apply. - public: gz::math::Angle steer; + public: math::Angle steer; /// \brief Suspension force to apply in N. public: double suspForce = 0.0; @@ -120,10 +120,10 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - sim::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -141,7 +141,7 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) Entity worldEntity = ecm->EntityByComponents(components::World()); - EXPECT_NE(sim::kNullEntity, worldEntity); + EXPECT_NE(kNullEntity, worldEntity); // Get both models Entity tireEntity = @@ -152,8 +152,8 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) ecm->EntityByComponents(components::Model(), components::Name("drum")); - EXPECT_NE(sim::kNullEntity, tireEntity); - EXPECT_NE(sim::kNullEntity, drumEntity); + EXPECT_NE(kNullEntity, tireEntity); + EXPECT_NE(kNullEntity, drumEntity); Entity wheelLinkEntity = ecm->EntityByComponents( components::ParentEntity(tireEntity), @@ -165,8 +165,8 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) components::Name("link"), components::Link()); - EXPECT_NE(sim::kNullEntity, wheelLinkEntity); - EXPECT_NE(sim::kNullEntity, drumLinkEntity); + EXPECT_NE(kNullEntity, wheelLinkEntity); + EXPECT_NE(kNullEntity, drumLinkEntity); auto wheelInertialComp = ecm->Component(wheelLinkEntity); @@ -193,8 +193,8 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) components::Name("collision"), components::Collision()); - EXPECT_NE(sim::kNullEntity, collisionWheelLinkEntity); - EXPECT_NE(sim::kNullEntity, collisionDrumLinkEntity); + EXPECT_NE(kNullEntity, collisionWheelLinkEntity); + EXPECT_NE(kNullEntity, collisionDrumLinkEntity); auto wheelCollisionComp = ecm->Component(collisionWheelLinkEntity); @@ -240,19 +240,16 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) // const double kp = surfaceContactOde->GetElement("kp")->Get(); // ASSERT_EQ(kp, 250e3); - double modelMass = 0.0; for (const auto &linkName : linksToCheck) { Entity linkEntity = ecm->EntityByComponents( components::ParentEntity(tireEntity), components::Name(linkName), components::Link()); - EXPECT_NE(sim::kNullEntity, linkEntity); + EXPECT_NE(kNullEntity, linkEntity); auto inertialComp = ecm->Component(linkEntity); EXPECT_NE(nullptr, inertialComp); - - modelMass += inertialComp->Data().MassMatrix().Mass(); } // Get axle wheel and steer joint of wheel model @@ -261,14 +258,14 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TireDrum)) components::Name("axle_wheel"), components::Joint()); - ASSERT_NE(sim::kNullEntity, wheelAxleJointEntity); + ASSERT_NE(kNullEntity, wheelAxleJointEntity); Entity wheelSteerJointEntity = ecm->EntityByComponents( components::ParentEntity(tireEntity), components::Name("steer"), components::Joint()); - ASSERT_NE(sim::kNullEntity, wheelSteerJointEntity); + ASSERT_NE(kNullEntity, wheelSteerJointEntity); const double wheelSpeed = -25.0 * metersPerMile / secondsPerHour / wheelRadius; @@ -382,10 +379,10 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); - sim::EntityComponentManager *ecm = nullptr; + EntityComponentManager *ecm = nullptr; test::Relay testSystem; - testSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &_ecm) + testSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &_ecm) { ecm = &_ecm; }); @@ -403,7 +400,7 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) Entity worldEntity = ecm->EntityByComponents(components::World()); - EXPECT_NE(sim::kNullEntity, worldEntity); + EXPECT_NE(kNullEntity, worldEntity); auto gravity = ecm->Component(worldEntity); @@ -415,14 +412,14 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) ecm->EntityByComponents(components::Model(), components::Name("trisphere_cycle0")); - EXPECT_NE(sim::kNullEntity, trisphereCycle0Entity); + EXPECT_NE(kNullEntity, trisphereCycle0Entity); Entity trisphereCycle1Entity = ecm->EntityByComponents(components::Model(), components::Name("trisphere_cycle1")); - EXPECT_NE(sim::kNullEntity, trisphereCycle1Entity); + EXPECT_NE(kNullEntity, trisphereCycle1Entity); // Check rear left wheel of first model Entity wheelRearLeftEntity = ecm->EntityByComponents( @@ -430,7 +427,7 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) components::Name("wheel_rear_left"), components::Link()); - EXPECT_NE(sim::kNullEntity, wheelRearLeftEntity); + EXPECT_NE(kNullEntity, wheelRearLeftEntity); Entity wheelRearLeftCollisionEntity = ecm->EntityByComponents( components::ParentEntity(wheelRearLeftEntity), @@ -450,28 +447,28 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) components::Name("wheel_rear_left_spin"), components::Joint()); - EXPECT_NE(sim::kNullEntity, wheelRearLeftSpin0Entity); + EXPECT_NE(kNullEntity, wheelRearLeftSpin0Entity); Entity wheelRearRightSpin0Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle0Entity), components::Name("wheel_rear_right_spin"), components::Joint()); - EXPECT_NE(sim::kNullEntity, wheelRearRightSpin0Entity); + EXPECT_NE(kNullEntity, wheelRearRightSpin0Entity); Entity wheelRearLeftSpin1Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle1Entity), components::Name("wheel_rear_left_spin"), components::Joint()); - EXPECT_NE(sim::kNullEntity, wheelRearLeftSpin1Entity); + EXPECT_NE(kNullEntity, wheelRearLeftSpin1Entity); Entity wheelRearRightSpin1Entity = ecm->EntityByComponents( components::ParentEntity(trisphereCycle1Entity), components::Name("wheel_rear_right_spin"), components::Joint()); - EXPECT_NE(sim::kNullEntity, wheelRearRightSpin1Entity); + EXPECT_NE(kNullEntity, wheelRearRightSpin1Entity); // Set speed of both models const double angularSpeed = 6.0; @@ -526,8 +523,8 @@ TEST_F(WheelSlipTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TricyclesUphill)) } test::Relay testSlipSystem; - testSlipSystem.OnPreUpdate([&](const sim::UpdateInfo &, - sim::EntityComponentManager &) + testSlipSystem.OnPreUpdate([&](const UpdateInfo &, + EntityComponentManager &) { auto wheelRearLeftVelocity0Cmd = ecm->Component(wheelRearLeftSpin0Entity); diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc index 0c8df2d725..492dad2672 100644 --- a/test/integration/wind_effects.cc +++ b/test/integration/wind_effects.cc @@ -85,7 +85,7 @@ class LinkComponentRecorder if (_createComp) { this->mockSystem->preUpdateCallback = - [this](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [this](const UpdateInfo &, EntityComponentManager &_ecm) { auto linkEntity = _ecm.EntityByComponents( components::Link(), components::Name(this->linkName)); @@ -101,8 +101,8 @@ class LinkComponentRecorder } this->mockSystem->postUpdateCallback = - [this](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + [this](const UpdateInfo &, + const EntityComponentManager &_ecm) { auto linkEntity = _ecm.EntityByComponents( components::Link(), components::Name(this->linkName)); diff --git a/test/performance/level_manager.cc b/test/performance/level_manager.cc index 3e79820fd6..347d6c1ef7 100644 --- a/test/performance/level_manager.cc +++ b/test/performance/level_manager.cc @@ -38,10 +38,10 @@ TEST(LevelManagerPerfrormance, GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelVsNoLevel)) common::Console::SetVerbosity(4); - gz::common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", + common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + "/test/worlds/level_performance.sdf"); math::Stopwatch watch; @@ -53,7 +53,7 @@ TEST(LevelManagerPerfrormance, GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelVsNoLevel)) // measuring time differences between levels and no levels. { serverConfig.SetUseLevels(true); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); server.Run(true, 1, false); @@ -62,7 +62,7 @@ TEST(LevelManagerPerfrormance, GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelVsNoLevel)) // Server with levels { serverConfig.SetUseLevels(true); - sim::Server server(serverConfig); + Server server(serverConfig); server.SetUpdatePeriod(1ns); watch.Start(true); @@ -74,7 +74,7 @@ TEST(LevelManagerPerfrormance, GZ_UTILS_TEST_DISABLED_ON_WIN32(LevelVsNoLevel)) // Server without levels { serverConfig.SetUseLevels(false); - sim::Server serverNoLevels(serverConfig); + Server serverNoLevels(serverConfig); serverNoLevels.SetUpdatePeriod(1ns); watch.Start(true); diff --git a/test/performance/sdf_runner.cc b/test/performance/sdf_runner.cc index 291e91ab4b..2c2ec27dbf 100644 --- a/test/performance/sdf_runner.cc +++ b/test/performance/sdf_runner.cc @@ -35,7 +35,7 @@ using namespace sim; ////////////////////////////////////////////////// int main(int _argc, char** _argv) { - gz::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); std::string sdfFile{""}; if (_argc >= 2) @@ -58,7 +58,7 @@ int main(int _argc, char** _argv) } gzdbg << "Update rate: " << updateRate << std::endl; - gz::sim::ServerConfig serverConfig; + ServerConfig serverConfig; if (!serverConfig.SetSdfFile(sdfFile)) { gzerr << "Failed to set SDF file [" << sdfFile << "]" << std::endl; @@ -70,23 +70,23 @@ int main(int _argc, char** _argv) serverConfig.SetUpdateRate(updateRate); // Create the Gazebo server - gz::sim::Server server(serverConfig); + Server server(serverConfig); - gz::transport::Node node; + transport::Node node; - std::vector msgs; + std::vector msgs; msgs.reserve(iterations); - std::function cb = - [&](const gz::msgs::Clock &_msg) + std::function cb = + [&](const msgs::Clock &_msg) { msgs.push_back(_msg); }; double progress = 0; - std::function cb2 = - [&](const gz::msgs::WorldStatistics &_msg) + std::function cb2 = + [&](const msgs::WorldStatistics &_msg) { double nIters = static_cast(_msg.iterations()); nIters = nIters / iterations * 100; diff --git a/test/plugins/TestSystem.cc b/test/plugins/TestSystem.cc index 61f8a566b3..295e700505 100644 --- a/test/plugins/TestSystem.cc +++ b/test/plugins/TestSystem.cc @@ -32,6 +32,6 @@ TestSystem::TestSystem() TestSystem::~TestSystem() = default; // Register this plugin -GZ_ADD_PLUGIN(TestSystem, gz::sim::System) +GZ_ADD_PLUGIN(TestSystem, System) GZ_ADD_PLUGIN_ALIAS(TestSystem, "gz::sim::TestSystem") diff --git a/test/worlds/diff_drive_no_plugin.sdf b/test/worlds/diff_drive_no_plugin.sdf index 55790f45ef..b1fd389d94 100644 --- a/test/worlds/diff_drive_no_plugin.sdf +++ b/test/worlds/diff_drive_no_plugin.sdf @@ -11,8 +11,8 @@ name="gz::sim::systems::Physics"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> diff --git a/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.sdf b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.sdf new file mode 100644 index 0000000000..b9386ade65 --- /dev/null +++ b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.sdf @@ -0,0 +1,5 @@ + + + + + diff --git a/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.world b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.world new file mode 100644 index 0000000000..b9386ade65 --- /dev/null +++ b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/test.world @@ -0,0 +1,5 @@ + + + + + diff --git a/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/thumbnails/1.png b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/thumbnails/1.png new file mode 100644 index 0000000000..a72d5aa96b Binary files /dev/null and b/test/worlds/fuel.gazebosim.org/openrobotics/worlds/test%20world/2/thumbnails/1.png differ diff --git a/test/worlds/hydrodynamics.sdf b/test/worlds/hydrodynamics.sdf index 234d6314ea..7fc4790c84 100644 --- a/test/worlds/hydrodynamics.sdf +++ b/test/worlds/hydrodynamics.sdf @@ -15,14 +15,6 @@ filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics"> - - - - true diff --git a/test/worlds/include.sdf b/test/worlds/include.sdf index ed6f427d9a..4432b9f3c4 100644 --- a/test/worlds/include.sdf +++ b/test/worlds/include.sdf @@ -6,7 +6,7 @@ - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/ground plane/1 + https://fuel.gazebosim.org/1.0/openrobotics/models/ground plane/1 diff --git a/tutorials.md.in b/tutorials.md.in index 9914dd6d0c..9be1fe6bc2 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -33,9 +33,9 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage underwater_vehicles "Underwater Vehicles": Understand how to simulate underwater vehicles. * \subpage particle_emitter "Particle emitter": Using particle emitters in simulation * \subpage blender_sdf_exporter "Blender SDF Exporter": Use a Blender script to export a model to the SDF format. -* \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in Blender for simulations. * \subpage blender_distort_meshes "Blender mesh distortion": Use a Blender Python script to programmatically deform and distort meshes to customized extents. * \subpage blender_procedural_datasets "Generation of Procedural Datasets with Blender": Use Blender with a Python script to generate procedural datasets of SDF models. +* \subpage model_and_optimize_meshes "Model and optimize meshes": Some recomendations when creating meshes in blender for simulations. * \subpage model_command "Model Command": Use the CLI to get information about the models in a simulation. * \subpage test_fixture "Test Fixture": Writing automated CI tests * \subpage model_photo_shoot "Model Photo Shoot" Taking perspective, top, front, and side pictures of a model. diff --git a/tutorials/gui_config.md b/tutorials/gui_config.md index b3c028a531..20cbfbb8f8 100644 --- a/tutorials/gui_config.md +++ b/tutorials/gui_config.md @@ -122,11 +122,11 @@ favorite editor and save this file as `fuel_preview.sdf`: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Gazebo diff --git a/tutorials/mesh_to_fuel.md b/tutorials/mesh_to_fuel.md index 87c26d6b5b..00ee4243c5 100644 --- a/tutorials/mesh_to_fuel.md +++ b/tutorials/mesh_to_fuel.md @@ -156,7 +156,7 @@ Scroll all the way to the bottom of the file until you see the `include` tag sec true Electrical Box 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Electrical Box + https://fuel.gazebosim.org/1.0/openrobotics/models/Electrical Box @@ -171,7 +171,7 @@ Change `Electrical Box` to `Electrical Box Test`. The syntax for including any model from Fuel is: ```xml -https://fuel.ignitionrobotics.org/1.0//models/ +https://fuel.gazebosim.org/1.0//models/ ``` ### Launch World diff --git a/tutorials/server_config.md b/tutorials/server_config.md index 3a0760eead..bbab5f5110 100644 --- a/tutorials/server_config.md +++ b/tutorials/server_config.md @@ -132,11 +132,11 @@ favorite editor and save this file as `fuel_preview.sdf`: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Construction Cone @@ -218,12 +218,12 @@ Let's start by saving this simple world with a camera sensor as - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun 0 0 1 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Gazebo + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Gazebo diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index e7af7f6b71..0e5bfafae0 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -290,3 +290,27 @@ Multiple `` tags can be used as well as with the `` tag. ``` + +Once both boxes have fallen, we can publish a message to invoke a service call +to reset the robot position as well as set the speed to 0. As shown below, the +`` sets the linear x speed to 0, and the `` tag contains +metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The +`reqMsg` is expressed in the human-readable form of Google Protobuf meesages. +Multiple `` tags can be used as well as with the `` tag. + +```xml + + + + linear: {x: 0} + + + + +```
Documentation:" "" - "" - "https://gazebosim.org/libs/gazebo" + "https://gazebosim.org/libs/sim" "" "