Fix bug where iterator was used after the underlying item was erased from the container

[azeey]

🦟 Bug fix

Fixes #2198

Summary

Reorders the erase operations to avoid using an invalidated iterator. This is already done in the ign-gazebo6 branch:

gz-sim/src/systems/physics/EntityFeatureMap.hh

Lines 255 to 286 in fb3f3d4

	public: bool Remove(Entity _entity)
	{
	auto it = this->entityMap.find(_entity);
	if (it != this->entityMap.end())
	{
	this->reverseMap.erase(it->second);
	this->physEntityById.erase(it->second->EntityID());
	this->entityByPhysId.erase(it->second->EntityID());
	this->castCache.erase(_entity);
	this->entityMap.erase(it);
	return true;
	}
	return false;
	}
	/// \brief Remove physics entity from all associated maps
	/// \param[in] _physicsEntity Physics entity.
	/// \return True if the entity was found and removed.
	public: bool Remove(const RequiredEntityPtr &_physicsEntity)
	{
	auto it = this->reverseMap.find(_physicsEntity);
	if (it != this->reverseMap.end())
	{
	this->entityMap.erase(it->second);
	this->physEntityById.erase(it->first->EntityID());
	this->entityByPhysId.erase(it->first->EntityID());
	this->castCache.erase(it->second);
	this->reverseMap.erase(it);
	return true;
	}
	return false;
	}

Checklist

• Signed all commits for DCO
• Added tests
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[Blast545]

CI seems to be failing unrelated to this PR, LGTM