First draft of gazebo_yarp_robotinterface plugin

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})

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();

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;
+}
+
+
 }

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()

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)

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).

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

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)
+}

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})
+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()

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=)

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