Update ControlBoardOnMultipleGazeboInstances unit test

- introduced controlboard plugin - check that DeviceRegistry has the correct number of device keys --> FAILING
robotology · Apr 9, 2024 · bb537e5 · bb537e5
1 parent 38587f9
commit bb537e5
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 9 deletions.
diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc
@@ -1,5 +1,5 @@
 #include <Common.hh>
-#include <Handler.hh>
+#include <DeviceRegistry.hh>
 
 #include <gtest/gtest.h>
 
@@ -33,26 +33,56 @@ namespace test
 
 TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances)
 {
+    std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device";
     auto plannedIterations = 100;
-    gz::sim::TestFixture fixture1("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf");
-    gz::sim::TestFixture fixture2("../../../tests/controlboard/coupled_pendulum_no_plugins.sdf");
+    yarp::dev::PolyDriver* driver1;
+    yarp::dev::PolyDriver* driver2;
+    yarp::dev::IEncoders* iEncoders1 = nullptr;
+    yarp::dev::IEncoders* iEncoders2 = nullptr;
+    double jointPosition1{}, jointPosition2{};
+    gz::sim::TestFixture fixture1("../../../tests/controlboard/"
+                                  "coupled_pendulum_multiple_gz_instances.sdf");
+    gz::sim::TestFixture fixture2("../../../tests/controlboard/"
+                                  "coupled_pendulum_multiple_gz_instances.sdf");
     gz::common::Console::SetVerbosity(4);
 
     fixture1
+        .OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                         const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                         gz::sim::EntityComponentManager& _ecm,
+                         gz::sim::EventManager& /*_eventMgr*/) {
+            driver1 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+            ASSERT_TRUE(driver1 != nullptr);
+            iEncoders1 = nullptr;
+            ASSERT_TRUE(driver1->view(iEncoders1));
+        })
         .OnPreUpdate(
             [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {})
         .OnPostUpdate(
             [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
-                std::cerr << _info.iterations << std::endl;
+                std::cerr << "Iteration: " << _info.iterations << std::endl;
+                iEncoders1->getEncoder(0, &jointPosition1);
+                std::cerr << "Joint position 1: " << jointPosition1 << std::endl;
             })
         .Finalize();
 
     fixture2
+        .OnConfigure([&](const gz::sim::Entity& _worldEntity,
+                         const std::shared_ptr<const sdf::Element>& /*_sdf*/,
+                         gz::sim::EntityComponentManager& _ecm,
+                         gz::sim::EventManager& /*_eventMgr*/) {
+            driver2 = gzyarp::DeviceRegistry::getHandler()->getDevice(deviceScopedName);
+            ASSERT_TRUE(driver2 != nullptr);
+            iEncoders2 = nullptr;
+            ASSERT_TRUE(driver2->view(iEncoders2));
+        })
         .OnPreUpdate(
             [&](const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm) {})
         .OnPostUpdate(
             [&](const gz::sim::UpdateInfo& _info, const gz::sim::EntityComponentManager& _ecm) {
-                std::cerr << _info.iterations << std::endl;
+                std::cerr << "Iteration: " << _info.iterations << std::endl;
+                iEncoders2->getEncoder(0, &jointPosition2);
+                std::cerr << "Joint position 2: " << jointPosition2 << std::endl;
             })
         .Finalize();
 
@@ -66,6 +96,9 @@ TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances)
 
     ASSERT_EQ(fixture1.Server()->IterationCount(), plannedIterations);
     ASSERT_EQ(fixture2.Server()->IterationCount(), plannedIterations);
+
+    // Check that DeviceRegistry has two devices, one for each Gazebo instance
+    ASSERT_EQ(gzyarp::DeviceRegistry::getHandler()->getDevicesKeys().size(), 2);
 }
 
 } // namespace test

diff --git a/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini b/tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
@@ -31,5 +31,5 @@ stictionUp    (0.0)
 stictionDwn   (0.0)
 
 [LIMITS]
-jntPosMax 200.0 10.0
-jntPosMin -200.0 -10.0
+jntPosMax (200.0 10.0)
+jntPosMin (-200.0 -10.0)
diff --git a/...trolboard/coupled_pendulum_no_plugins.sdf → ...oupled_pendulum_multiple_gz_instances.sdf b/...trolboard/coupled_pendulum_no_plugins.sdf → ...oupled_pendulum_multiple_gz_instances.sdf
@@ -159,12 +159,12 @@
         </visual>
       </link>
 
-      <!-- <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
+      <plugin name="gzyarp::ControlBoard" filename="gz-sim-yarp-controlboard-system">
         <yarpConfigurationFile>
           ../../../tests/controlboard/conf/gazebo_controlboard_multiple_joints.ini
         </yarpConfigurationFile>
         <initialConfiguration>0.0</initialConfiguration>
-      </plugin> -->
+      </plugin>
       <!-- <plugin name='robotinterface' filename='libgazebo_yarp_robotinterface.so'>
       <yarpRobotInterfaceConfigurationFile>model://coupled_pendulum/conf/coupled_pendulum_nws.xml</yarpRobotInterfaceConfigurationFile>
       </plugin> -->