From e75a33db452c13ab4de4d26af80884e0c3eb92b4 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 25 Jan 2021 10:47:01 +0100
Subject: [PATCH 01/17] First draft of gazebo_yarp_robotinterface plugin

---
 CMakeLists.txt                                |  15 +-
 .../include/GazeboYarpPlugins/Handler.hh      |  17 +-
 libraries/singleton/src/Handler.cc            |  11 ++
 plugins/CMakeLists.txt                        |   4 +
 plugins/robotinterface/CMakeLists.txt         |  12 ++
 plugins/robotinterface/README.md              |   5 +
 .../gazebo/GazeboYarpRobotInterface.hh        |  37 ++++
 .../src/GazeboYarpRobotInterface.cc           | 106 ++++++++++
 tests/CMakeLists.txt                          |   6 +-
 tests/robotinterface/CMakeLists.txt           |  14 ++
 .../RobotInterfaceControlBoardConfig.ini      |  41 ++++
 tests/robotinterface/RobotInterfaceTest.cc    | 110 +++++++++++
 tests/robotinterface/RobotInterfaceTest.world | 186 ++++++++++++++++++
 tests/robotinterface/RobotInterfaceTest.xml   |  21 ++
 14 files changed, 577 insertions(+), 8 deletions(-)
 create mode 100644 plugins/robotinterface/CMakeLists.txt
 create mode 100644 plugins/robotinterface/README.md
 create mode 100644 plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
 create mode 100644 plugins/robotinterface/src/GazeboYarpRobotInterface.cc
 create mode 100644 tests/robotinterface/CMakeLists.txt
 create mode 100644 tests/robotinterface/RobotInterfaceControlBoardConfig.ini
 create mode 100644 tests/robotinterface/RobotInterfaceTest.cc
 create mode 100644 tests/robotinterface/RobotInterfaceTest.world
 create mode 100644 tests/robotinterface/RobotInterfaceTest.xml

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 44708dc63..8f9425aa8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,7 @@ PROJECT(GazeboYARPPlugins)
 # Project version
 set(${PROJECT_NAME}_MAJOR_VERSION 3)
 set(${PROJECT_NAME}_MINOR_VERSION 5)
-set(${PROJECT_NAME}_PATCH_VERSION 1)
+set(${PROJECT_NAME}_PATCH_VERSION 100)
 
 set(${PROJECT_NAME}_VERSION
     ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION})
@@ -17,6 +17,12 @@ set(${PROJECT_NAME}_VERSION
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html
 include(GNUInstallDirs)
 
+# Build all the plugins in the same directory to simplify running tests
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+
+
 #
 option(BUILD_TESTING "Create tests using CMake" OFF)
 if(BUILD_TESTING)
@@ -26,6 +32,7 @@ endif()
 # Finding dependencies
 find_package(OpenCV QUIET)
 option(GAZEBO_YARP_PLUGINS_HAS_OPENCV "Compile plugins that depend on OpenCV" ${OpenCV_FOUND})
+option(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE "Compile plugins that depend on libYARP_robotinterface" ON)
 
 if(GAZEBO_YARP_PLUGINS_HAS_OPENCV)
     find_package(OpenCV REQUIRED)
@@ -34,7 +41,11 @@ else()
     set(YARP_ADDITIONAL_COMPONENTS_REQUIRED "")
 endif()
 
-find_package(YARP 3.2.102 REQUIRED COMPONENTS os sig dev math ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
+if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
+    list(APPEND YARP_ADDITIONAL_COMPONENTS_REQUIRED "robotinterface")
+endif()
+
+find_package(YARP 3.4 REQUIRED COMPONENTS os sig dev math robotinterface ${YARP_ADDITIONAL_COMPONENTS_REQUIRED})
 find_package(Gazebo REQUIRED)
 if (Gazebo_VERSION_MAJOR LESS 7.0)
     message(status "Gazebo version : " ${Gazebo_VERSION_MAJOR}.${Gazebo_VERSION_MINOR}.${Gazebo_VERSION_PATCH})
diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index 73ca7dd9c..6aabf5250 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -37,11 +37,8 @@ namespace gazebo
     }
 }
 
-namespace yarp {
-    namespace dev {
-        class PolyDriver;
-    }
-}
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/PolyDriverList.h>
 
 namespace GazeboYarpPlugins {
 
@@ -121,6 +118,16 @@ public:
      */
     void removeDevice(const std::string& deviceName);
 
+    /** 
+     * \brief Returns a list of the opened devices
+     * \note This list acts just as a view of the available devices, 
+     *       and it does not transfer or share ownership of the devices.
+     *       The consumer code needs to make sure that the driver lifetime
+     *       is longer then the consumer lifetime.
+     */
+    void getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list);
+    
+
     /** Destructor
      */
     ~Handler();
diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc
index e5340994e..f53735896 100644
--- a/libraries/singleton/src/Handler.cc
+++ b/libraries/singleton/src/Handler.cc
@@ -228,4 +228,15 @@ void Handler::removeDevice(const std::string& deviceName)
     return;
 }
 
+void Handler::getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list)
+{
+    for(auto&& devicesMapElem: m_devicesMap) {
+        yDebug() << "DEBUG TO REMOVE: Add device to " << devicesMapElem.first;
+        list.push(devicesMapElem.second.object(), devicesMapElem.first.c_str());
+    }
+
+    return;
+}
+
+
 }
diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
index 3a8e6489a..11ee008a4 100644
--- a/plugins/CMakeLists.txt
+++ b/plugins/CMakeLists.txt
@@ -40,3 +40,7 @@ add_subdirectory(contactloadcellarray)
 add_subdirectory(modelposepublisher)
 add_subdirectory(basestate)
 add_subdirectory(configurationoverride)
