Merge fa0dfa5 into 5a3b1b8

gazebosim · Jun 15, 2021 · 93c93f8 · 93c93f8
2 parents 5a3b1b8 + fa0dfa5
commit 93c93f8
Show file tree

Hide file tree

Showing 3 changed files with 84 additions and 38 deletions.
diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc
@@ -152,10 +152,6 @@ class ignition::gazebo::systems::WindEffectsPrivate
   /// valid.
   public: bool validConfig{false};
 
-  /// \brief Whether links have been initialized. Initialization involves
-  /// creating components this system needs on links that are affected by wind.
-  public: bool linksInitialized{false};
-
   /// \brief Mutex to protect currWindVelSeed and windEnabled
   public: std::mutex windInfoMutex;
 
@@ -542,45 +538,39 @@ void WindEffects::PreUpdate(const UpdateInfo &_info,
   // Process commands
   this->dataPtr->ProcessCommandQueue(_ecm);
 
-  if (this->dataPtr->validConfig)
+  if (!this->dataPtr->validConfig)
+    return;
+
+  _ecm.EachNew<components::Link, components::WindMode>(
+      [&](const Entity &_entity, components::Link *,
+          components::WindMode *_windMode) -> bool
   {
-    if (!this->dataPtr->linksInitialized)
+    if (_windMode->Data())
     {
-      _ecm.Each<components::Link, components::WindMode>(
-          [&](const Entity &_entity, components::Link *,
-              components::WindMode *_windMode) -> bool
-          {
-            if (_windMode->Data())
-            {
-              // Create a WorldLinearVelocity component on the link so that
-              // physics can populate it
-              if (!_ecm.Component<components::WorldLinearVelocity>(_entity))
-              {
-                _ecm.CreateComponent(_entity,
-                                     components::WorldLinearVelocity());
-              }
-              if (!_ecm.Component<components::WorldPose>(_entity))
-              {
-                _ecm.CreateComponent(_entity, components::WorldPose());
-              }
-            }
-            return true;
-          });
-
-      this->dataPtr->linksInitialized = true;
+      // Create a WorldLinearVelocity component on the link so that
+      // physics can populate it
+      if (!_ecm.Component<components::WorldLinearVelocity>(_entity))
+      {
+        _ecm.CreateComponent(_entity,
+                             components::WorldLinearVelocity());
+      }
+      if (!_ecm.Component<components::WorldPose>(_entity))
+      {
+        _ecm.CreateComponent(_entity, components::WorldPose());
+      }
     }
-    else
-    {
-      if (_info.paused)
-        return;
+    return true;
+  });
 
-      if (!this->dataPtr->currentWindInfo.enable_wind())
-        return;
+  if (_info.paused)
+    return;
+
+  if (!this->dataPtr->currentWindInfo.enable_wind())
+    return;
+
+  this->dataPtr->UpdateWindVelocity(_info, _ecm);
+  this->dataPtr->ApplyWindForce(_info, _ecm);
 
-      this->dataPtr->UpdateWindVelocity(_info, _ecm);
-      this->dataPtr->ApplyWindForce(_info, _ecm);
-    }
-  }
 }
 
 IGNITION_ADD_PLUGIN(WindEffects, System,

diff --git a/test/integration/wind_effects.cc b/test/integration/wind_effects.cc
@@ -326,3 +326,55 @@ TEST_F(WindEffectsTest , TopicsAndServices)
     lastVelMagnitude = velMagnitude;
   }
 }
+
+/// Test if adding a link with wind after first iteration adds
+/// WorldLinearVelocity component properly
+TEST_F(WindEffectsTest, WindEntityAddedAfterStart)
+{
+  const std::string windBox = R"EOF(
+  <?xml version="1.0" ?>
+  <sdf version="1.6">
+      <model name="box_wind">
+        <pose>5 5 5 0 0 0</pose>
+        <enable_wind>true</enable_wind>
+        <link name="test_link_wind">
+          <collision name="collision">
+            <geometry>
+              <box>
+                <size>1 1 1</size>
+              </box>
+            </geometry>
+          </collision>
+        </link>
+      </model>
+  </sdf>)EOF";
+
+  this->StartServer("/test/worlds/wind_effects.sdf");
+
+  LinkComponentRecorder<components::WorldLinearVelocity>
+    linkVelocityComponent("test_link_wind");
+  this->server->AddSystem(linkVelocityComponent.systemPtr);
+  EXPECT_TRUE(linkVelocityComponent.values.empty());
+
+  // Run the logger for a time, check it is still empty
+  this->server->Run(true, 10, false);
+  EXPECT_TRUE(linkVelocityComponent.values.empty());
+
+  // Add the box to be logged via the command system
+  // and check that is not empty
+  transport::Node node;
+  msgs::EntityFactory req;
+  unsigned int timeout = 5000;
+  std::string service{"/world/wind_demo/create"};
+  msgs::Boolean res;
+  bool result;
+
+  req.set_sdf(windBox);
+  EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+  EXPECT_TRUE(result);
+  EXPECT_TRUE(res.data());
+
+  // Now box_wind WorldLinearVelocity component should be added
+  this->server->Run(true, 10, false);
+  ASSERT_FALSE(linkVelocityComponent.values.empty());
+}
diff --git a/test/worlds/wind_effects.sdf b/test/worlds/wind_effects.sdf
@@ -7,6 +7,10 @@
       filename="ignition-gazebo-physics-system"
       name="ignition::gazebo::systems::Physics">
     </plugin>
+    <plugin
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
+    </plugin>
 
     <plugin
       filename="ignition-gazebo-wind-effects-system"