Test no z axis influence measurement

gazebosim · Nov 29, 2023 · 8746414 · 8746414
1 parent fc32fbe
commit 8746414
Showing 1 changed file with 116 additions and 0 deletions.
diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc
@@ -345,3 +345,119 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveWindy))
   EXPECT_NEAR(10.04987562, msg.xy_speed(), 1e-3);
   EXPECT_NEAR(-0.0996686524911, msg.xy_direction(), 1e-2);
 }
+
+/////////////////////////////////////////////////
+// The test checks that the sensor does not include airflow in its z-axis
+// in its measurement
+TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveZAxisWindy))
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "air_flow_windy.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  const std::string sensorName = "air_flow_sensor";
+
+  auto topic = "world/air_flow_sensor/model/air_flow_model/link/link/"
+      "sensor/air_flow_sensor/air_flow";
+
+  bool updateChecked{false};
+
+  // Create a system that checks sensor topic
+  TestFixture testSystem(serverConfig);
+
+  Link body;
+  Model model;
+
+
+  testSystem.OnConfigure(
+    [&](const Entity &_worldEntity,
+      const std::shared_ptr<const sdf::Element> &/*_sdf*/,
+      EntityComponentManager &_ecm,
+      EventManager &/*eventMgr*/)
+    {
+        World world(_worldEntity);
+        auto modelEntity =  world.ModelByName(_ecm, "air_flow_model");
+        EXPECT_NE(modelEntity, kNullEntity);
+        model = Model(modelEntity);
+
+        auto bodyEntity = model.LinkByName(_ecm, "link");
+        EXPECT_NE(bodyEntity, kNullEntity);
+
+        body = Link(bodyEntity);
+        body.EnableVelocityChecks(_ecm);
+
+        body.SetLinearVelocity(_ecm, gz::math::Vector3d(0, 0, -1));
+
+    }).OnPostUpdate([&](const UpdateInfo &_info,
+                              const EntityComponentManager &_ecm)
+      {
+
+        _ecm.Each<components::AirFlowSensor, components::Name>(
+            [&](const Entity &_entity,
+                const components::AirFlowSensor *,
+                const components::Name *_name) -> bool
+            {
+              EXPECT_EQ(_name->Data(), sensorName);
+
+              auto sensorComp = _ecm.Component<components::Sensor>(_entity);
+              EXPECT_NE(nullptr, sensorComp);
+
+              if (_info.iterations == 1)
+                return true;
+
+              // This component is created on the 2nd PreUpdate
+              auto topicComp = _ecm.Component<components::SensorTopic>(_entity);
+              EXPECT_NE(nullptr, topicComp);
+              if (topicComp)
+              {
+                EXPECT_EQ(topic, topicComp->Data());
+              }
+
+              updateChecked = true;
+
+              return true;
+            });
+      }).Finalize();
+
+  // Subscribe to air_flow topic
+  bool received{false};
+  msgs::AirFlowSensor msg;
+  msg.Clear();
+  std::function<void(const msgs::AirFlowSensor &)>  cb =
+      [&received, &msg](const msgs::AirFlowSensor &_msg)
+  {
+
+    msg = _msg;
+    received = true;
+  };
+
+  transport::Node node;
+  node.Subscribe(topic, cb);
+
+
+  testSystem.Server()->Run(true, 10, false);
+  // Run server
+  // server.Run(true, 10, false);
+  EXPECT_TRUE(updateChecked);
+
+
+  // Wait for message to be received
+  for (int sleep = 0; sleep < 30; ++sleep)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
+
+  EXPECT_TRUE(received);
+
+  // check air flow
+  EXPECT_TRUE(msg.has_header());
+  EXPECT_TRUE(msg.header().has_stamp());
+  EXPECT_NEAR(5.0, msg.xy_speed(), 1e-3);
+  EXPECT_NEAR(0.0, msg.xy_direction(), 1e-2);
+}