+
+if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
+  add_subdirectory(robotinterface)
+endif()
\ No newline at end of file
diff --git a/plugins/robotinterface/CMakeLists.txt b/plugins/robotinterface/CMakeLists.txt
new file mode 100644
index 000000000..ef2245c6b
--- /dev/null
+++ b/plugins/robotinterface/CMakeLists.txt
@@ -0,0 +1,12 @@
+# Copyright (C) Fondazione Istituto Italiano di Tecnologia
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+
+include(AddGazeboYarpPluginTarget)
+
+add_gazebo_yarp_plugin_target(LIBRARY_NAME robotinterface
+                              INCLUDE_DIRS include/gazebo
+                              SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}  ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
+                              LINKED_LIBRARIES gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}
+                              HEADERS include/gazebo/GazeboYarpRobotInterface.hh 
+                              SOURCES src/GazeboYarpRobotInterface.cc)
diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md
new file mode 100644
index 000000000..049ab6ea6
--- /dev/null
+++ b/plugins/robotinterface/README.md
@@ -0,0 +1,5 @@
+gazebo_yarp_robotinterface
+==========================
+
+The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices]() that can be attached to YARP devices 
+already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface).
\ No newline at end of file
diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
new file mode 100644
index 000000000..c00264fbf
--- /dev/null
+++ b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) Fondazione Istituto Italiano di Tecnologia 
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
+ */
+
+#ifndef GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH
+#define GAZEBOYARP_ROBOTINTERFACEPLUGIN_HH
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/common/Time.hh>
+
+#include <yarp/robotinterface/experimental/XMLReader.h>
+
+namespace gazebo
+{
+    class GazeboYarpRobotInterface;
+}
+
+/**
+ * See gazebo-yarp-plugins/plugins/robotinterface/README.md for documentation on this plugin.
+ *
+ */
+class gazebo::GazeboYarpRobotInterface : public gazebo::ModelPlugin
+{
+public:
+    GazeboYarpRobotInterface();
+    virtual ~GazeboYarpRobotInterface();
+    
+    void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+
+private:
+    yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader;
+    yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult;
+};
+
+
+#endif
diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
new file mode 100644
index 000000000..e90de0c50
--- /dev/null
+++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
@@ -0,0 +1,106 @@
+/*
+ * Copyright (C) 2013-2015 Fondazione Istituto Italiano di Tecnologia RBCS & iCub Facility & ADVR
+ * Authors: see AUTHORS file.
+ * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
+ */
+
+#include "GazeboYarpRobotInterface.hh"
+#include <GazeboYarpPlugins/Handler.hh>
+#include <GazeboYarpPlugins/common.h>
+
+#include <gazebo/physics/Model.hh>
+
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Network.h>
+
+namespace gazebo
+{
+
+GazeboYarpRobotInterface::GazeboYarpRobotInterface()
+{
+    std::cerr << "GazeboYarpRobotInterface constructor" << std::endl;
+}
+
+GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
+{
+    // Close robotinterface 
+    bool ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1);
+    if (!ok) {
+        yError() << "GazeboYarpRobotInterface: impossible to run phase ActionPhaseInterrupt1 robotinterface";
+    }
+    ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown);
+    if (!ok) {
+        yError() << "GazeboYarpRobotInterface: impossible  to run phase ActionPhaseShutdown in robotinterface";
+    }
+
+    yarp::os::Network::fini();
+}
+
+void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::ElementPtr _sdf)
+{
+    yarp::os::Network::init();
+
+    if (!yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
+        yError() << "GazeboYarpRobotInterface : yarp network does not seem to be available, is the yarpserver running?";
+        return;
+    }
+
+    if (!_parentModel) {
+        gzerr << "GazeboYarpRobotInterface plugin requires a parent.\n";
+        return;
+    }
+
+    GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parentModel));
+
+    // Getting .xml and loading configuration file from sdf
+    bool loaded_configuration = false;
+    if (_sdf->HasElement("yarpRobotInterfaceConfigurationFile"))
+    {
+        std::string robotinterface_file_name = _sdf->Get<std::string>("yarpRobotInterfaceConfigurationFile");
+        std::string robotinterface_file_path = gazebo::common::SystemPaths::Instance()->FindFileURI(robotinterface_file_name);
+
+        if (robotinterface_file_name == "") {
+            yError() << "GazeboYarpRobotInterface error: failure in finding robotinterface configuration for model" << _parentModel->GetName() << "\n"
+                      << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n"
+                      << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path;
+            loaded_configuration = false;
+        } else {
+            m_xmlRobotInterfaceResult = m_xmlRobotInterfaceReader.getRobotFromFile(robotinterface_file_path);
+
+            if (m_xmlRobotInterfaceResult.parsingIsSuccessful) {
+                loaded_configuration = true;
+            } else {
+                yError() << "GazeboYarpRobotInterface error: failure in loading robotinterface configuration for model" << _parentModel->GetName() << "\n"
+                      << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n"
+                      << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path;
+                loaded_configuration = false; 
+            }
+        }
+    }
+
+    if (!loaded_configuration) {
+        yError() << "GazeboYarpRobotInterface : xml file specified in yarpRobotInterfaceConfigurationFile not found or not loaded.";
+        return;
+    }
+
+    // Extract externalDriverList of  devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
+    yarp::dev::PolyDriverList externalDriverList;
+    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(externalDriverList);
+
+    // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
+    bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList);
+    if (!ok) {
+        yError() << "GazeboYarpRobotInterface : impossible to set external devices";
+    }
+
+    // Start robotinterface 
+    ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseStartup);
+    if (!ok) {
+        yError() << "GazeboYarpRobotInterface : impossible to start robotinterface";
+    }
+}
+
+
+// Register this plugin with the simulator
+GZ_REGISTER_MODEL_PLUGIN(GazeboYarpRobotInterface)
+}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 4d2bdfa13..314471426 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -33,4 +33,8 @@ target_compile_definitions(ControlBoardControlTest PRIVATE -DCMAKE_CURRENT_BINAR
 target_compile_definitions(ControlBoardControlTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$<TARGET_FILE_DIR:gazebo_yarp_controlboard>")
 add_test(NAME ControlBoardControlTest COMMAND ControlBoardControlTest)
 # Ensure that YARP devices can be found
-set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL})
\ No newline at end of file
+set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL})
+
+if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
+  add_subdirectory(robotinterface)
+endif()
\ No newline at end of file
diff --git a/tests/robotinterface/CMakeLists.txt b/tests/robotinterface/CMakeLists.txt
new file mode 100644
index 000000000..d6102faa4
--- /dev/null
+++ b/tests/robotinterface/CMakeLists.txt
@@ -0,0 +1,14 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+
+add_executable(RobotInterfaceTest RobotInterfaceTest.cc)
+target_include_directories(RobotInterfaceTest PUBLIC ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(RobotInterfaceTest PUBLIC ${GAZEBO_LIBRARIES} ${GAZEBO_TEST_LIB} gyp-gazebo-classic-gtest YARP::YARP_dev YARP::YARP_os ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES} gazebo_yarp_lib_common gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
+target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+target_compile_definitions(RobotInterfaceTest PRIVATE -DCMAKE_CURRENT_BINARY_DIR="${CMAKE_CURRENT_BINARY_DIR}")
+target_compile_definitions(RobotInterfaceTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$<TARGET_FILE_DIR:gazebo_yarp_robotinterface>")
+add_test(NAME RobotInterfaceTest COMMAND RobotInterfaceTest)
+# Ensure that YARP devices can be found
+# Disable use of online model database
+set_property(TEST RobotInterfaceTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=)
\ No newline at end of file
diff --git a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
new file mode 100644
index 000000000..6d719c3d4
--- /dev/null
+++ b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
@@ -0,0 +1,41 @@
+[WRAPPER]
+# name of the wrapper device to be instatiated by the factory
+device controlboardwrapper2
+# rate of output streaming from ports in ms
+period 10
+# output port name 
+name /pendulumGazebo/body
+# Total number of joints
+joints 1
+# list of MotorControl device to use
+networks ( pendulum_controlboard )
+# for each network specify the joint map
+pendulum  0 0 0 0
+# Verbose output (on if present, off if commented out)
+# verbose
+
+# Specify configuration of MotorControl devices
+[pendulum_controlboard]
+# name of the device to be instatiated by the factory
+device gazebo_controlboard
+#jointNames list
+jointNames upper_joint
+name pendulum
+
+#PIDs:
+
+[POSITION_CONTROL]
+controlUnits  metric_units
+controlLaw    joint_pid_gazebo_v1
+kp            20.0
+kd            0.122 
+ki            0.003
+maxInt        9999 
+maxOutput     9999
+shift         0.0
+ko            0.0
+stictionUp    0.0
+stictionDwn   0.0
+
+[VELOCITY_CONTROL]
+velocityControlImplementationType integrator_and_position_pid
diff --git a/tests/robotinterface/RobotInterfaceTest.cc b/tests/robotinterface/RobotInterfaceTest.cc
new file mode 100644
index 000000000..3bfff2613
--- /dev/null
+++ b/tests/robotinterface/RobotInterfaceTest.cc
@@ -0,0 +1,110 @@
+/*
+ * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia
+ *
+ * Licensed under either the GNU Lesser General Public License v3.0 :
+ * https://www.gnu.org/licenses/lgpl-3.0.html
+ * or the GNU Lesser General Public License v2.1 :
+ * https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html
+ * at your option.
+ */
+
+#include <gazebo/test/ServerFixture.hh>
+#include <gazebo/test/helper_physics_generator.hh>
+
+#include <yarp/dev/Drivers.h>
+#include <yarp/dev/IControlMode.h>
+#include <yarp/dev/IEncoders.h>
+#include <yarp/dev/IPositionControl.h>
+#include <yarp/dev/IVelocityControl.h>
+#include <yarp/dev/PolyDriver.h>
+
+class RobotInterfaceTest : public gazebo::ServerFixture,
+                                        public testing::WithParamInterface<const char*>
+{
+  struct PluginTestHelperOptions
+  {
+  };
+
+  public: gazebo::event::ConnectionPtr updateConnection;
+
+  public: void PluginTest(const std::string &_physicsEngine);
+  public: void PluginTestHelper(const std::string &_physicsEngine,
+                                const std::string &worldName,
+                                const PluginTestHelperOptions& options);
+};
+
+void RobotInterfaceTest::PluginTestHelper(const std::string &_physicsEngine,
+                                             const std::string &worldName,
+                                             const PluginTestHelperOptions& options)
+{
+  bool worldPaused = true;
+  std::string worldAbsPath = CMAKE_CURRENT_SOURCE_DIR"/" + worldName;
+  Load(worldAbsPath, worldPaused, _physicsEngine);
+
+  gazebo::physics::WorldPtr world = gazebo::physics::get_world("default");
+  ASSERT_TRUE(world != NULL);
+
+  // Verify physics engine type
+  gazebo::physics::PhysicsEnginePtr physics = world->Physics();
+  ASSERT_TRUE(physics != NULL);
+  EXPECT_EQ(physics->GetType(), _physicsEngine);
+
+  gzdbg << "RobotInterfaceTest: testing world " << worldName << " with physics engine " << _physicsEngine << std::endl;
+
+  // Run a few step of simulation to ensure that YARP plugin start correctly
+  world->Step(10);
+
+  // Try to connect to the model controlboard using the wrapper opened by the 
+  // gazebo_yarp_robotinterface plugin
+  yarp::os::Property option;
+  yarp::dev::PolyDriver        driver;
+  yarp::dev::IEncoders        *ienc = 0;
+  option.put("device","remote_controlboard");
+  option.put("remote","/pendulumGazebo/openedByTheRobotInterface");
+  option.put("local","/RobotInterfaceTest");
+
+  ASSERT_TRUE(driver.open(option));
+
+  // open the views
+  ASSERT_TRUE(driver.view(ienc));
+
+  // retrieve number of axes
+  int nAxes = 0;
+  ienc->getAxes(&nAxes);
+  EXPECT_EQ(nAxes, 1);
+
+  // Unload the simulation
+  Unload();
+}
+
+/////////////////////////////////////////////////////////////////////
+void RobotInterfaceTest::PluginTest(const std::string &_physicsEngine)
+{
+  // Make sure that the YARP network does not require yarpserver running
+  yarp::os::NetworkBase::setLocalMode(true);
+
+  // Defined by CMake
+  std::string pluginDir = GAZEBO_YARP_PLUGINS_DIR;
+  gazebo::common::SystemPaths::Instance()->AddPluginPaths(pluginDir);
+  std::string modelDir = CMAKE_CURRENT_SOURCE_DIR;
+  gazebo::common::SystemPaths::Instance()->AddModelPaths(modelDir);
+
+  PluginTestHelperOptions options;
+  this->PluginTestHelper(_physicsEngine, "RobotInterfaceTest.world", options);
+}
+
+TEST_P(RobotInterfaceTest, PluginTest)
+{
+  PluginTest(GetParam());
+}
+
+// Only test for ode
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, RobotInterfaceTest, ::testing::Values("ode"));
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/tests/robotinterface/RobotInterfaceTest.world b/tests/robotinterface/RobotInterfaceTest.world
new file mode 100644
index 000000000..79e1984db
--- /dev/null
+++ b/tests/robotinterface/RobotInterfaceTest.world
@@ -0,0 +1,186 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+<world name="default">
+  <include>
+    <uri>model://ground_plane</uri>
+  </include>
+  <include>
+    <uri>model://sun</uri>
+  </include>
+  <!-- Based on https://bitbucket.org/osrf/gazebo_models/src/default/double_pendulum_with_base/model.sdf -->
+  <model name="pendulum_with_base">
+    <link name="base">
+      <inertial>
+        <mass>100</mass>
+      </inertial>
+      <visual name="vis_plate_on_ground">
+        <pose>0 0 0.01 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.8</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="vis_pole">
+        <pose>-0.275 0 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.2 0.2 2.2</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+        </material>
+      </visual>
+      <collision name="col_plate_on_ground">
+        <pose>0 0 0.01 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.8</radius>
+            <length>0.02</length>
+          </cylinder>
+        </geometry>
+      </collision>
+      <collision name="col_pole">
+        <pose>-0.275 0 1.1 0 0 0</pose>
+        <geometry>
+          <box>
+            <size>0.2 0.2 2.2</size>
+          </box>
+        </geometry>
+      </collision>
+    </link>
+    <!-- upper link, length 1, IC -90 degrees -->
+    <link name="upper_link">
+      <pose>0 0 2.1 -1.5708 0 0</pose>
+      <self_collide>0</self_collide>
+      <inertial>
+        <pose>0 0 0.5 0 0 0</pose>
+        <inertia>
+          <ixx>0.01</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.01</iyy>
+          <iyz>0</iyz>
+          <izz>0.01</izz>
+        </inertia>
+        <mass>1.0</mass>
+      </inertial>
+
+      <visual name="vis_upper_joint">
+        <pose>-0.05 0 0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="vis_lower_joint">
+        <pose>0 0 1.0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.2</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+        </material>
+      </visual>
+      <visual name="vis_cylinder">
+        <pose>0 0 0.5 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.9</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/Grey</name>
+          </script>
+        </material>
+      </visual>
+      <collision name="col_upper_joint">
+        <pose>-0.05 0 0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.3</length>
+          </cylinder>
+        </geometry>
+      </collision>
+      <collision name="col_lower_joint">
+        <pose>0 0 1.0 0 1.5708 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.2</length>
+          </cylinder>
+        </geometry>
+      </collision>
+      <collision name="col_cylinder">
+        <pose>0 0 0.5 0 0 0</pose>
+        <geometry>
+          <cylinder>
+            <radius>0.1</radius>
+            <length>0.9</length>
+          </cylinder>
+        </geometry>
+      </collision>
+    </link>
+    <!-- pin joint for upper link, at origin of upper link -->
+    <joint name="upper_joint" type="revolute">
+      <parent>base</parent>
+      <child>upper_link</child>
+      <axis>
+        <xyz>1.0 0 0</xyz>
+        <limit>
+          <lower>-100</lower>
+          <upper>100</upper>
+          <effort>1000.0</effort>
+          <velocity>50</velocity>
+        </limit>
+        <dynamics>
+          <damping>4</damping>
+          <friction>0</friction>
+          <spring_reference>0</spring_reference>
+          <spring_stiffness>0</spring_stiffness>
+        </dynamics>
+      </axis>
+
+    </joint>
+    <!-- gazebo_yarp_controlboard plugin -->
+    <plugin name="controlboard" filename="libgazebo_yarp_controlboard.so">
+        <!-- This is found because the source directory is passed to GAZEBO_MODEL_PATH -->
+        <yarpConfigurationFile>model://RobotInterfaceControlBoardConfig.ini</yarpConfigurationFile>
+        <initialConfiguration>0.0</initialConfiguration>
+    </plugin>
+    <!-- gazebo_yarp_robotinterface plugin -->
+    <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
+        <yarpRobotInterfaceConfigurationFile>model://RobotInterfaceTest.xml</yarpRobotInterfaceConfigurationFile>
+    </plugin>
+  </model>
+</world>
+</sdf>
diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
new file mode 100644
index 000000000..b8af4a8c0
--- /dev/null
+++ b/tests/robotinterface/RobotInterfaceTest.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+<robot name="robotinterface_additional_controlboard_nws" prefix="robotinterface_additional_controlboard_nws">
+  <devices>
+    <device name="additional_controlboard_nws" type="controlboardwrapper2">
+      <param name="period"> 10 </param>
+      <param name="name"> /pendulumGazebo/openedByTheRobotInterface</param>
+      <paramlist name="networks">
+        <elem name="FirstSetOfJoints">(  0  0  0  0 )</elem>
+      </paramlist>
+      <param name="joints"> 1                        </param>
+      <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+          <!-- This should match the name of the device in the .ini file loaded in the Gazebo plugin, prefied by the Gazebo model name -->
+          <elem name="pendulum_with_base::pendulum">  pendulum_with_base::pendulum </elem>
+        </paramlist>
+      </action>
+      <action phase="shutdown" level="5" type="detach" />
+    </device>
+  </devices>
+</robot>

From e778fd30d6fec90f93a6c6d2cf485525b8f8b60c Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 1 Feb 2021 19:20:19 +0100
Subject: [PATCH 02/17] Fix .ini conf file

---
 tests/robotinterface/RobotInterfaceControlBoardConfig.ini | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
index 6d719c3d4..cd2c60c15 100644
--- a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
+++ b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
@@ -10,7 +10,7 @@ joints 1
 # list of MotorControl device to use
 networks ( pendulum_controlboard )
 # for each network specify the joint map
-pendulum  0 0 0 0
+pendulum_controlboard  (0 0 0 0)
 # Verbose output (on if present, off if commented out)
 # verbose
 

From d7c83e9c63b1d7cc081743bba05f54e272208233 Mon Sep 17 00:00:00 2001
From: Marco Randazzo <marco.randazzo@iit.it>
Date: Tue, 2 Feb 2021 09:54:24 +0000
Subject: [PATCH 03/17] cfg files fixed

---
 tests/robotinterface/RobotInterfaceControlBoardConfig.ini | 2 +-
 tests/robotinterface/RobotInterfaceTest.xml               | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
index cd2c60c15..a15fdcf75 100644
--- a/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
+++ b/tests/robotinterface/RobotInterfaceControlBoardConfig.ini
@@ -20,7 +20,7 @@ pendulum_controlboard  (0 0 0 0)
 device gazebo_controlboard
 #jointNames list
 jointNames upper_joint
-name pendulum
+name pendulum_aaa
 
 #PIDs:
 
diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
index b8af4a8c0..4eb862ff1 100644
--- a/tests/robotinterface/RobotInterfaceTest.xml
+++ b/tests/robotinterface/RobotInterfaceTest.xml
@@ -12,7 +12,7 @@
       <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
           <!-- This should match the name of the device in the .ini file loaded in the Gazebo plugin, prefied by the Gazebo model name -->
-          <elem name="pendulum_with_base::pendulum">  pendulum_with_base::pendulum </elem>
+          <elem name="FirstSetOfJoints">  pendulum_with_base::pendulum_aaa </elem>
         </paramlist>
       </action>
       <action phase="shutdown" level="5" type="detach" />

From 8dfb0d58f202a06aed4cf36cf6803a00ac1bdfb5 Mon Sep 17 00:00:00 2001
From: Marco Randazzo <marco.randazzo@iit.it>
Date: Tue, 2 Feb 2021 10:49:21 +0000
Subject: [PATCH 04/17] added yarpDevicename parameter to DepthCamera,
 laserSensor and DoubleLaser

---
 plugins/depthCamera/src/DepthCamera.cc | 19 +++++++++++++++++++
 plugins/doublelaser/src/DoubleLaser.cc | 20 +++++++++++++++++++-
 plugins/lasersensor/src/LaserSensor.cc | 22 +++++++++++++++++++++-
 3 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/plugins/depthCamera/src/DepthCamera.cc b/plugins/depthCamera/src/DepthCamera.cc
index 11a594173..9eb0bbab0 100644
--- a/plugins/depthCamera/src/DepthCamera.cc
+++ b/plugins/depthCamera/src/DepthCamera.cc
@@ -120,6 +120,25 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     {
         yError() << "GazeboYarpDepthCamera : error in connecting wrapper and device ";
     }
+    
+    //Register the device with the given name
+    if(!m_driverParameters.check("yarpDeviceName"))
+    {
+        yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file.";
+        //return;
+    }
+    else
+    {
+        std::string sensorName = _sensor->ScopedName();
+        std::string deviceId = m_driverParameters.find("yarpDeviceName").asString();
+        std::string scopedDeviceName = sensorName + "::" + deviceId; 
+                
+        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_cameraDriver))
+        {
+            yError()<<"GazeboYarpDepthCamera: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+            return;
+        }
+    }
 }
 
 }
diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc
index f9ab9b180..9ffdc54cd 100644
--- a/plugins/doublelaser/src/DoubleLaser.cc
+++ b/plugins/doublelaser/src/DoubleLaser.cc
@@ -261,7 +261,25 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
 
         //Insert the pointer in the singleton handler for retrieving it in the yarp driver
         GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
-
+        
+        // 9) Register the device with the given name
+        if(!doublelaser_dev_parameters.check("yarpDeviceName"))
+        {
+           yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file.";
+           //return;
+        }
+        else
+        {
+            std::string robotName = _parent->GetScopedName();
+            std::string deviceId = doublelaser_dev_parameters.find("yarpDeviceName").asString();
+            std::string scopedDeviceName = robotName + "::" + deviceId; 
+             
+            if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
+            {
+               yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+               return;
+            }
+        }
      }
 
 }
diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc
index 6b8f1c7de..cf2c118ef 100644
--- a/plugins/lasersensor/src/LaserSensor.cc
+++ b/plugins/lasersensor/src/LaserSensor.cc
@@ -98,18 +98,38 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
         return;
     }
 
+    //Register the device with the given name
+#if 0
     if(!driver_properties.check("deviceId"))
     {
         yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file.";
         return;
     }
     std::string deviceId = driver_properties.find("deviceId").asString();
