Skip to content

Commit

Permalink
Improve contact sensor / visualization performance (#1452)
Browse files Browse the repository at this point in the history 
* add GetByPhysicsEntityId to improve performance of getting contact data

Signed-off-by: Ian Chen <[email protected]>

* remove unused var

Signed-off-by: Ian Chen <[email protected]>
  • Loading branch information
@iche033
iche033 authored Apr 21, 2022
1 parent df62658 commit 9927a26
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 17 deletions.
23 changes: 22 additions & 1 deletion src/systems/physics/EntityFeatureMap.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,6 +203,20 @@ namespace systems::physics_system
return nullptr;
}

/// \brief Get Gazebo entity that corresponds to the physics entity ID
/// \param[in] _id Physics entity ID
/// \return If found, returns the corresponding Gazebo entity. Otherwise,
/// kNullEntity
public: Entity GetByPhysicsId(std::size_t _id) const
{
auto it = this->entityByPhysId.find(_id);
if (it != this->entityByPhysId.end())
{
return it->second;
}
return kNullEntity;
}

/// \brief Check whether there is a physics entity associated with the given
/// Gazebo entity
/// \param[in] _entity Gazebo entity.
Expand Down Expand Up @@ -232,6 +246,7 @@ namespace systems::physics_system
this->entityMap[_entity] = _physicsEntity;
this->reverseMap[_physicsEntity] = _entity;
this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity;
this->entityByPhysId[_physicsEntity->EntityID()] = _entity;
}

/// \brief Remove entity from all associated maps
Expand All @@ -244,6 +259,7 @@ namespace systems::physics_system
{
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;
Expand All @@ -261,6 +277,7 @@ namespace systems::physics_system
{
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;
Expand All @@ -282,7 +299,8 @@ namespace systems::physics_system
public: std::size_t TotalMapEntryCount() const
{
return this->entityMap.size() + this->reverseMap.size() +
this->castCache.size() + this->physEntityById.size();
this->castCache.size() + this->physEntityById.size() +
this->entityByPhysId.size();
}

/// \brief Map from Gazebo entity to physics entities with required features
Expand All @@ -295,6 +313,9 @@ namespace systems::physics_system
/// with required features
private: std::unordered_map<std::size_t, RequiredEntityPtr> physEntityById;

/// \brief Map of physics entity IDs to Gazebo entity
private: std::unordered_map<std::size_t, Entity> entityByPhysId;

/// \brief Cache map from Gazebo entity to physics entities with optional
/// features
private: mutable std::unordered_map<Entity, ValueType> castCache;
Expand Down
22 changes: 14 additions & 8 deletions src/systems/physics/EntityFeatureMap_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,56 +124,62 @@ TEST_F(EntityFeatureMapFixture,

testMap.AddEntity(gazeboWorld1Entity, testWorld1);

// After adding the entity, there should be one entry each in three maps
EXPECT_EQ(3u, testMap.TotalMapEntryCount());
// After adding the entity, there should be one entry each in four maps
EXPECT_EQ(4u, testMap.TotalMapEntryCount());
EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity));
EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1));
EXPECT_EQ(gazeboWorld1Entity, testMap.GetByPhysicsId(testWorld1->EntityID()));

// Cast to optional feature1
auto testWorld1Feature1 =
testMap.EntityCast<TestOptionalFeatures1>(gazeboWorld1Entity);
ASSERT_NE(nullptr, testWorld1Feature1);
// After the cast, there should be one more entry in the cache map.
EXPECT_EQ(4u, testMap.TotalMapEntryCount());
EXPECT_EQ(5u, testMap.TotalMapEntryCount());

// Cast to optional feature2
auto testWorld1Feature2 =
testMap.EntityCast<TestOptionalFeatures2>(gazeboWorld1Entity);
ASSERT_NE(nullptr, testWorld1Feature2);
// After the cast, the number of entries should remain the same because we
// have not added an entity.
EXPECT_EQ(4u, testMap.TotalMapEntryCount());
EXPECT_EQ(5u, testMap.TotalMapEntryCount());

// Add another entity
WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2");
testMap.AddEntity(gazeboWorld2Entity, testWorld2);
EXPECT_EQ(7u, testMap.TotalMapEntryCount());
EXPECT_EQ(9u, testMap.TotalMapEntryCount());
EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity));
EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2));
EXPECT_EQ(gazeboWorld2Entity, testMap.GetByPhysicsId(testWorld2->EntityID()));

auto testWorld2Feature1 =
testMap.EntityCast<TestOptionalFeatures1>(testWorld2);
ASSERT_NE(nullptr, testWorld2Feature1);
// After the cast, there should be one more entry in the cache map.
EXPECT_EQ(8u, testMap.TotalMapEntryCount());
EXPECT_EQ(10u, testMap.TotalMapEntryCount());

auto testWorld2Feature2 =
testMap.EntityCast<TestOptionalFeatures2>(testWorld2);
ASSERT_NE(nullptr, testWorld2Feature2);
// After the cast, the number of entries should remain the same because we
// have not added an entity.
EXPECT_EQ(8u, testMap.TotalMapEntryCount());
EXPECT_EQ(10u, testMap.TotalMapEntryCount());

// Remove entitites
testMap.Remove(gazeboWorld1Entity);
EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity));
EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity));
EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld1));
EXPECT_EQ(4u, testMap.TotalMapEntryCount());
EXPECT_EQ(gazebo::kNullEntity,
testMap.GetByPhysicsId(testWorld1->EntityID()));
EXPECT_EQ(5u, testMap.TotalMapEntryCount());

testMap.Remove(testWorld2);
EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity));
EXPECT_EQ(nullptr, testMap.Get(gazeboWorld2Entity));
EXPECT_EQ(gazebo::kNullEntity, testMap.Get(testWorld2));
EXPECT_EQ(gazebo::kNullEntity, testMap.GetByPhysicsId(
testWorld2->EntityID()));
EXPECT_EQ(0u, testMap.TotalMapEntryCount());
}
17 changes: 9 additions & 8 deletions src/systems/physics/Physics.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3227,15 +3227,16 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm)
// Note that we are temporarily storing pointers to elements in this
// ("allContacts") container. Thus, we must make sure it doesn't get destroyed
// until the end of this function.
auto allContacts = worldCollisionFeature->GetContactsFromLastStep();
auto allContacts =
std::move(worldCollisionFeature->GetContactsFromLastStep());

for (const auto &contactComposite : allContacts)
{
const auto &contact = contactComposite.Get<WorldShapeType::ContactPoint>();
auto coll1Entity =
this->entityCollisionMap.Get(ShapePtrType(contact.collision1));
this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID());
auto coll2Entity =
this->entityCollisionMap.Get(ShapePtrType(contact.collision2));

this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID());

if (coll1Entity != kNullEntity && coll2Entity != kNullEntity)
{
Expand Down Expand Up @@ -3334,10 +3335,10 @@ void PhysicsPrivate::EnableContactSurfaceCustomization(const Entity &_world)
Feature::ContactSurfaceParams<Policy> &_params)
{
const auto &contact = _contact.Get<ContactPoint>();
auto coll1Entity = this->entityCollisionMap.Get(
ShapePtrType(contact.collision1));
auto coll2Entity = this->entityCollisionMap.Get(
ShapePtrType(contact.collision2));
auto coll1Entity = this->entityCollisionMap.GetByPhysicsId(
contact.collision1->EntityID());
auto coll2Entity = this->entityCollisionMap.GetByPhysicsId(
contact.collision2->EntityID());

// check if at least one of the entities wants contact surface
// customization
Expand Down

0 comments on commit 9927a26

Please sign in to comment.