From c1201aeb086e25796da3c141ba61fe49a39b35b9 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 11 Feb 2021 19:26:58 +0100 Subject: [PATCH 1/4] Fix Gazebo freezing if YARP_CLOCK is set Fix https://github.com/robotology/gazebo-yarp-plugins/issues/526 Add also regression test for the issue --- plugins/clock/include/gazebo/Clock.hh | 4 + plugins/clock/src/Clock.cc | 30 ++++++- tests/CMakeLists.txt | 8 +- tests/Fetchtiny-process-library.cmake | 52 +++++++++++ tests/clock/CMakeLists.txt | 17 ++++ tests/clock/ClockTest.cc | 121 ++++++++++++++++++++++++++ tests/clock/empty.world | 13 +++ 7 files changed, 242 insertions(+), 3 deletions(-) create mode 100644 tests/Fetchtiny-process-library.cmake create mode 100644 tests/clock/CMakeLists.txt create mode 100644 tests/clock/ClockTest.cc create mode 100644 tests/clock/empty.world diff --git a/plugins/clock/include/gazebo/Clock.hh b/plugins/clock/include/gazebo/Clock.hh index 565d68b5b..89413080d 100644 --- a/plugins/clock/include/gazebo/Clock.hh +++ b/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; + }; diff --git a/plugins/clock/src/Clock.cc b/plugins/clock/src/Clock.cc index 8456db821..0ebc07a3a 100644 --- a/plugins/clock/src/Clock.cc +++ b/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() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4d2bdfa13..82e84b382 100644 --- a/tests/CMakeLists.txt +++ b/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="$") 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}) + + +add_subdirectory(clock) \ No newline at end of file diff --git a/tests/Fetchtiny-process-library.cmake b/tests/Fetchtiny-process-library.cmake new file mode 100644 index 000000000..93d59fe02 --- /dev/null +++ b/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 + $) + +endif() \ No newline at end of file diff --git a/tests/clock/CMakeLists.txt b/tests/clock/CMakeLists.txt new file mode 100644 index 000000000..35ed9adb0 --- /dev/null +++ b/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_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) \ No newline at end of file diff --git a/tests/clock/ClockTest.cc b/tests/clock/ClockTest.cc new file mode 100644 index 000000000..5c607b3dd --- /dev/null +++ b/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 +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "process.hpp" +#include + +class ClockTest : public gazebo::ServerFixture, + public testing::WithParamInterface +{ + struct PluginTestHelperOptions + { + }; + + public: gazebo::event::ConnectionPtr updateConnection; + public: std::unique_ptr 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(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(); +} diff --git a/tests/clock/empty.world b/tests/clock/empty.world new file mode 100644 index 000000000..532167242 --- /dev/null +++ b/tests/clock/empty.world @@ -0,0 +1,13 @@ + + + + + + model://sun + + + + model://ground_plane + + + From 2fe3b1736707b80eca06dde18c48986010441039 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 11 Feb 2021 19:41:47 +0100 Subject: [PATCH 2/4] Update CHANGELOG.md --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5df3e4082..7ca43530f 100644 --- a/CHANGELOG.md +++ b/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 From f7bb98bc2ae9aa65ead2948daeca399697d52326 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 12 Feb 2021 11:51:38 +0100 Subject: [PATCH 3/4] Apply suggestions from code review --- plugins/clock/src/Clock.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/clock/src/Clock.cc b/plugins/clock/src/Clock.cc index 0ebc07a3a..ab374f1dc 100644 --- a/plugins/clock/src/Clock.cc +++ b/plugins/clock/src/Clock.cc @@ -68,9 +68,9 @@ namespace gazebo void GazeboYarpClock::Load(int _argc, char **_argv) { - // To avoid deadlock during initialation if YARP_CLOCK is set, + // To avoid deadlock during initialization if YARP_CLOCK is set, // if the YARP network is not initialized we always initialize - // it with system clock, and then we + // it with system clock, and then we switch back to the default clock later // This avoid the problems discussed in https://github.com/robotology/gazebo-yarp-plugins/issues/526 bool networkIsNotInitialized = !yarp::os::NetworkBase::isNetworkInitialized(); From 0102462f04d9c7c8896c2c1e07ea072969d9654a Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 12 Feb 2021 12:51:02 +0100 Subject: [PATCH 4/4] Update CMakeLists.txt --- tests/CMakeLists.txt | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 82e84b382..2cf97b5a9 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -24,9 +24,6 @@ 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}) @@ -38,5 +35,9 @@ 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}) - -add_subdirectory(clock) \ No newline at end of file +# Workaround for https://github.com/robotology/gazebo-yarp-plugins/issues/530 +if(NOT APPLE) + list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}) + include(Fetchtiny-process-library) + add_subdirectory(clock) +endif()