From 5298c545bb66dc78f1154792d1dfcafc5ab22833 Mon Sep 17 00:00:00 2001
From: Alessandro Croci <alessandro.croci@iit.it>
Date: Tue, 9 Apr 2024 14:22:11 +0200
Subject: [PATCH] Update ControlBoardOnMultipleGazeboInstances unit test

- introduced controlboard plugin
- check that DeviceRegistry has the correct number of device keys --> FAILING
---
 .../ControlBoardOnMultipleGazeboInstances.cc  | 37 +++++++++++++++++--
 .../gazebo_controlboard_multiple_joints.ini   |  4 +-
 ...oupled_pendulum_multiple_gz_instances.sdf} |  4 +-
 3 files changed, 38 insertions(+), 7 deletions(-)
 rename tests/controlboard/{coupled_pendulum_no_plugins.sdf => coupled_pendulum_multiple_gz_instances.sdf} (97%)

diff --git a/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc b/tests/controlboard/ControlBoardOnMultipleGazeboInstances.cc
index 3c1cd9d..a1b76d8 100644
--- 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,54 @@ namespace test
 
 TEST(ControlBoardOnMultipleGazeboInstances, StartConcurrentGazeboInstances)
 {
+    std::string deviceScopedName = "model/coupled_pendulum/controlboard_plugin_device";
     auto plannedIterations = 100;
+    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_no_plugins.sdf");
     gz::sim::TestFixture fixture2("../../../tests/controlboard/coupled_pendulum_no_plugins.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 +94,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
index 99a888f..63878b4 100644
--- 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/tests/controlboard/coupled_pendulum_no_plugins.sdf b/tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf
similarity index 97%
rename from tests/controlboard/coupled_pendulum_no_plugins.sdf
rename to tests/controlboard/coupled_pendulum_multiple_gz_instances.sdf
index 48f2d1f..fa155d4 100644
--- a/tests/controlboard/coupled_pendulum_no_plugins.sdf
+++ b/tests/controlboard/coupled_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> -->