-
     if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
     {
         yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
         return;
     }
+#else
+    if(!driver_properties.check("yarpDeviceName"))
+    {
+       yError()<<"GazeboYarpLaserSensor: cannot find yarpDeviceName parameter in ini file.";
+       //return;
+    }
+    else
+    {
+        std::string sensorName = _sensor->ScopedName();
+        std::string deviceId = driver_properties.find("yarpDeviceName").asString();
+        std::string scopedDeviceName = sensorName + "::" + deviceId; 
+
+        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_laserDriver))
+        {
+           yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
+           return;
+        }
+    }
+#endif
 
     //Attach the driver to the wrapper
     ::yarp::dev::PolyDriverList driver_list;

From f51984f16c92f61117a3db7f78f1acd552bd3e87 Mon Sep 17 00:00:00 2001
From: Marco Randazzo <marco.randazzo@iit.it>
Date: Tue, 2 Feb 2021 13:14:19 +0000
Subject: [PATCH 05/17] minor fix

---
 plugins/doublelaser/src/DoubleLaser.cc |  5 +++--
 plugins/lasersensor/src/LaserSensor.cc | 23 ++++++++++++++---------
 2 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/plugins/doublelaser/src/DoubleLaser.cc b/plugins/doublelaser/src/DoubleLaser.cc
index 9ffdc54cd..abb3a2756 100644
--- a/plugins/doublelaser/src/DoubleLaser.cc
+++ b/plugins/doublelaser/src/DoubleLaser.cc
@@ -263,7 +263,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
         GazeboYarpPlugins::Handler::getHandler()->setRobot(get_pointer(_parent));
         
         // 9) Register the device with the given name
-        if(!doublelaser_dev_parameters.check("yarpDeviceName"))
+        if(!m_parameters.check("yarpDeviceName"))
         {
            yError()<<"GazeboYarpDoubleLaser: cannot find yarpDeviceName parameter in ini file.";
            //return;
@@ -271,7 +271,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
         else
         {
             std::string robotName = _parent->GetScopedName();
-            std::string deviceId = doublelaser_dev_parameters.find("yarpDeviceName").asString();
+            std::string deviceId = m_parameters.find("yarpDeviceName").asString();
             std::string scopedDeviceName = robotName + "::" + deviceId; 
              
             if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(scopedDeviceName, &m_driver_doublelaser))
@@ -279,6 +279,7 @@ GZ_REGISTER_MODEL_PLUGIN(GazeboYarpDoubleLaser)
                yError()<<"GazeboYarpDoubleLaser: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
                return;
             }
+            //yDebug() << "GazeboYarpDoubleLaser: register device:" << scopedDeviceName;
         }
      }
 
diff --git a/plugins/lasersensor/src/LaserSensor.cc b/plugins/lasersensor/src/LaserSensor.cc
index cf2c118ef..d1b5227b6 100644
--- a/plugins/lasersensor/src/LaserSensor.cc
+++ b/plugins/lasersensor/src/LaserSensor.cc
@@ -99,19 +99,23 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     }
 
     //Register the device with the given name
-#if 0
+//#if 0
+    //this block will be soon deprecated
     if(!driver_properties.check("deviceId"))
     {
         yError()<<"GazeboYarpLaserSensor Plugin failed: cannot find deviceId parameter in ini file.";
-        return;
     }
-    std::string deviceId = driver_properties.find("deviceId").asString();
-    if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
+    else
     {
-        yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
-        return;
-    }
-#else
+        yError() << "GazeboYarpLaserSensor: deviceId parameter has been deprecated. Please use yarpDeviceName instead";
+        std::string deviceId = driver_properties.find("deviceId").asString();
+        if(!GazeboYarpPlugins::Handler::getHandler()->setDevice(deviceId, &m_laserDriver))
+        {
+           yError()<<"GazeboYarpLaserSensor: failed setting deviceId(=" << deviceId << ")";
+           return;
+        }
+   }
+//#else
     if(!driver_properties.check("yarpDeviceName"))
     {
        yError()<<"GazeboYarpLaserSensor: cannot find yarpDeviceName parameter in ini file.";
@@ -128,8 +132,9 @@ void GazeboYarpLaserSensor::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
            yError()<<"GazeboYarpLaserSensor: failed setting scopedDeviceName(=" << scopedDeviceName << ")";
            return;
         }
+        //yDebug() << "GazeboYarpLaserSensor: registered device:" << scopedDeviceName;
     }
-#endif
+//#endif
 
     //Attach the driver to the wrapper
     ::yarp::dev::PolyDriverList driver_list;

From 640e9e45ab20b965f660955157d346507621fb6f Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 9 Feb 2021 19:26:49 +0100
Subject: [PATCH 06/17] Cleanup handling of yarpDeviceName in robotinterface
 plugin

---
 .../include/GazeboYarpPlugins/Handler.hh      | 38 +++++++---
 libraries/singleton/src/Handler.cc            | 75 +++++++++++++++----
 .../src/GazeboYarpRobotInterface.cc           |  6 +-
 tests/robotinterface/RobotInterfaceTest.xml   |  6 +-
 4 files changed, 100 insertions(+), 25 deletions(-)

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index 6aabf5250..32e12300c 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -100,23 +100,35 @@ public:
 
     /** \brief Adds a new yarp device pointer to the "database".
      *
-     *  If the device already exists and the pointer are the same return success, if pointers doesn't match returns error.
-     * \param deviceName the name of the device to be added to the internal database
+     * The YARP device are stored in these database using the following schema:
+     *  * For YARP devices created by Model plugins, the deviceDatabaseKey is 
+     *    defined as deviceDatabaseKey = Model::GetScopedName() + "::" + yarpDeviceName
+     *  * For YARP devices created by Sensor plugins, the deviceDatabaseKey is 
+     *    defined as deviceDatabaseKey = Sensor::GetScopedName() + "::" + yarpDeviceName
+     *
+     * yarpDeviceName is a non-scoped identifier of the specific instance of the YARP device, 
+     * that is tipically specified by the Gazebo plugin configuration file, and corresponds to the 
+     * name attribute of the device XML element when the device is created with the robotinterface 
+     * XML format.
+     *
+     * If the device with the same deviceDatabaseKey exists and the pointer are the same return success, 
+     * if pointers doesn't match returns error.
+     * \param deviceDatabaseKey the deviceDatabaseKey of the device to be added to the internal database
      * \param device2add the pointer of the device to be added to the internal database
      * \return true if successfully added, or the device already exists. False otherwise.
      */
