diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index fa33557129..97b0ba1a74 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -601,6 +601,17 @@ namespace ignition public: std::unordered_set ComponentTypesWithPeriodicChanges() const; + /// \brief Get a cache of components with periodic changes. + /// \param[inout] _changes A list of components with the latest periodic + /// changes. If a component has a periodic change, it is added to the + /// hash map. It the component or entity was removed, it is removed from + /// the hashmap. This way the hashmap stores a list of components and + /// entities which have had periodic changes in the past and still + /// exist within the ECM. + /// \sa EntityComponentManager::PeriodicStateFromCache + public: void UpdatePeriodicChangeCache(std::unordered_map>&) const; + /// \brief Set the absolute state of the ECM from a serialized message. /// Entities / components that are in the new state but not in the old /// one will be created. @@ -628,6 +639,19 @@ namespace ignition const std::unordered_set &_types = {}, bool _full = false) const; + /// \brief Populate a message with relevant changes to the state given + /// a periodic change cache. + /// \details The header of the message will not be populated, it is the + /// responsibility of the caller to timestamp it before use. Additionally, + /// changes such as addition or removal will not be populated. + /// \param[inout] _state The serialized state message to populate. + /// \param[in] _cache A map of entities and components to serialize. + /// \sa EntityComponenetManager::UpdatePeriodicChangeCache + public: void PeriodicStateFromCache( + msgs::SerializedStateMap &_state, + const std::unordered_map> &_cache) const; + /// \brief Get a message with the serialized state of all entities and /// components that are changing in the current iteration /// diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index d768971aa4..c03c4b911b 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -963,6 +963,42 @@ std::unordered_set return periodicComponents; } +///////////////////////////////////////////////// +void EntityComponentManager::UpdatePeriodicChangeCache( + std::unordered_map> &_changes) const +{ + // Get all changes + for (const auto &[componentType, entities] : + this->dataPtr->periodicChangedComponents) + { + _changes[componentType].insert( + entities.begin(), entities.end()); + } + + // Get all removed components + for (const auto &[entity, components] : + this->dataPtr->componentsMarkedAsRemoved) + { + for (const auto &comp : components) + { + _changes[comp].erase(entity); + } + } + + // Get all removed entities + for (const auto &entity : this->dataPtr->toRemoveEntities) { + for ( + auto components = _changes.begin(); + components != _changes.end(); components++) { + // Its ok to leave component entries empty, the serialization + // code will simply ignore it. In any case the number of components + // is limited, so this cache will never grow too large. + components->second.erase(entity); + } + } +} + ///////////////////////////////////////////////// bool EntityComponentManager::HasEntity(const Entity _entity) const { @@ -1678,6 +1714,48 @@ void EntityComponentManager::State( }); } +////////////////////////////////////////////////// +void EntityComponentManager::PeriodicStateFromCache( + msgs::SerializedStateMap &_state, + const std::unordered_map> &_cache) const +{ + for (auto &[typeId, entities] : _cache) { + // Serialize components that have changed + for (auto &entity : entities) { + // Add entity to message if it does not exist + auto entIter = _state.mutable_entities()->find(entity); + if (entIter == _state.mutable_entities()->end()) + { + msgs::SerializedEntityMap ent; + ent.set_id(entity); + (*_state.mutable_entities())[static_cast(entity)] = ent; + entIter = _state.mutable_entities()->find(entity); + } + + // Find the component in the message + auto compIter = entIter->second.mutable_components()->find(typeId); + if (compIter != entIter->second.mutable_components()->end()) + { + // If the component is present we don't need to update it. + continue; + } + + auto compIdx = this->dataPtr->componentTypeIndex[entity][typeId]; + auto &comp = this->dataPtr->componentStorage[entity][compIdx]; + + // Add the component to the message + msgs::SerializedComponent cmp; + cmp.set_type(comp->TypeId()); + std::ostringstream ostr; + comp->Serialize(ostr); + cmp.set_component(ostr.str()); + (*(entIter->second.mutable_components()))[ + static_cast(typeId)] = cmp; + } + } +} + ////////////////////////////////////////////////// void EntityComponentManager::SetState( const msgs::SerializedState &_stateMsg) diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 28ff2083b6..56cc5fe363 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2204,6 +2204,72 @@ TEST_P(EntityComponentManagerFixture, Descendants) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + IGN_UTILS_TEST_DISABLED_ON_WIN32(UpdatePeriodicChangeCache)) +{ + Entity e1 = manager.CreateEntity(); + auto c1 = manager.CreateComponent(e1, IntComponent(123)); + + std::unordered_map> changeTracker; + + // No periodic changes keep cache empty. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 0u); + + // Create a periodic change. + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + + // 1 periodic change, should be reflected in cache. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + + manager.RunSetAllComponentsUnchanged(); + + // Has periodic change. Cache should be full. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 1u); + + // Serialize state + msgs::SerializedStateMap state; + manager.PeriodicStateFromCache(state, changeTracker); + EXPECT_EQ(state.entities().size(), 1u); + EXPECT_EQ( + state.entities().find(e1)->second.components().size(), 1u); + EXPECT_NE(state.entities().find(e1)->second + .components().find(c1->TypeId()), + state.entities().find(e1)->second.components().end()); + + // Component removed cache should be updated. + manager.RemoveComponent(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 0u); + + manager.RunSetAllComponentsUnchanged(); + + // Add another component to the entity + auto c2 = manager.CreateComponent(e1, IntComponent(123)); + manager.UpdatePeriodicChangeCache(changeTracker); + + // Cache does not track additions, only PeriodicChanges + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); + + // Track change + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 1u); + + // Entity removed cache should be updated. + manager.RequestRemoveEntity(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(SetChanged)) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 96911d7776..9ec4d0433a 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -57,6 +57,9 @@ class ignition::gazebo::systems::JointPositionControllerPrivate /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; + /// \brief Is the maximum PID gain set. + public: bool isMaxSet {false}; + /// \brief Model interface public: Model model{kNullEntity}; @@ -149,6 +152,7 @@ void JointPositionController::Configure(const Entity &_entity, if (_sdf->HasElement("cmd_max")) { cmdMax = _sdf->Get("cmd_max"); + this->dataPtr->isMaxSet = true; } if (_sdf->HasElement("cmd_min")) { @@ -306,14 +310,14 @@ void JointPositionController::PreUpdate( auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; // Limit the maximum change to maxMovement - if (abs(error) > maxMovement) + if (abs(error) > maxMovement && this->dataPtr->isMaxSet) { targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : -this->dataPtr->posPid.CmdMax(); } else { - targetVel = -error; + targetVel = - error / dt; } // Set velocity and return diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 80256b936c..0d986c2567 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -257,8 +258,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief Flag used to indicate if periodic changes need to be published /// This is currently only used in playback mode. public: bool pubPeriodicChanges{false}; + + /// \brief Stores a cache of components that are changed. (This prevents + /// dropping of periodic change components which may not be updated + /// frequently enough) + public: std::unordered_map> changedComponents; }; + ////////////////////////////////////////////////// SceneBroadcaster::SceneBroadcaster() : System(), dataPtr(std::make_unique()) @@ -341,6 +349,9 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // removed entities are removed from the scene graph for the next update cycle this->dataPtr->SceneGraphRemoveEntities(_manager); + // Iterate through entities and their changes to cache them. + _manager.UpdatePeriodicChangeCache(this->dataPtr->changedComponents); + // Publish state only if there are subscribers and // * throttle rate to 60 Hz // * also publish off-rate if there are change events: @@ -383,15 +394,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, else if (!_info.paused) { IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState"); - - if (_manager.HasPeriodicComponentChanges()) - { - auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); - _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, periodicComponents); - this->dataPtr->pubPeriodicChanges = false; - } - else + if (!_manager.HasPeriodicComponentChanges()) { // log files may be recorded at lower rate than sim time step. So in // playback mode, the scene broadcaster may not see any periodic @@ -416,6 +419,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // we may be able to remove this in the future and update tests this->dataPtr->stepMsg.mutable_state(); } + + // Apply changes that were caught by the periodic state tracker and then + // clear the change tracker. + _manager.PeriodicStateFromCache(*this->dataPtr->stepMsg.mutable_state(), + this->dataPtr->changedComponents); + this->dataPtr->changedComponents.clear(); } // Full state on demand diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index f91788334a..fbc850e14a 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -56,8 +56,8 @@ TEST_F(JointPositionControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller.sdf"; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "/test/worlds/joint_position_controller.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -133,8 +133,8 @@ TEST_F(JointPositionControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller_velocity.sdf"; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "joint_position_controller_velocity.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -170,8 +170,8 @@ TEST_F(JointPositionControllerTestFixture, const components::Name *_name, const components::JointPosition *_position) -> bool { - EXPECT_EQ(_name->Data(), jointName); - currentPosition = _position->Data(); + if (_name->Data() == jointName) + currentPosition = _position->Data(); return true; }); }); @@ -184,7 +184,7 @@ TEST_F(JointPositionControllerTestFixture, EXPECT_NEAR(0, currentPosition.at(0), TOL); // joint moves to initial_position at -2.0 - const std::size_t initPosIters = 1000; + const std::size_t initPosIters = 1; server.Run(true, initPosIters, false); double expectedInitialPosition = -2.0; EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL); @@ -200,10 +200,108 @@ TEST_F(JointPositionControllerTestFixture, pub.Publish(msg); // Wait for the message to be published - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(1ms); - const std::size_t testIters = 1000; - server.Run(true, testIters , false); + const std::size_t testIters = 1; + server.Run(true, testIters, false); EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } + + +///////////////////////////////////////////////// +// Tests that the JointPositionController respects the maximum command +TEST_F(JointPositionControllerTestFixture, + IGN_UTILS_TEST_DISABLED_ON_WIN32(JointPositonVelocityCommandWithMax)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "joint_position_controller_velocity.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j2"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + if(_name->Data() == jointName) + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // joint pos starts at 0 + const std::size_t initIters = 1; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // joint moves to initial_position at -2.0 + const std::size_t initPosIters = 2; + server.Run(true, initPosIters, false); + double expectedInitialPosition = -2.0; + EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/joint_position_controller_test_with_max/joint/j2/0/cmd_pos"); + + const double targetPosition{2.0}; + msgs::Double msg; + msg.set_data(targetPosition); + + int sleep{0}; + int maxSleep{30}; + for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) { + std::this_thread::sleep_for(100ms); + } + + pub.Publish(msg); + + // Wait for the message to be published + std::this_thread::sleep_for(1ms); + + const std::size_t testInitialIters = 1; + server.Run(true, testInitialIters , false); + + // We should not have reached our target yet. + EXPECT_GT(fabs(currentPosition.at(0) - targetPosition), TOL); + + // Eventually reach target + const std::size_t testIters = 1000; + server.Run(true, testIters , false); + EXPECT_NEAR(currentPosition.at(0), targetPosition, TOL); +} diff --git a/test/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf index b7f5fdc774..cf3a9c5080 100644 --- a/test/worlds/joint_position_controller_velocity.sdf +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -111,6 +111,85 @@ name="ignition::gazebo::systems::JointPositionController"> j1 true + -2.0 + + + + + 100 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + j2 + true 1000 -2.0