Contact system: don't use 'EachNew' in PreUpdate

src/systems/contact/Contact.cc

@@ -85,7 +85,7 @@ class gz::sim::systems::ContactPrivate
 {
   /// \brief Create sensors that correspond to entities in the simulation
   /// \param[in] _ecm Mutable reference to ECM.
-  public: void CreateSensors(EntityComponentManager &_ecm);
+  public: void CreateSensors(const EntityComponentManager &_ecm);
 
   /// \brief Update and publish sensor data
   /// \param[in] _ecm Immutable reference to ECM.
@@ -100,6 +100,11 @@ class gz::sim::systems::ContactPrivate
   /// \brief A map of Contact entity to its Contact sensor.
   public: std::unordered_map<Entity,
       std::unique_ptr<ContactSensor>> entitySensorMap;
+
+  /// \brief Keep list of sensors that were created during the previous
+  /// `PostUpdate`, so that components can be created during the next
+  /// `PreUpdate`.
+  public: std::unordered_set<Entity> newSensors;
 };
 
 //////////////////////////////////////////////////
@@ -154,7 +159,7 @@ void ContactSensor::Publish()
 }
 
 //////////////////////////////////////////////////
-void ContactPrivate::CreateSensors(EntityComponentManager &_ecm)
+void ContactPrivate::CreateSensors(const EntityComponentManager &_ecm)
 {
   GZ_PROFILE("ContactPrivate::CreateSensors");
   _ecm.EachNew<components::ContactSensor>(
@@ -193,10 +198,6 @@ void ContactPrivate::CreateSensors(EntityComponentManager &_ecm)
             // We assume that if childEntities is not empty, it only has one
             // element.
             collisionEntities.push_back(childEntities.front());
-
-            // Create component to be filled by physics.
-            _ecm.CreateComponent(childEntities.front(),
-                                 components::ContactSensorData());
           }
         }
 
@@ -206,6 +207,7 @@ void ContactPrivate::CreateSensors(EntityComponentManager &_ecm)
         sensor->Load(_contact->Data(), defaultTopic, collisionEntities);
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
+        this->newSensors.insert(_entity);
 
         return true;
       });
@@ -218,12 +220,17 @@ void ContactPrivate::UpdateSensors(const UpdateInfo &_info,
   GZ_PROFILE("ContactPrivate::UpdateSensors");
   for (const auto &item : this->entitySensorMap)
   {
+    // Check if any new sensors have been created in the last preUpdate
+    // if yes, omit update for those
+    if (this->newSensors.find(item.first) != this->newSensors.end())
+    {
+        continue;
+    }
+
     for (const Entity &entity : item.second->collisionEntities)
     {
       auto contacts = _ecm.Component<components::ContactSensorData>(entity);
 
-      // We will assume that the ContactData component will have been created if
-      // this entity is in the collisionEntities list
       if (contacts->Data().contact_size() > 0)
       {
         item.second->AddContacts(_info.simTime, contacts->Data());
@@ -263,7 +270,25 @@ Contact::Contact() : System(), dataPtr(std::make_unique<ContactPrivate>())
 void Contact::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm)
 {
   GZ_PROFILE("Contact::PreUpdate");
-  this->dataPtr->CreateSensors(_ecm);
+
+  // Create components
+  for (auto entity : this->dataPtr->newSensors)
+  {
+    auto it = this->dataPtr->entitySensorMap.find(entity);
+    if (it == this->dataPtr->entitySensorMap.end())
+    {
+      gzerr << "Entity [" << entity
+             << "] isn't in sensor map, this shouldn't happen." << std::endl;
+      continue;
+    }
+
+    for(const auto &collisionEntity : it->second->collisionEntities){
+      _ecm.CreateComponent(collisionEntity, components::ContactSensorData());
+    }
+
+  }
+
+  this->dataPtr->newSensors.clear();
 }
 
 //////////////////////////////////////////////////
@@ -280,6 +305,8 @@ void Contact::PostUpdate(const UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
+  this->dataPtr->CreateSensors(_ecm);
+
   if (!_info.paused)
   {
     this->dataPtr->UpdateSensors(_info, _ecm);

src/systems/touch_plugin/TouchPlugin.cc

@@ -115,6 +115,9 @@ class gz::sim::systems::TouchPluginPrivate
   /// \brief Mutex for variables mutated by the service callback.
   /// The variables are: touchPub, touchStart, enabled
   public: std::mutex serviceMutex;
+
+  /// \brief Flag if the first simulation step has already passed
+  public: bool ranOnce{false};
 };
 
 //////////////////////////////////////////////////
@@ -384,7 +387,8 @@ void TouchPlugin::Configure(const Entity &_entity,
 void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm)
 {
   GZ_PROFILE("TouchPlugin::PreUpdate");
-  if ((!this->dataPtr->initialized) && this->dataPtr->sdfConfig)
+
+  if ((!this->dataPtr->initialized) && this->dataPtr->sdfConfig && this->dataPtr->ranOnce)
   {
     // We call Load here instead of Configure because we can't be guaranteed
     // that all entities have been created when Configure is called
@@ -405,6 +409,7 @@ void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm)
         });
     this->dataPtr->AddTargetEntities(_ecm, potentialEntities);
   }
+  this->dataPtr->ranOnce = true;
 }
 
 //////////////////////////////////////////////////