-    bool setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add);
+    bool setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add);
 
-    /** Returns the pointer to the device matching the sensor name
-     * \param deviceName device name to be looked for
+    /** Returns the pointer to the device matching the deviceDatabaseKey
+     * \param deviceDatabaseKey deviceDatabaseKey to be looked for
      * \return the pointer to the device
      */
-    yarp::dev::PolyDriver* getDevice(const std::string& deviceName) const;
+    yarp::dev::PolyDriver* getDevice(const std::string& deviceDatabaseKey) const;
 
     /** \brief Removes a device from the internal database
-     *  \param deviceName the name of the device to be removed
+     *  \param deviceDatabaseKey the deviceDatabaseKey of the device to be removed
      */
-    void removeDevice(const std::string& deviceName);
+    void removeDevice(const std::string& deviceDatabaseKey);
 
     /** 
      * \brief Returns a list of the opened devices
@@ -124,8 +136,16 @@ public:
      *       and it does not transfer or share ownership of the devices.
      *       The consumer code needs to make sure that the driver lifetime
      *       is longer then the consumer lifetime.
+     *
+     * This method returns all the YARP devices that have been created by the specified model, 
+     * and by all its nested model and sensors. As the PolyDriverList is effectively a map in which 
+     * the key is a string and the value is the PolyDriver pointer, in this case the key of the PolyDriverList
+     * is the yarpDeviceName without any scope, i.e. the yarpDeviceName and not the deviceDatabaseKey .
+     * 
+     * If after removing the scope two devices have the same yarpDeviceName, the getModelDevicesAsPolyDriverList
+     * prints an error and returns false, while true is returned if everything works as expected.
      */
