diff --git a/examples/plugin/reset_plugin/CMakeLists.txt b/examples/plugin/reset_plugin/CMakeLists.txt new file mode 100644 index 0000000000..51412f097d --- /dev/null +++ b/examples/plugin/reset_plugin/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(ResetPlugins) + +find_package(ignition-plugin2 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin2_VERSION_MAJOR}) + +find_package(ignition-gazebo7 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo7_VERSION_MAJOR}) + +add_library(JointPositionRandomizer SHARED JointPositionRandomizer.cc) +target_link_libraries(JointPositionRandomizer + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::core) diff --git a/examples/plugin/reset_plugin/JointPositionRandomizer.cc b/examples/plugin/reset_plugin/JointPositionRandomizer.cc new file mode 100644 index 0000000000..1f53bfac49 --- /dev/null +++ b/examples/plugin/reset_plugin/JointPositionRandomizer.cc @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ignition; +using namespace gazebo; +namespace reset_plugin +{ +class JointPositionRandomizer : public System, + public ISystemConfigure, + public ISystemReset +{ + void Configure(const Entity &_entity, + const std::shared_ptr &, + EntityComponentManager &, EventManager &) override + { + this->targetEntity = _entity; + } + + void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) override + { + std::cout << "Reset joints\n"; + auto joints = _ecm.EntitiesByComponents( + components::ParentEntity(this->targetEntity), components::Joint()); + + for (auto joint : joints) + { + auto jointType = _ecm.Component(joint); + + double pos = 0.0; + + if (jointType->Data() == sdf::JointType::PRISMATIC) + { + pos = math::Rand::DblUniform(-0.8, 0.1); + std::cout << "prismatic joint (" << joint + << ") pos: (" << pos << " m)"<< std::endl; + } + else if (jointType->Data() == sdf::JointType::REVOLUTE) + { + pos = math::Rand::DblUniform(0, IGN_PI); + std::cout << "revolute joint (" << joint + << ") pos: (" << pos << " rad)"<< std::endl; + } + _ecm.SetComponentData(joint, {pos}); + } + } + + private: Entity targetEntity; +}; +} // namespace reset_plugin + +IGNITION_ADD_PLUGIN(reset_plugin::JointPositionRandomizer, + ignition::gazebo::System, + reset_plugin::JointPositionRandomizer::ISystemConfigure, + reset_plugin::JointPositionRandomizer::ISystemReset) diff --git a/examples/plugin/reset_plugin/README.md b/examples/plugin/reset_plugin/README.md new file mode 100644 index 0000000000..9a1c25b207 --- /dev/null +++ b/examples/plugin/reset_plugin/README.md @@ -0,0 +1,34 @@ +# System Reset API + +This example uses the JointPositionRandomizer system to randomize the joint +positions of a robot arm at every reset. + + +## Build + +~~~ +cd examples/plugin/reset_plugin +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `JointPositionRandomizer` library under `build`. + +## Run + +Add the library to the path: + +~~~ +cd examples/plugin/reset_plugin +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then run a world that loads the plugin as follows: + + ign gazebo -r -v 4 joint_position_randomizer.sdf + +In another terminal, run the following to reset the world. + + ign service -s /world/default/control --reqtype ignition.msgs.WorldControl --reptype ignition.msgs.Boolean --timeout 3000 --req 'reset: {all: true}' diff --git a/examples/plugin/reset_plugin/joint_position_randomizer.sdf b/examples/plugin/reset_plugin/joint_position_randomizer.sdf new file mode 100644 index 0000000000..0d2055fdb5 --- /dev/null +++ b/examples/plugin/reset_plugin/joint_position_randomizer.sdf @@ -0,0 +1,44 @@ + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Simple Arm + + + + diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 7a7a6ff171..03c4da2d77 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -3174,6 +3174,7 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(1, foundEntities); } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, CopyEcm) { Entity entity = manager.CreateEntity(); @@ -3195,6 +3196,7 @@ TEST_P(EntityComponentManagerFixture, CopyEcm) }); } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, ComputeDiff) { Entity entity1 = manager.CreateEntity(); @@ -3243,6 +3245,7 @@ TEST_P(EntityComponentManagerFixture, ComputeDiff) } } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) { Entity entity1 = manager.CreateEntity(); @@ -3293,6 +3296,7 @@ TEST_P(EntityComponentManagerFixture, ResetToWithDeletedEntity) } } +////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, ResetToWithAddedEntity) { Entity entity1 = manager.CreateEntity(); diff --git a/src/systems/physics/CanonicalLinkModelTracker.hh b/src/systems/physics/CanonicalLinkModelTracker.hh index 4f6f01443e..1e2fdd10e9 100644 --- a/src/systems/physics/CanonicalLinkModelTracker.hh +++ b/src/systems/physics/CanonicalLinkModelTracker.hh @@ -58,6 +58,10 @@ namespace systems::physics_system /// \param[in] _ecm EntityComponentManager public: void AddNewModels(const EntityComponentManager &_ecm); + /// \brief Save mappings for all models and their canonical links + /// \param[in] _ecm EntityComponentManager + public: void AddAllModels(const EntityComponentManager &_ecm); + /// \brief Get a topological ordering of models that have a particular /// canonical link /// \param[in] _canonicalLink The canonical link @@ -79,7 +83,7 @@ namespace systems::physics_system /// \brief An empty set of models that is returned from the /// CanonicalLinkModels method for links that map to no models - private: const std::set emptyModelOrdering{}; + private: static inline const std::set kEmptyModelOrdering{}; }; void CanonicalLinkModelTracker::AddNewModels( @@ -93,6 +97,17 @@ namespace systems::physics_system return true; }); } + void CanonicalLinkModelTracker::AddAllModels( + const EntityComponentManager &_ecm) + { + _ecm.Each( + [this](const Entity &_model, const components::Model *, + const components::ModelCanonicalLink *_canonicalLinkComp) + { + this->linkModelMap[_canonicalLinkComp->Data()].insert(_model); + return true; + }); + } const std::set &CanonicalLinkModelTracker::CanonicalLinkModels( const Entity _canonicalLink) const @@ -102,7 +117,7 @@ namespace systems::physics_system return it->second; // if an invalid entity was given, it maps to no models - return this->emptyModelOrdering; + return this->kEmptyModelOrdering; } void CanonicalLinkModelTracker::RemoveLink(const Entity &_link) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 094fafa3f1..987ef773a1 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -191,27 +191,44 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Create physics entities /// \param[in] _ecm Constant reference to ECM. - public: void CreatePhysicsEntities(const EntityComponentManager &_ecm); + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreatePhysicsEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create world entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateWorldEntities(const EntityComponentManager &_ecm); - + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreateWorldEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create model entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateModelEntities(const EntityComponentManager &_ecm); + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreateModelEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create link entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateLinkEntities(const EntityComponentManager &_ecm); + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreateLinkEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create collision entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateCollisionEntities(const EntityComponentManager &_ecm); + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreateCollisionEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create joint entities /// \param[in] _ecm Constant reference to ECM. - public: void CreateJointEntities(const EntityComponentManager &_ecm); + /// \param[in] _warnIfEntityExists True to emit warnings if the same entity + /// already exists in the physics system + public: void CreateJointEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists = true); /// \brief Create Battery entities /// \param[in] _ecm Constant reference to ECM. @@ -225,6 +242,10 @@ class ignition::gazebo::systems::PhysicsPrivate /// \param[in] _ecm Mutable reference to ECM. public: void UpdatePhysics(EntityComponentManager &_ecm); + /// \brief Reset physics from components + /// \param[in] _ecm Constant reference to ECM. + public: void ResetPhysics(EntityComponentManager &_ecm); + /// \brief Step the simulation for each world /// \param[in] _dt Duration /// \returns Output data from the physics engine (this currently contains @@ -838,14 +859,6 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Physics::Update"); - // \TODO(anyone) Support rewind - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - ignwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - if (this->dataPtr->engine) { this->dataPtr->CreatePhysicsEntities(_ecm); @@ -867,23 +880,37 @@ void Physics::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm) +void Physics::Reset(const UpdateInfo &, EntityComponentManager &_ecm) +{ + IGN_PROFILE("Physics::Reset"); + + if (this->dataPtr->engine) + { + igndbg << "Resetting Physics\n"; + this->dataPtr->ResetPhysics(_ecm); + } +} + +////////////////////////////////////////////////// +void PhysicsPrivate::CreatePhysicsEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { // Clear the set of links that were added to a model. this->linkAddedToModel.clear(); this->jointAddedToModel.clear(); - this->CreateWorldEntities(_ecm); - this->CreateModelEntities(_ecm); - this->CreateLinkEntities(_ecm); + this->CreateWorldEntities(_ecm, _warnIfEntityExists); + this->CreateModelEntities(_ecm, _warnIfEntityExists); + this->CreateLinkEntities(_ecm, _warnIfEntityExists); // We don't need to add visuals to the physics engine. - this->CreateCollisionEntities(_ecm); - this->CreateJointEntities(_ecm); + this->CreateCollisionEntities(_ecm, _warnIfEntityExists); + this->CreateJointEntities(_ecm, _warnIfEntityExists); this->CreateBatteryEntities(_ecm); } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { // Get all the new worlds _ecm.EachNew( @@ -895,9 +922,12 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) // Check if world already exists if (this->entityWorldMap.HasEntity(_entity)) { - ignwarn << "World entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "World entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -964,7 +994,8 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNew( @@ -980,9 +1011,12 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) // Check if model already exists if (this->entityModelMap.HasEntity(_entity)) { - ignwarn << "Model entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Model entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } // TODO(anyone) Don't load models unless they have collisions @@ -1099,7 +1133,8 @@ void PhysicsPrivate::CreateModelEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNew( @@ -1124,9 +1159,12 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) // Check if link already exists if (this->entityLinkMap.HasEntity(_entity)) { - ignwarn << "Link entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Link entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1169,7 +1207,8 @@ void PhysicsPrivate::CreateLinkEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNewentityCollisionMap.HasEntity(_entity)) { - ignwarn << "Collision entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Collision entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1389,7 +1431,8 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) +void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm, + bool _warnIfEntityExists) { _ecm.EachNewentityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Joint entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1501,9 +1547,12 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) // Check if joint already exists if (this->entityJointMap.HasEntity(_entity)) { - ignwarn << "Joint entity [" << _entity - << "] marked as new, but it's already on the map." - << std::endl; + if (_warnIfEntityExists) + { + ignwarn << "Joint entity [" << _entity + << "] marked as new, but it's already on the map." + << std::endl; + } return true; } @@ -1663,6 +1712,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) } } this->entityLinkMap.Remove(childLink); + this->entityFreeGroupMap.Remove(childLink); this->topLevelModelMap.erase(childLink); this->staticEntities.erase(childLink); this->linkWorldPoses.erase(childLink); @@ -2388,6 +2438,131 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) } // NOLINT readability/fn_size // TODO (azeey) Reduce size of function and remove the NOLINT above +////////////////////////////////////////////////// +void PhysicsPrivate::ResetPhysics(EntityComponentManager &_ecm) +{ + IGN_PROFILE("PhysicsPrivate::ResetPhysics"); + // Clear worldPoseCmdsToRemove because pose commands that were issued before + // the reset will be ignored. + this->linkWorldPoses.clear(); + this->canonicalLinkModelTracker = CanonicalLinkModelTracker(); + this->modelWorldPoses.clear(); + this->worldPoseCmdsToRemove.clear(); + + this->CreatePhysicsEntities(_ecm, false); + this->canonicalLinkModelTracker.AddAllModels(_ecm); + + // Update link pose, linear velocity, and angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *) + { + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + { + ignwarn << "Failed to find link [" << _entity << "]." << std::endl; + return true; + } + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + if (freeGroup->RootLink() == linkPtrPhys) + { + auto linkWorldPose = worldPose(_entity, _ecm); + freeGroup->SetWorldPose(math::eigen3::convert(linkWorldPose)); + } + + auto worldAngularVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to reset link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be reset." + << std::endl; + informed = true; + } + return true; + } + else + { + worldAngularVelFeature->SetWorldAngularVelocity( + Eigen::Vector3d::Zero()); + } + + auto worldLinearVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + else + { + worldLinearVelFeature->SetWorldLinearVelocity( + Eigen::Vector3d::Zero()); + } + + return true; + }); + + // Handle joint state + _ecm.Each( + [&](const Entity &_entity, const components::Joint *) + { + auto jointPhys = this->entityJointMap.Get(_entity); + if (nullptr == jointPhys) + { + ignwarn << "Failed to find joint [" << _entity << "]." << std::endl; + return true; + } + + // Assume initial joint position and velocities are zero + // Reset the velocity + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) + { + jointPhys->SetVelocity(i, 0.0); + jointPhys->SetPosition(i, 0.0); + } + + return true; + }); + + // Also update modelWorldPoses. This is a workaround to the problem that we + // don't have a way to reset the physics engine and clear its internal cache + // of link poses. In the event that a model's canonical link's pose hasn't + // changed after reset, the parent model's world pose won't be recorded in + // the modelWorldPoses map. If any of the model's other links have changed, + // however, we try to look for the parent model's world pose in + // modelWorldPoses and fail. So the workaround here is to update the world + // poses of all models. + _ecm.Each( + [&](const Entity &_entity, const components::Model *) + { + this->modelWorldPoses[_entity] = gazebo::worldPose(_entity, _ecm); + return true; + }); + + this->RemovePhysicsEntities(_ecm); +} + ////////////////////////////////////////////////// ignition::physics::ForwardStep::Output PhysicsPrivate::Step( const std::chrono::steady_clock::duration &_dt) @@ -3425,6 +3600,7 @@ void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) IGNITION_ADD_PLUGIN(Physics, ignition::gazebo::System, Physics::ISystemConfigure, + Physics::ISystemReset, Physics::ISystemUpdate) IGNITION_ADD_PLUGIN_ALIAS(Physics, "ignition::gazebo::systems::Physics") diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index 98ebde22d5..c43b7ecbd5 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -80,6 +80,7 @@ namespace systems class Physics: public System, public ISystemConfigure, + public ISystemReset, public ISystemUpdate { /// \brief Constructor @@ -94,7 +95,11 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; - /// Documentation inherited + // Documentation inherited + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + // Documentation inherited public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2f8ec87d78..339bee21be 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -52,6 +52,7 @@ set(tests pose_publisher_system.cc rf_comms.cc recreate_entities.cc + reset.cc save_world.cc scene_broadcaster_system.cc sdf_frame_semantics.cc diff --git a/test/integration/reset.cc b/test/integration/reset.cc new file mode 100644 index 0000000000..bc7f669f01 --- /dev/null +++ b/test/integration/reset.cc @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/EventManager.hh" +#include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/test_config.hh" + +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; +namespace components = ignition::gazebo::components; + +////////////////////////////////////////////////// +class ResetFixture: public InternalFixture<::testing::Test> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + auto plugin = sm.LoadPlugin("libMockSystem.so", + "ignition::gazebo::MockSystem", + nullptr); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: ignition::gazebo::SystemPluginPtr systemPtr; + public: gazebo::MockSystem *mockSystem; + + private: gazebo::SystemLoader sm; +}; + +///////////////////////////////////////////////// +void worldReset() +{ + ignition::msgs::WorldControl req; + ignition::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + transport::Node node; + + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request("/world/default/control", req, timeout, rep, result); + + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +///////////////////////////////////////////////// +/// This test checks that that the physics system handles cases where entities +/// are removed and then added back +TEST_F(ResetFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(HandleReset)) +{ + ignition::gazebo::ServerConfig serverConfig; + + const std::string sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "reset.sdf"); + + serverConfig.SetSdfFile(sdfFile); + + sdf::Root root; + root.Load(sdfFile); + gazebo::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + gazebo::EntityComponentManager *ecm = nullptr; + + this->mockSystem->configureCallback = + [&ecm](const Entity &, + const std::shared_ptr &, + EntityComponentManager &_ecm, + EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + // Verify initial conditions of the world + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(0u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(0u, this->mockSystem->updateCallCount); + EXPECT_EQ(0u, this->mockSystem->postUpdateCallCount); + } + + // Run so that things will happen in the world + // In this case, the box should fall some + server.Run(true, 100, false); + { + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_LT(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(100u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(100u, this->mockSystem->updateCallCount); + EXPECT_EQ(100u, this->mockSystem->postUpdateCallCount); + } + + // Validate update info in the reset + this->mockSystem->resetCallback = + [](const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &) + { + EXPECT_EQ(0u, _info.iterations); + EXPECT_EQ(std::chrono::steady_clock::duration{0}, _info.simTime); + }; + + // Send command to reset to initial state + worldReset(); + + // It takes two iterations for this to propagate, + // the first is for the message to be received and internal state setup + server.Run(true, 1, false); + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(0u, this->mockSystem->resetCallCount); + EXPECT_EQ(101u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(101u, this->mockSystem->updateCallCount); + EXPECT_EQ(101u, this->mockSystem->postUpdateCallCount); + + // The second iteration is where the reset actually occurs. + server.Run(true, 1, false); + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_FLOAT_EQ(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + + // These should not increment, because only reset is called + EXPECT_EQ(101u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(101u, this->mockSystem->updateCallCount); + EXPECT_EQ(101u, this->mockSystem->postUpdateCallCount); + } + + server.Run(true, 100, false); + { + ASSERT_NE(nullptr, ecm); + auto entity = ecm->EntityByComponents(components::Name("box")); + ASSERT_NE(kNullEntity, entity); + auto poseComp = ecm->Component(entity); + ASSERT_NE(nullptr, poseComp); + EXPECT_LT(poseComp->Data().Z(), 1.1f); + + EXPECT_EQ(1u, this->mockSystem->configureCallCount); + EXPECT_EQ(1u, this->mockSystem->resetCallCount); + EXPECT_EQ(201u, this->mockSystem->preUpdateCallCount); + EXPECT_EQ(201u, this->mockSystem->updateCallCount); + EXPECT_EQ(201u, this->mockSystem->postUpdateCallCount); + } +} diff --git a/test/plugins/MockSystem.cc b/test/plugins/MockSystem.cc index d7602c5c38..e8c72e9c25 100644 --- a/test/plugins/MockSystem.cc +++ b/test/plugins/MockSystem.cc @@ -4,6 +4,7 @@ IGNITION_ADD_PLUGIN(ignition::gazebo::MockSystem, ignition::gazebo::System, ignition::gazebo::MockSystem::ISystemConfigure, + ignition::gazebo::MockSystem::ISystemReset, ignition::gazebo::MockSystem::ISystemPreUpdate, ignition::gazebo::MockSystem::ISystemUpdate, ignition::gazebo::MockSystem::ISystemPostUpdate) diff --git a/test/plugins/MockSystem.hh b/test/plugins/MockSystem.hh index b4fd804bb7..6705e7eee6 100644 --- a/test/plugins/MockSystem.hh +++ b/test/plugins/MockSystem.hh @@ -26,6 +26,7 @@ namespace ignition { class MockSystem : public gazebo::System, public gazebo::ISystemConfigure, + public gazebo::ISystemReset, public gazebo::ISystemPreUpdate, public gazebo::ISystemUpdate, public gazebo::ISystemPostUpdate @@ -33,6 +34,7 @@ namespace ignition { public: MockSystem() = default; public: ~MockSystem() = default; public: size_t configureCallCount {0}; + public: size_t resetCallCount {0}; public: size_t preUpdateCallCount {0}; public: size_t updateCallCount {0}; public: size_t postUpdateCallCount {0}; @@ -49,6 +51,8 @@ namespace ignition { EntityComponentManager &, EventManager &)> configureCallback; + + public: CallbackType resetCallback; public: CallbackType preUpdateCallback; public: CallbackType updateCallback; public: CallbackTypeConst postUpdateCallback; @@ -63,6 +67,14 @@ namespace ignition { this->configureCallback(_entity, _sdf, _ecm, _eventMgr); } + public: void Reset(const gazebo::UpdateInfo &_info, + gazebo::EntityComponentManager &_ecm) override final + { + ++this->resetCallCount; + if (this->resetCallback) + this->resetCallback(_info, _ecm); + } + public: void PreUpdate(const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) override final { diff --git a/test/worlds/reset.sdf b/test/worlds/reset.sdf new file mode 100644 index 0000000000..10c6c63426 --- /dev/null +++ b/test/worlds/reset.sdf @@ -0,0 +1,43 @@ + + + + + + + + + + + + 0 + + + + 0 0 1.1 0 0 0 + false + + + + 5 5 2.5 + + + + + 5 5 2.5 + + + 0 1 0 0.8 + 0 1 0 0.8 + 1 1 1 0.8 + + + + + +