Fix Gazebo freezing if YARP_CLOCK is set

CHANGELOG.md

@@ -14,6 +14,11 @@ 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.
+
+### Fixed
+- Fix the support for running Gazebo itself with the `gazebo_yarp_clock` with YARP_CLOCK set, without Gazebo freezing at startup.  
+  In particular, setting YARP_CLOCK is suggested to ensure that all the threads of YARP Network Wrapper Servers are executed with 
+  the frequency correctly synchronized with the Gazebo simulation (https://github.com/robotology/gazebo-yarp-plugins/pull/537).
 
 ## [3.5.1] - 2020-10-05
 

plugins/clock/include/gazebo/Clock.hh

@@ -132,6 +132,10 @@ private:
     yarp::os::Port *m_rpcPort;
     GazeboYarpPlugins::ClockServer *m_clockServer;
 
+    // True if the YARP network needs to be reset to
+    // YARP_CLOCK_DEFAULT after the port has been created
+    bool m_resetYARPClockAfterPortCreation;
+
 };
 
 

plugins/clock/src/Clock.cc

@@ -68,7 +68,20 @@ namespace gazebo
 
     void GazeboYarpClock::Load(int _argc, char **_argv)
     {
-        m_network = new yarp::os::Network();
+        // To avoid deadlock during initialation if YARP_CLOCK is set,
+        // if the YARP network is not initialized we always initialize
+        // it with system clock, and then we
+        // This avoid the problems discussed in https://github.com/robotology/gazebo-yarp-plugins/issues/526
+        bool networkIsNotInitialized = !yarp::os::NetworkBase::isNetworkInitialized();
+
+        if (networkIsNotInitialized) {
+            m_network = new yarp::os::Network(yarp::os::YARP_CLOCK_SYSTEM);
+            m_resetYARPClockAfterPortCreation = true;
+        } else {
+            m_network = new yarp::os::Network();
+            m_resetYARPClockAfterPortCreation = false;
+        }
+
         if (!m_network
             || !yarp::os::Network::checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
             yError() << "GazeboYarpClock::Load error: yarp network does not seem to be available, is the yarpserver running?";
@@ -88,7 +101,7 @@ namespace gazebo
             m_portName = portName.asString();
         }
 
-        yDebug() << "GazeboYarpClock loaded. Clock port will be " << m_portName;
+        yInfo() << "GazeboYarpClock loaded. Clock port will be " << m_portName;
 
         //The proper loading is done when the world is created
         m_worldCreatedEvent = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboYarpClock::gazeboYarpClockLoad,this,_1));
@@ -158,6 +171,19 @@ namespace gazebo
             b.addInt(currentTime.nsec);
             m_clockPort->write();
         }
+
+        // As the port is now created and contains streams data,
+        // if necessary reset the YARP clock to YARP_CLOCK_DEFAULT
+        // Unfortunatly, the yarpClockInit blocks on the port until it
+        // receives data, so we need to launch it in a different thread
+        if (m_resetYARPClockAfterPortCreation) {
+            std::cerr << "Resetting YARP clock to default" << std::endl;
+            auto resetYARPNetworkClockLambda =
+                []() { yarp::os::NetworkBase::yarpClockInit(yarp::os::YARP_CLOCK_DEFAULT); };
+            std::thread resetYARPNetworkClockThread(resetYARPNetworkClockLambda);
+            resetYARPNetworkClockThread.detach();
+            m_resetYARPClockAfterPortCreation = false;
+        }
     }
 
     void GazeboYarpClock::clockPause()

tests/CMakeLists.txt

@@ -24,6 +24,9 @@ if(NOT gazebo-classic-gtest_POPULATED)
   target_include_directories(gyp-gazebo-classic-gtest PUBLIC ${gazebo-classic-gtest_SOURCE_DIR} ${gazebo-classic-gtest_SOURCE_DIR}/include)
 endif()
 
+list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR})
+include(Fetchtiny-process-library)
+
 add_executable(ControlBoardControlTest ControlBoardControlTest.cc)
 target_include_directories(ControlBoardControlTest PUBLIC ${GAZEBO_INCLUDE_DIRS})
 find_library(GAZEBO_TEST_LIB NAMES gazebo_test_fixture HINTS ${GAZEBO_LIBRARY_DIRS})
@@ -33,4 +36,7 @@ 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})
+
+
+add_subdirectory(clock)

tests/Fetchtiny-process-library.cmake