-    void getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list);
+    bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list);
     
 
     /** Destructor
diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc
index f53735896..3e950eb41 100644
--- a/libraries/singleton/src/Handler.cc
+++ b/libraries/singleton/src/Handler.cc
@@ -12,6 +12,8 @@
 #include <gazebo/physics/Entity.hh>
 #include <gazebo/sensors/sensors.hh>
 
+#include <unordered_map> 
+
 using namespace gazebo;
 
 namespace GazeboYarpPlugins {
@@ -170,10 +172,10 @@ void Handler::removeSensor(const std::string& sensorName)
     }
 }
 
-bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2add)
+bool Handler::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add)
 {
     bool ret = false;
-    DevicesMap::iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         //device already exists. Increment reference counting
         if(device->second.object() == device2add)
@@ -190,7 +192,7 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad
     } else {
         //device does not exists. Add to map
         ReferenceCountingDevice countedDevice(device2add);
-        if (!m_devicesMap.insert(std::pair<std::string, ReferenceCountingDevice>(deviceName, countedDevice)).second) {
+        if (!m_devicesMap.insert(std::pair<std::string, ReferenceCountingDevice>(deviceDatabaseKey, countedDevice)).second) {
             yError() << " Error in GazeboYarpPlugins::Handler while inserting a new device pointer!";
             ret = false;
         } else {
@@ -200,11 +202,11 @@ bool Handler::setDevice(std::string deviceName, yarp::dev::PolyDriver* device2ad
     return ret;
 }
 
-yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const
+yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceDatabaseKey) const
 {
     yarp::dev::PolyDriver* tmp = NULL;
 
-    DevicesMap::const_iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::const_iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         tmp = device->second.object();
     } else {
@@ -213,9 +215,9 @@ yarp::dev::PolyDriver* Handler::getDevice(const std::string& deviceName) const
     return tmp;
 }
 
-void Handler::removeDevice(const std::string& deviceName)
+void Handler::removeDevice(const std::string& deviceDatabaseKey)
 {
-    DevicesMap::iterator device = m_devicesMap.find(deviceName);
+    DevicesMap::iterator device = m_devicesMap.find(deviceDatabaseKey);
     if (device != m_devicesMap.end()) {
         device->second.decrementCount();
         if (!device->second.count()) {
@@ -223,19 +225,66 @@ void Handler::removeDevice(const std::string& deviceName)
             m_devicesMap.erase(device);
         }
     } else {
-        yError() << "Could not remove device " << deviceName << ". Device was not found";
+        yError() << "Could not remove device " << deviceDatabaseKey << ". Device was not found";
     }
     return;
 }
 
-void Handler::getDevicesAsPolyDriverList(yarp::dev::PolyDriverList& list)
+inline bool startsWith(const std::string&completeString, 
+                       const std::string&candidatePrefix)
+{
+    // https://stackoverflow.com/a/40441240
+    return (completeString.rfind(candidatePrefix, 0) == 0);
+} 
+
+bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list)
 {
-    for(auto&& devicesMapElem: m_devicesMap) {
-        yDebug() << "DEBUG TO REMOVE: Add device to " << devicesMapElem.first;
-        list.push(devicesMapElem.second.object(), devicesMapElem.first.c_str());
+    list = yarp::dev::PolyDriverList();
+
+    // This map contains only the yarpDeviceName that we actually added
+    // to the returned yarp::dev::PolyDriverList
+    std::unordered_map<std::string, std::string> inserted_yarpDeviceName2deviceDatabaseKey;
+
+    for (auto&& devicesMapElem: m_devicesMap) {
+        yDebug() << "DEBUG TO REMOVE: Add device deviceDatabaseKey " << devicesMapElem.first
+                 << " modelScopedName "  << modelScopedName;
+
+        std::string deviceDatabaseKey = devicesMapElem.first;
+
+        std::string yarpDeviceName;
+        
+        // If the deviceDatabaseKey starts with the modelScopedName, then it is eligible for insertion
+        // in the returned list
+        if (startsWith(deviceDatabaseKey, modelScopedName)) {
+            // Extract yarpDeviceName from deviceDatabaseKey
+            yarpDeviceName = deviceDatabaseKey.substr(deviceDatabaseKey.find_last_of(":")+1);
+
+            // Check if a device with the same yarpDeviceName was already inserted
+            auto got = inserted_yarpDeviceName2deviceDatabaseKey.find(yarpDeviceName);
+
+            // If not found, insert and continue
+            if (got == inserted_yarpDeviceName2deviceDatabaseKey.end()) {
+                // If no name collision is found, insert and continue
+                inserted_yarpDeviceName2deviceDatabaseKey.insert({yarpDeviceName, deviceDatabaseKey});
+                list.push(devicesMapElem.second.object(), yarpDeviceName.c_str());
+                yDebug() << " add  yarpDeviceName " << yarpDeviceName;
+            } else {
+                // If a name collision is found, print a clear error and return
+                yError() << "GazeboYARPPlugins robotinterface getDevicesAsPolyDriverList error: ";
+                yError() << "two YARP devices with yarpDeviceName " << yarpDeviceName 
+                         << " found in model " << modelScopedName;
+                yError() << "First instance: " << got->second;
+                yError() << "Second instance: " << deviceDatabaseKey;
+                yError() << "Please eliminate or rename one of the two instances. ";
+                list = yarp::dev::PolyDriverList();
+                return false;
+            }
+
+        }
+
     }
 
-    return;
+    return true;
 }
 
 
diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
index e90de0c50..5604dfd8c 100644
--- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
+++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
@@ -85,18 +85,22 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
 
     // Extract externalDriverList of  devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     yarp::dev::PolyDriverList externalDriverList;
-    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(externalDriverList);
+    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList);
 
     // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList);
     if (!ok) {
         yError() << "GazeboYarpRobotInterface : impossible to set external devices";
+        return;
     }
 
     // Start robotinterface 
     ok = m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseStartup);
     if (!ok) {
         yError() << "GazeboYarpRobotInterface : impossible to start robotinterface";
+        m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseInterrupt1);
+        m_xmlRobotInterfaceResult.robot.enterPhase(yarp::robotinterface::experimental::ActionPhaseShutdown);
+        return;
     }
 }
 
diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
index 4eb862ff1..c3cd68d72 100644
--- a/tests/robotinterface/RobotInterfaceTest.xml
+++ b/tests/robotinterface/RobotInterfaceTest.xml
@@ -11,8 +11,10 @@
       <param name="joints"> 1                        </param>
       <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-          <!-- This should match the name of the device in the .ini file loaded in the Gazebo plugin, prefied by the Gazebo model name -->
-          <elem name="FirstSetOfJoints">  pendulum_with_base::pendulum_aaa </elem>
+          <!-- This should match the yarpDeviceName in the .ini file loaded in the Gazebo plugin. -->
+          <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element 
+               for devices that are created via the robotinterface XML format. -->
+          <elem name="FirstSetOfJoints">  pendulum_controlboard_ </elem>
         </paramlist>
       </action>
       <action phase="shutdown" level="5" type="detach" />

From 7e2b70add8e54b9e72f01d738abf3807362663af Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Tue, 9 Feb 2021 19:36:23 +0100
Subject: [PATCH 07/17] Fix typo in RobotInterfaceText.xml

---
 tests/robotinterface/RobotInterfaceTest.xml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
index c3cd68d72..1bf3e3573 100644
--- a/tests/robotinterface/RobotInterfaceTest.xml
+++ b/tests/robotinterface/RobotInterfaceTest.xml
@@ -14,7 +14,7 @@
           <!-- This should match the yarpDeviceName in the .ini file loaded in the Gazebo plugin. -->
           <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element 
                for devices that are created via the robotinterface XML format. -->
-          <elem name="FirstSetOfJoints">  pendulum_controlboard_ </elem>
+          <elem name="FirstSetOfJoints">  pendulum_controlboard </elem>
         </paramlist>
       </action>
       <action phase="shutdown" level="5" type="detach" />

From 50e2395d9fc8a43c577255fd6a35a3ddc92a7359 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Wed, 10 Feb 2021 11:08:14 +0100
Subject: [PATCH 08/17] Update README.md

---
 plugins/robotinterface/README.md | 82 +++++++++++++++++++++++++++++++-
 1 file changed, 80 insertions(+), 2 deletions(-)

diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md
index 049ab6ea6..30e5734d1 100644
--- a/plugins/robotinterface/README.md
+++ b/plugins/robotinterface/README.md
@@ -1,5 +1,83 @@
 gazebo_yarp_robotinterface
 ==========================
 
-The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices]() that can be attached to YARP devices 
-already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface).
\ No newline at end of file
+The `gazebo_yarp_robotinterface` plugin permits to load several [YARP devices](http://www.yarp.it/git-master/note_devices.html) that can be attached to YARP devices 
+already opened by other Gazebo-YARP plugins using the same XML format used by the [`yarprobotinterface`](http://www.yarp.it/git-master/yarprobotinterface.html) tool and the [`libYARP_robotinterface` C++ library](https://github.com/robotology/yarp/tree/master/src/libYARP_robotinterface).
+
+## Usage 
+
+### Add the plugin in the SDF model
+The `gazebo_yarp_robotinterface` plugin can be used by including in any SDF model:
+~~~xml
+<model name="model_name">
+  <!-- Insert models, links, joints -->
+  <!-- ... -->
+
+  <!-- Insert other plugins -->
+  <!-- ... -->
+  
+  <!-- Insert other gazebo_yarp_robotinterface -->
+  <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
+    <!-- This file can be specified with the model:// URI already used in Gazebo, see http://gazebosim.org/tutorials?tut=components&cat=get_started -->
+    <yarpRobotInterfaceConfigurationFile>model://RobotInterfaceConfigurationFile.xml</yarpRobotInterfaceConfigurationFile>
+  </plugin>
+</model>
+~~~
+
+**Warning: the `gazebo_yarp_robotinterface` plugin is only able to be attached to YARP devices that have been already created once the `gazebo_yarp_robotinterface` plugin has been spawned, so it is a good practice to always include it as last tag in a model.**
+
+### Example of the robotinterface XML file
+
+The file specified in `yarpRobotInterfaceConfigurationFile` is a XML file specified according to the `yarprobotinterface` format, for example:
+~~~xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+<robot name="robot_name" prefix="robot_prefix">
+  <!-- YARP devices to spawn in the robotinterface be specified with the devices tag -->
+  <devices>
+    <!-- each YARP device is specified with the device tag -->
+    <device name="yarp_device_name" type="yarp_device_type">
+      <!-- specify any parameter to the device with param tags -->
+      <param name="integer_param_example"> 10 </param>
+      <param name="string_param_example">this_is_a_string/param>
+      <!-- To specify the existing YARP devices to which the newly spawned device needs to attach -->
+      <action phase="startup" level="5" type="attach">
+        <paramlist name="networks">
+          <!-- This should match the yarpDeviceName in the .ini file loaded in the Gazebo plugin. -->
+          <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element 
+               for devices that are created via the robotinterface XML format. -->
+          <elem name="list_key_example"> yarp_device_name_of_device_to_which_to_attach </elem>
+        </paramlist>
+      </action>
+      <!-- If you added an attach action, always specify the detach action during the shutdown phase -->
+      <action phase="shutdown" level="5" type="detach" />
+    </device>
+  </devices>
+</robot>
+~~~
+
+
+### How to specify existing YARP devices to which to attach
+
+The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to YARP devices that have been already spawned by other `gazebo-yarp-plugins`. For this reason, it is necessary to have a way to specify to which YARP devices a device spawned by `gazebo_yarp_robotinterface` is attached, i.e. the values of each element in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` element. In this content, we call this "device instance identified" as **YARP device instance name**, as for devices spawned by the XML of robotinterface, this is specified by the `name` attribute of the `device` tag. It is important not to confuse this with the **YARP device type name**, that is instead the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag.
+
+The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin  contained in any nested sensor or model. 
+
+For historic reason, not all the `gazebo-yarp-plugins` support specifying the **YARP device instance name** for the device that they spawn to permit to use them with the `gazebo_yarp_robotinterface` plugin. If you need to have this functionality in a specific plugin, feel free to open an issue. 
+
+**Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to use the `gazebo_yarp_robotinterface` plugin that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.**
+
+The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table, together with the details with which the **YARP device instance name** can be specified:
+
+| Plugin     | Details                                              | 
+|:----------:|:----------------------------------------------------:|
+| `gazebo_yarp_controlboard` | This plugin can create multiple YARP devices that expose joint-level motor and control interfaces such as [`yarp::dev::IPositionControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IPositionControl.html), [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1ITorqueControl.html) and [`yarp::dev::ITorqueControl`](https://www.yarp.it/git-master/classyarp_1_1dev_1_1IEncoders.html). For this plugin, the **YARP device instance name** for each created device is specified with the `networks` parameter list in the plugin configuration. |
+| `gazebo_yarp_depthcamera` | This plugin can create a YARP device that expose a depth-camera interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
+| `gazebo_yarp_lasersensor` | This plugin can create a YARP device that expose a laser-seensor interface. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
+| `gazebo_yarp_doublelaser` | This plugin can create a YARP device network wrapper server that expose two existing laser sensors. For this plugin, the **YARP device instance name** can be specified by the `yarpDeviceName` parameter in the plugin configuration. |
+
+
+
+
+
+

From a8eaa0f05cf042c36752a437b9a2d9fe5c5e2286 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 11 Feb 2021 11:06:02 +0100
Subject: [PATCH 09/17] Fix early destruction of device

---
 .../include/GazeboYarpPlugins/Handler.hh      | 12 +++++++++-
 libraries/singleton/src/Handler.cc            | 22 ++++++++++++++-----
 .../gazebo/GazeboYarpRobotInterface.hh        |  1 +
 .../src/GazeboYarpRobotInterface.cc           |  5 +++--
 4 files changed, 32 insertions(+), 8 deletions(-)

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index 32e12300c..1ee599ebe 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -145,8 +145,18 @@ public:
      * If after removing the scope two devices have the same yarpDeviceName, the getModelDevicesAsPolyDriverList
      * prints an error and returns false, while true is returned if everything works as expected.
      */
-    bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list);
+    bool getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector<std::string>& deviceScopedNames);
     
+    /** 
+     * \brief Decrease the usage count for the devices that are acquired with the getDevicesAsPolyDriverList
+     * 
+     * As Gazebo plugins are not denstructed in the same order that they are loaded, it is necessary to keep 
+     * a count of the users of each device, to ensure that is only destructed when no device are still attached to it.
+     * 
+     * This function needs to be called by any plugin that has called the getDevicesAsPolyDriverList method during
+     * the unload/destruction process.
+     */
+    void releaseDevicesInList(const std::vector<std::string>& deviceScopedNames);
 
     /** Destructor
      */
diff --git a/libraries/singleton/src/Handler.cc b/libraries/singleton/src/Handler.cc
index 3e950eb41..7722b128e 100644
--- a/libraries/singleton/src/Handler.cc
+++ b/libraries/singleton/src/Handler.cc
@@ -237,8 +237,10 @@ inline bool startsWith(const std::string&completeString,
     return (completeString.rfind(candidatePrefix, 0) == 0);
 } 
 
-bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list)
+bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yarp::dev::PolyDriverList& list, std::vector<std::string>& deviceScopedNames)
 {
+    deviceScopedNames.resize(0);
+
     list = yarp::dev::PolyDriverList();
 
     // This map contains only the yarpDeviceName that we actually added
@@ -246,9 +248,6 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
     std::unordered_map<std::string, std::string> inserted_yarpDeviceName2deviceDatabaseKey;
 
     for (auto&& devicesMapElem: m_devicesMap) {
-        yDebug() << "DEBUG TO REMOVE: Add device deviceDatabaseKey " << devicesMapElem.first
-                 << " modelScopedName "  << modelScopedName;
-
         std::string deviceDatabaseKey = devicesMapElem.first;
 
         std::string yarpDeviceName;
@@ -267,7 +266,9 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
                 // If no name collision is found, insert and continue
                 inserted_yarpDeviceName2deviceDatabaseKey.insert({yarpDeviceName, deviceDatabaseKey});
                 list.push(devicesMapElem.second.object(), yarpDeviceName.c_str());
-                yDebug() << " add  yarpDeviceName " << yarpDeviceName;
+                deviceScopedNames.push_back(deviceDatabaseKey);
+                // Increase usage counter
+                setDevice(deviceDatabaseKey, devicesMapElem.second.object());
             } else {
                 // If a name collision is found, print a clear error and return
                 yError() << "GazeboYARPPlugins robotinterface getDevicesAsPolyDriverList error: ";
@@ -277,6 +278,8 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
                 yError() << "Second instance: " << deviceDatabaseKey;
                 yError() << "Please eliminate or rename one of the two instances. ";
                 list = yarp::dev::PolyDriverList();
+                releaseDevicesInList(deviceScopedNames);
+                deviceScopedNames.resize(0);
                 return false;
             }
 
@@ -288,4 +291,13 @@ bool Handler::getDevicesAsPolyDriverList(const std::string& modelScopedName, yar
 }
 
 
+void Handler::releaseDevicesInList(const std::vector<std::string>& deviceScopedNames)
+{
+    for (auto&& deviceScopedName: deviceScopedNames) {
+        removeDevice(deviceScopedName);
+    }
+    return;
+}
+
+
 }
diff --git a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
index c00264fbf..c021bbd10 100644
--- a/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
+++ b/plugins/robotinterface/include/gazebo/GazeboYarpRobotInterface.hh
@@ -31,6 +31,7 @@ public:
 private:
     yarp::robotinterface::experimental::XMLReader m_xmlRobotInterfaceReader;
     yarp::robotinterface::experimental::XMLReaderResult m_xmlRobotInterfaceResult;
+    std::vector<std::string> m_deviceScopedNames;
 };
 
 
diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
index 5604dfd8c..6b0e5500a 100644
--- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
+++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
@@ -18,7 +18,6 @@ namespace gazebo
 
 GazeboYarpRobotInterface::GazeboYarpRobotInterface()
 {
-    std::cerr << "GazeboYarpRobotInterface constructor" << std::endl;
 }
 
 GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
@@ -33,6 +32,8 @@ GazeboYarpRobotInterface::~GazeboYarpRobotInterface()
         yError() << "GazeboYarpRobotInterface: impossible  to run phase ActionPhaseShutdown in robotinterface";
     }
 
+    GazeboYarpPlugins::Handler::getHandler()->releaseDevicesInList(m_deviceScopedNames);
+
     yarp::os::Network::fini();
 }
 
@@ -85,7 +86,7 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
 
     // Extract externalDriverList of  devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     yarp::dev::PolyDriverList externalDriverList;
-    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList);
+    GazeboYarpPlugins::Handler::getHandler()->getDevicesAsPolyDriverList(_parentModel->GetScopedName(), externalDriverList, m_deviceScopedNames);
 
     // Set external devices from the one that have been already opened in the Gazebo model by other gazebo_yarp plugins 
     bool ok = m_xmlRobotInterfaceResult.robot.setExternalDevices(externalDriverList);

From c9cf020382ea5265b64011f735ba254b99cedd1b Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 11 Feb 2021 14:22:44 +0100
Subject: [PATCH 10/17] Update ci.yml

---
 .github/workflows/ci.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 9887ad31f..9eec05087 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -117,7 +117,8 @@ jobs:
       shell: bash
       run: |
         cd build
-        ctest --output-on-failure -C ${{ matrix.build_type }} . 
+        # Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/536
+        ctest --output-on-failure -C ${{ matrix.build_type }} -E "ControlBoardControlTest" . 
 
     - name: Install [Ubuntu/macOS]
       if: contains(matrix.os, 'ubuntu') || matrix.os == 'macOS-latest'

From 365c4909309aaf1cf33bd0f742786c0b3ea0eeee Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 11 Feb 2021 15:20:00 +0100
Subject: [PATCH 11/17] Update CHANGELOG.md

---
 CHANGELOG.md | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 5df3e4082..faca859ed 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -14,6 +14,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
   configuration. This generator enables the trajectory to follow a trapezoidal speed profile in position control mode, limited
   by provided reference speed (saturation) and acceleration (both ramps) values. If already executing a trajectory in this manner,
   newly generated trajectories take into account previous joint velocities and update the motion accordingly.
+- Add `gazebo_yarp_robotinterface` plugin, the documentation for it can be found at [plugins/robotinterface/README.md](plugins/robotinterface/README.md) (https://github.com/robotology/gazebo-yarp-plugins/pull/532).  
+- The `gazebo_yarp_depthcamera` and `gazebo_yarp_doublesensor` now accept a `yarpDeviceName` parameter (https://github.com/robotology/gazebo-yarp-plugins/pull/532).
+
+
+### Changed 
+- The `deviceId` parameter of the `gazebo_yarp_lasersensor` is now named `yarpDeviceName` )https://github.com/robotology/gazebo-yarp-plugins/pull/532).
 
 ## [3.5.1] - 2020-10-05
 

From a5bb3b808990983dba9ddc2b0e156cd874cb79a6 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 15 Feb 2021 17:54:48 +0100
Subject: [PATCH 12/17] Update
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>
---
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index 1ee599ebe..c8649b682 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -100,7 +100,7 @@ public:
 
     /** \brief Adds a new yarp device pointer to the "database".
      *
-     * The YARP device are stored in these database using the following schema:
+     * The YARP devices are stored in this database using the following schema:
      *  * For YARP devices created by Model plugins, the deviceDatabaseKey is 
      *    defined as deviceDatabaseKey = Model::GetScopedName() + "::" + yarpDeviceName
      *  * For YARP devices created by Sensor plugins, the deviceDatabaseKey is 

From dac6e7f845ccc8c205089b20e27c426dd7234ebb Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 15 Feb 2021 18:02:05 +0100
Subject: [PATCH 13/17] Apply suggestions from code review

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>
---
 plugins/CMakeLists.txt                                 | 2 +-
 plugins/depthCamera/src/DepthCamera.cc                 | 1 -
 plugins/robotinterface/README.md                       | 5 ++---
 plugins/robotinterface/src/GazeboYarpRobotInterface.cc | 2 +-
 tests/CMakeLists.txt                                   | 2 +-
 tests/robotinterface/CMakeLists.txt                    | 2 +-
 6 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt
index 11ee008a4..34d321895 100644
--- a/plugins/CMakeLists.txt
+++ b/plugins/CMakeLists.txt
@@ -43,4 +43,4 @@ add_subdirectory(configurationoverride)
 
 if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
   add_subdirectory(robotinterface)
-endif()
\ No newline at end of file
+endif()
diff --git a/plugins/depthCamera/src/DepthCamera.cc b/plugins/depthCamera/src/DepthCamera.cc
index 9eb0bbab0..2dbe180a0 100644
--- a/plugins/depthCamera/src/DepthCamera.cc
+++ b/plugins/depthCamera/src/DepthCamera.cc
@@ -125,7 +125,6 @@ void GazeboYarpDepthCamera::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sd
     if(!m_driverParameters.check("yarpDeviceName"))
     {
         yError()<<"GazeboYarpDepthCamera: cannot find yarpDeviceName parameter in ini file.";
-        //return;
     }
     else
     {
diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md
index 30e5734d1..4b6bbae72 100644
--- a/plugins/robotinterface/README.md
+++ b/plugins/robotinterface/README.md
@@ -16,7 +16,7 @@ The `gazebo_yarp_robotinterface` plugin can be used by including in any SDF mode
   <!-- Insert other plugins -->
   <!-- ... -->
   
-  <!-- Insert other gazebo_yarp_robotinterface -->
+  <!-- Insert gazebo_yarp_robotinterface -->
   <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
     <!-- This file can be specified with the model:// URI already used in Gazebo, see http://gazebosim.org/tutorials?tut=components&cat=get_started -->
     <yarpRobotInterfaceConfigurationFile>model://RobotInterfaceConfigurationFile.xml</yarpRobotInterfaceConfigurationFile>
@@ -65,7 +65,7 @@ The `gazebo_yarp_robotinterface` can be attached to any YARP device created by a
 
 For historic reason, not all the `gazebo-yarp-plugins` support specifying the **YARP device instance name** for the device that they spawn to permit to use them with the `gazebo_yarp_robotinterface` plugin. If you need to have this functionality in a specific plugin, feel free to open an issue. 
 
-**Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to use the `gazebo_yarp_robotinterface` plugin that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.**
+**Warning: as the YARP device instance name is specified without any specific Gazebo model or sensor namespace, it is important to observe, while using the `gazebo_yarp_robotinterface` plugin, that all the YARP devices contained in the model have a unique YARP device instance name. If this is not the case, the plugin will print a clear error and exit without starting.**
 
 The plugins that spawn YARP devices in a way that they can be then attached to the yarprobotinterface as specified in the following table, together with the details with which the **YARP device instance name** can be specified:
 
@@ -80,4 +80,3 @@ The plugins that spawn YARP devices in a way that they can be then attached to t
 
 
 
-
diff --git a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
index 6b0e5500a..d6d7a709f 100644
--- a/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
+++ b/plugins/robotinterface/src/GazeboYarpRobotInterface.cc
@@ -71,7 +71,7 @@ void GazeboYarpRobotInterface::Load(physics::ModelPtr _parentModel, sdf::Element
             if (m_xmlRobotInterfaceResult.parsingIsSuccessful) {
                 loaded_configuration = true;
             } else {
-                yError() << "GazeboYarpRobotInterface error: failure in loading robotinterface configuration for model" << _parentModel->GetName() << "\n"
+                yError() << "GazeboYarpRobotInterface error: failure in parsing robotinterface configuration for model" << _parentModel->GetName() << "\n"
                       << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile : " << robotinterface_file_name << "\n"
                       << "GazeboYarpRobotInterface error: yarpRobotInterfaceConfigurationFile absolute path : " << robotinterface_file_path;
                 loaded_configuration = false; 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 314471426..2105b422c 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -37,4 +37,4 @@ set_property(TEST ControlBoardControlTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${
 
 if(GAZEBO_YARP_PLUGINS_HAS_YARP_ROBOTINTERFACE)
   add_subdirectory(robotinterface)
-endif()
\ No newline at end of file
+endif()
diff --git a/tests/robotinterface/CMakeLists.txt b/tests/robotinterface/CMakeLists.txt
index d6102faa4..b8134bbc4 100644
--- a/tests/robotinterface/CMakeLists.txt
+++ b/tests/robotinterface/CMakeLists.txt
@@ -11,4 +11,4 @@ target_compile_definitions(RobotInterfaceTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR=
 add_test(NAME RobotInterfaceTest COMMAND RobotInterfaceTest)
 # Ensure that YARP devices can be found
 # Disable use of online model database
-set_property(TEST RobotInterfaceTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=)
\ No newline at end of file
+set_property(TEST RobotInterfaceTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=)

From 7c2ef3c5c9c34467f802d5955acff469788520bf Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 15 Feb 2021 18:02:19 +0100
Subject: [PATCH 14/17] Update
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>
---
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index c8649b682..fa08180e3 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -140,7 +140,7 @@ public:
      * This method returns all the YARP devices that have been created by the specified model, 
      * and by all its nested model and sensors. As the PolyDriverList is effectively a map in which 
      * the key is a string and the value is the PolyDriver pointer, in this case the key of the PolyDriverList
-     * is the yarpDeviceName without any scope, i.e. the yarpDeviceName and not the deviceDatabaseKey .
+     * is the yarpDeviceName without any scope, i.e. not the deviceDatabaseKey .
      * 
      * If after removing the scope two devices have the same yarpDeviceName, the getModelDevicesAsPolyDriverList
      * prints an error and returns false, while true is returned if everything works as expected.

From 334d67b7fb55f510dd55aed59330c798b9dbf386 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Mon, 15 Feb 2021 18:02:30 +0100
Subject: [PATCH 15/17] Update
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh

Co-authored-by: Prashanth Ramadoss <prashanthramadoss@gmail.com>
---
 libraries/singleton/include/GazeboYarpPlugins/Handler.hh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
index fa08180e3..9bb06481d 100644
--- a/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
+++ b/libraries/singleton/include/GazeboYarpPlugins/Handler.hh
@@ -150,7 +150,7 @@ public:
     /** 
      * \brief Decrease the usage count for the devices that are acquired with the getDevicesAsPolyDriverList
      * 
-     * As Gazebo plugins are not denstructed in the same order that they are loaded, it is necessary to keep 
+     * As Gazebo plugins are not destructed in the same order that they are loaded, it is necessary to keep 
      * a count of the users of each device, to ensure that is only destructed when no device are still attached to it.
      * 
      * This function needs to be called by any plugin that has called the getDevicesAsPolyDriverList method during

From 6d68489719d72a66c279996b1bd946d3fa4861d2 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 18 Feb 2021 12:33:45 +0100
Subject: [PATCH 16/17] Update RobotInterfaceTest.xml

---
 tests/robotinterface/RobotInterfaceTest.xml | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/tests/robotinterface/RobotInterfaceTest.xml b/tests/robotinterface/RobotInterfaceTest.xml
index 1bf3e3573..e8eb4f732 100644
--- a/tests/robotinterface/RobotInterfaceTest.xml
+++ b/tests/robotinterface/RobotInterfaceTest.xml
@@ -2,6 +2,12 @@
 <!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
 <robot name="robotinterface_additional_controlboard_nws" prefix="robotinterface_additional_controlboard_nws">
   <devices>
+    <!-- This file contains a list of YARP devices that are spawned by the gazebo_yarp_robotinterface plugin.
+         Diffently from most uses of this XML files with the yarprobotinterface tool, in this case 
+         the devices that are created by the yarprobotinterface can be "attached" also to device 
+         that have not been created by the gazebo_yarp_robotinterface plugin, but YARP devices that have been 
+         created by other plugins, in this specific case a controlboardwrapper2 devices is attached to a 
+         YARP device created by the gazebo_yarp_controlboard plugin. -->
     <device name="additional_controlboard_nws" type="controlboardwrapper2">
       <param name="period"> 10 </param>
       <param name="name"> /pendulumGazebo/openedByTheRobotInterface</param>
@@ -11,9 +17,8 @@
       <param name="joints"> 1                        </param>
       <action phase="startup" level="5" type="attach">
         <paramlist name="networks">
-          <!-- This should match the yarpDeviceName in the .ini file loaded in the Gazebo plugin. -->
-          <!-- The yarpDeviceName plays the same role of the name attribute of the device XML element 
-               for devices that are created via the robotinterface XML format. -->
+          <!-- This should match the YARP device instance name specifiedin the .ini file loaded in the Gazebo plugin.
+               Chech the gazebo_yarp_robotinterface documentation for more details. -->
           <elem name="FirstSetOfJoints">  pendulum_controlboard </elem>
         </paramlist>
       </action>

From 6a12ac2b71e6f71571ba4a77a55b4a9246750b64 Mon Sep 17 00:00:00 2001
From: Silvio Traversaro <silvio.traversaro@iit.it>
Date: Thu, 18 Feb 2021 12:40:29 +0100
Subject: [PATCH 17/17] Update README.md

---
 plugins/robotinterface/README.md | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/plugins/robotinterface/README.md b/plugins/robotinterface/README.md
index 4b6bbae72..0b36b06d7 100644
--- a/plugins/robotinterface/README.md
+++ b/plugins/robotinterface/README.md
@@ -59,7 +59,9 @@ The file specified in `yarpRobotInterfaceConfigurationFile` is a XML file specif
 
 ### How to specify existing YARP devices to which to attach
 
-The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to YARP devices that have been already spawned by other `gazebo-yarp-plugins`. For this reason, it is necessary to have a way to specify to which YARP devices a device spawned by `gazebo_yarp_robotinterface` is attached, i.e. the values of each element in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` element. In this content, we call this "device instance identified" as **YARP device instance name**, as for devices spawned by the XML of robotinterface, this is specified by the `name` attribute of the `device` tag. It is important not to confuse this with the **YARP device type name**, that is instead the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag.
+The main use of the `gazebo_yarp_robotinterface` plugin is to spawn YARP devices that are **attached** to other devices that have been already spawned by other Gazebp plugins. For this reason, we need a way to specify to which target devices the devices created by robotinterface needs  to be attached.
+This is achieved by setting the elements in the `<paramlist name="networks">` list under the `<action phase="startup" level="5" type="attach">` XML element. 
+In this context, we call this "device instance identified" as **YARP device instance name**, as for devices created by the robotinterface, this is specified by the `name` attribute of the `device` XML tag. It is important not to confuse this with the **YARP device type name**, i.e. the name that identifies the type of plugin that is spawned, i.e. the `type` attribute of the `device` tag.
 
 The `gazebo_yarp_robotinterface` can be attached to any YARP device created by any plugin inside the model, or in any plugin  contained in any nested sensor or model.