@@ -0,0 +1,52 @@
+include(FetchContent)
+FetchContent_Declare(
+  tinyprocesslibrary
+  GIT_REPOSITORY https://gitlab.com/eidheim/tiny-process-library.git
+  GIT_TAG v2.0.2)
+
+FetchContent_GetProperties(tinyprocesslibrary)
+
+if(NOT tinyprocesslibrary_POPULATED)
+  FetchContent_Populate(tinyprocesslibrary)
+
+  # We don't want to install this library in the system, we instead
+  # compile it as an OBJECT library and embed in either the shared or
+  # static libraries that need it.
+  # From https://gitlab.kitware.com/cmake/cmake/-/issues/18935 it seems
+  # that OBJECT libraries that are not installed become INTERFACE when
+  # part of an EXPORT set.
+  # This behaviour allows setting transitively tiny-process-library infos
+  # to the consuming targets while not breaking the EXPORT process. In fact,
+  # the conversion to INTERFACE allows to add tiny-process-library to the
+  # targets of the EXPORT that contains targets linking against it.
+  # See also https://cmake.org/pipermail/cmake/2018-September/068250.html.
+
+  if(WIN32)
+    add_library(tiny-process-library OBJECT
+      ${tinyprocesslibrary_SOURCE_DIR}/process.cpp
+      ${tinyprocesslibrary_SOURCE_DIR}/process_win.cpp)
+    #If compiled using MSYS2, use sh to run commands
+    if(MSYS)
+      target_compile_definitions(tiny-process-library
+        PUBLIC MSYS_PROCESS_USE_SH)
+    endif()
+  else()
+    add_library(tiny-process-library OBJECT
+      ${tinyprocesslibrary_SOURCE_DIR}/process.cpp
+      ${tinyprocesslibrary_SOURCE_DIR}/process_unix.cpp)
+  endif()
+  add_library(tiny-process-library::tiny-process-library ALIAS tiny-process-library)
+
+  if(MSVC)
+    target_compile_definitions(tiny-process-library
+      PRIVATE /D_CRT_SECURE_NO_WARNINGS)
+  endif()
+
+  find_package(Threads REQUIRED)
+
+  target_link_libraries(tiny-process-library PRIVATE
+    ${CMAKE_THREAD_LIBS_INIT})
+  target_include_directories(tiny-process-library PUBLIC
+    $<BUILD_INTERFACE:${tinyprocesslibrary_SOURCE_DIR}>)
+
+endif()

tests/clock/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia
+# CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
+
+add_executable(ClockTest ClockTest.cc)
+target_include_directories(ClockTest PUBLIC ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(ClockTest 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(ClockTest PRIVATE -DCMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
+target_compile_definitions(ClockTest PRIVATE -DCMAKE_CURRENT_BINARY_DIR="${CMAKE_CURRENT_BINARY_DIR}")
+target_compile_definitions(ClockTest PRIVATE -DGAZEBO_YARP_PLUGINS_DIR="$<TARGET_FILE_DIR:gazebo_yarp_clock>")
+target_compile_definitions(ClockTest PRIVATE -DYARP_SERVER_LOCATION="${YARP_INSTALL_PREFIX}/bin/yarpserver")
+add_test(NAME ClockTest COMMAND ClockTest)
+set_tests_properties(ClockTest PROPERTIES TIMEOUT 15) 
+# Ensure that YARP devices can be found
+# Disable use of online model database
+set_property(TEST ClockTest PROPERTY ENVIRONMENT YARP_DATA_DIRS=${YARP_DATA_INSTALL_DIR_FULL};GAZEBO_MODEL_DATABASE_URI=) 
+# As this test does not use localmode, launch yarpserver explicitly via tiny-process-library
+target_link_libraries(ClockTest PUBLIC tiny-process-library)

tests/clock/ClockTest.cc

@@ -0,0 +1,121 @@
+/*
+ * 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 <gazebo/gazebo.hh>
+
+#include <yarp/conf/environment.h>
+#include <yarp/serversql/yarpserversql.h>
+
+#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>
+
+#include "process.hpp"
+#include <memory>
+
+class ClockTest : public gazebo::ServerFixture,
+                  public testing::WithParamInterface<const char*>
+{
+  struct PluginTestHelperOptions
+  {
+  };
+
+  public: gazebo::event::ConnectionPtr updateConnection;
+  public: std::unique_ptr<TinyProcessLib::Process> yarpserverProcess;
+
+  public: void PluginTest(const std::string &_physicsEngine);
+  public: void PluginTestHelper(const std::string &_physicsEngine,
+                                const std::string &worldName,
+                                const PluginTestHelperOptions& options);
+
+
+};
+
+void ClockTest::PluginTestHelper(const std::string &_physicsEngine,
+                                             const std::string &worldName,
+                                             const PluginTestHelperOptions& options)
+{
+  // Set YARP_CLOCK to /clock and check if test works
+  // Regression test for https://github.com/robotology/gazebo-yarp-plugins/issues/526
+  yarp::conf::environment::setEnvironment("YARP_CLOCK", "/clock");
+
+  // Load plugin libgazebo_yarp_clock
+  gazebo::addPlugin("libgazebo_yarp_clock.so");
+
+
+  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 << "ClockTest: 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);
+
+  // Check if the /clock port has been correctly created
+  EXPECT_TRUE(yarp::os::NetworkBase::exists("/clock"));
+
+  // Unload the simulation
+  Unload();
+
+  // Close yarpserver
+  yarpserverProcess->kill();
+}
+
+/////////////////////////////////////////////////////////////////////
+void ClockTest::PluginTest(const std::string &_physicsEngine)
+{
+  // In this case we do not use setLocalMode as we need to 
+  // ensure that the low level of YARP behave like in the case 
+  // of when the user uses them, i.e. using an external yarpserver
+  std::string yarpserverLocation = YARP_SERVER_LOCATION;
+  gzdbg << "ClockTest: launching yarpserver from " << YARP_SERVER_LOCATION << std::endl;
+  yarpserverProcess = 
+    std::make_unique<TinyProcessLib::Process>(yarpserverLocation);
+  std::this_thread::sleep_for(std::chrono::seconds(3));
+
+  // 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, "empty.world", options);
+}
+
+TEST_P(ClockTest, PluginTest)
+{
+  PluginTest(GetParam());
+}
+
+// Only test for ode
+INSTANTIATE_TEST_CASE_P(PhysicsEngines, ClockTest, ::testing::Values("ode"));
+
+/////////////////////////////////////////////////
+/// Main
+int main(int argc, char **argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

tests/clock/empty.world

@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <!-- A ground plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+  </world>
+</sdf>