From 796aa8a58f2e06c852e0bbcaaf7af388c3e34230 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Mon, 6 Dec 2021 12:34:08 +0100 Subject: [PATCH 1/6] added device for synchronization --- src/devices/CMakeLists.txt | 1 + src/devices/systemReady/CMakeLists.txt | 48 +++++++++++++++ .../systemReady/SystemReady_nws_yarp.cpp | 49 +++++++++++++++ .../systemReady/SystemReady_nws_yarp.h | 60 +++++++++++++++++++ 4 files changed, 158 insertions(+) create mode 100644 src/devices/systemReady/CMakeLists.txt create mode 100644 src/devices/systemReady/SystemReady_nws_yarp.cpp create mode 100644 src/devices/systemReady/SystemReady_nws_yarp.h diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 2453db464e0..7873afe7e13 100644 --- a/src/devices/CMakeLists.txt +++ b/src/devices/CMakeLists.txt @@ -101,6 +101,7 @@ yarp_begin_plugin_library(yarpmod add_subdirectory(mobileBaseVelocityControl) add_subdirectory(mobileBaseVelocityControlMsgs) add_subdirectory(odometry2D) + add_subdirectory(systemReady) add_subdirectory(portaudio) # DEPRECATED Since YARP 3.2 diff --git a/src/devices/systemReady/CMakeLists.txt b/src/devices/systemReady/CMakeLists.txt new file mode 100644 index 00000000000..10260033472 --- /dev/null +++ b/src/devices/systemReady/CMakeLists.txt @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(systemReady_nws_yarp + CATEGORY device + TYPE SystemReady_nws_yarp + INCLUDE SystemReady_nws_yarp.h + EXTRA_CONFIG + WRAPPER=systemReady_nws_yarp + DEFAULT ON +) + +if(NOT SKIP_systemReady_nws_yarp) + yarp_add_plugin(yarp_systemReady_nws_yarp) + + target_sources(yarp_systemReady_nws_yarp + PRIVATE + SystemReady_nws_yarp.cpp + SystemReady_nws_yarp.h + ) + + target_link_libraries(yarp_systemReady_nws_yarp + PRIVATE + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + ) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS + YARP_os + YARP_sig + YARP_dev + ) + + yarp_install( + TARGETS yarp_systemReady_nws_yarp + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_systemReady_nws_yarp PROPERTY FOLDER "Plugins/Device/NWS") +endif() + diff --git a/src/devices/systemReady/SystemReady_nws_yarp.cpp b/src/devices/systemReady/SystemReady_nws_yarp.cpp new file mode 100644 index 00000000000..adf720deaf2 --- /dev/null +++ b/src/devices/systemReady/SystemReady_nws_yarp.cpp @@ -0,0 +1,49 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + +#include "SystemReady_nws_yarp.h" +#include +#include + +YARP_LOG_COMPONENT(SYSTEMREADY_NWS_YARP, "yarp.devices.SystemReady_nws_yarp") + +SystemReady_nws_yarp::SystemReady_nws_yarp() +{ +} + +bool SystemReady_nws_yarp::open(yarp::os::Searchable &config) +{ + if (config.check("PORT_LIST")) { + yarp::os::Bottle port_list = config.findGroup("PORT_LIST"); + // skips the first one since it is PORT_LIST + for (int index = 1; index < port_list.size(); index++) { + std::string current_port_property = port_list.get(index).toString(); + std::string token; + int pos; + char delimiter = ' '; + pos = current_port_property.find(delimiter); + token = current_port_property.substr(0, pos); + std::string current_port_name = port_list.find(token).asString(); + yCError(SYSTEMREADY_NWS_YARP) << port_list.get(index).asList()[0].toString(); + yCError(SYSTEMREADY_NWS_YARP) << current_port_property; + yCError(SYSTEMREADY_NWS_YARP) << token; + yCError(SYSTEMREADY_NWS_YARP) << port_list.find(token).asString(); + yarp::os::Port* current_port = new yarp::os::Port; + current_port->open(current_port_name); + port_pointers_list.push_back(current_port); + } + return true; + } + return false; +} + + +bool SystemReady_nws_yarp::close() +{ + for (auto elem: port_pointers_list){ + elem->close(); + delete elem; + } +} diff --git a/src/devices/systemReady/SystemReady_nws_yarp.h b/src/devices/systemReady/SystemReady_nws_yarp.h new file mode 100644 index 00000000000..3b830f1f3bf --- /dev/null +++ b/src/devices/systemReady/SystemReady_nws_yarp.h @@ -0,0 +1,60 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_SYSTEMREADY_NWS_YARP_H +#define YARP_SYSTEMREADY_NWS_YARP_H + +#include +#include + + +/** + * @ingroup dev_impl_network_clients dev_impl_navigation + * + * \section SystemReady_nws_yarp_parameters Device description + * \brief `SystemReady_nws_yarp`: A yarp nws to open port for synchronization: + * it opens the specified ports inside the PORT_LIST group. + * Related to awaitSystemReady_nws_yarp. + * + * Parameters required by this device are: + * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | + * |:-------------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:-----------------------------: |:-----------------------------------------------------| + * | PORT_NAME | - | group | | | Yes | the group inside which port names are located | + * + * example of xml file with a fake odometer + * + * \code{.unparsed} + * + * + * + * + * + * + * /pippo + * /pluto + * + * + * + * + * \endcode + * + */ + +class SystemReady_nws_yarp : + public yarp::dev::DeviceDriver +{ +public: + SystemReady_nws_yarp(); + + // DeviceDriver + bool open(yarp::os::Searchable ¶ms) override; + bool close() override; + +private: + std::vector port_pointers_list; + +}; + +#endif // YARP_SYSTEMREADY_NWS_YARP_H From 09b4b4edea0dee7fd38dcf73f2161ba287e04365 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Mon, 6 Dec 2021 16:55:12 +0100 Subject: [PATCH 2/6] fixes --- doc/release/master/systemReady.md | 7 +++++++ src/devices/systemReady/CMakeLists.txt | 1 - .../systemReady/SystemReady_nws_yarp.cpp | 18 ++++++++++-------- src/devices/systemReady/SystemReady_nws_yarp.h | 3 ++- 4 files changed, 19 insertions(+), 10 deletions(-) create mode 100644 doc/release/master/systemReady.md diff --git a/doc/release/master/systemReady.md b/doc/release/master/systemReady.md new file mode 100644 index 00000000000..e547fb38867 --- /dev/null +++ b/doc/release/master/systemReady.md @@ -0,0 +1,7 @@ +systemReady {#master} +----------------------- + +### yarp +#### devices +* added systemReady device that opens some ports when called. It is useful for synchronize processes opened with yarprobotinterface and yarpmanager to avoid crashes when some processes are not started + diff --git a/src/devices/systemReady/CMakeLists.txt b/src/devices/systemReady/CMakeLists.txt index 10260033472..421d9337474 100644 --- a/src/devices/systemReady/CMakeLists.txt +++ b/src/devices/systemReady/CMakeLists.txt @@ -45,4 +45,3 @@ if(NOT SKIP_systemReady_nws_yarp) set_property(TARGET yarp_systemReady_nws_yarp PROPERTY FOLDER "Plugins/Device/NWS") endif() - diff --git a/src/devices/systemReady/SystemReady_nws_yarp.cpp b/src/devices/systemReady/SystemReady_nws_yarp.cpp index adf720deaf2..c5d842f6656 100644 --- a/src/devices/systemReady/SystemReady_nws_yarp.cpp +++ b/src/devices/systemReady/SystemReady_nws_yarp.cpp @@ -16,9 +16,9 @@ SystemReady_nws_yarp::SystemReady_nws_yarp() bool SystemReady_nws_yarp::open(yarp::os::Searchable &config) { if (config.check("PORT_LIST")) { - yarp::os::Bottle port_list = config.findGroup("PORT_LIST"); // skips the first one since it is PORT_LIST - for (int index = 1; index < port_list.size(); index++) { + yarp::os::Bottle port_list = config.findGroup("PORT_LIST").tail(); + for (int index = 0; index < port_list.size(); index++) { std::string current_port_property = port_list.get(index).toString(); std::string token; int pos; @@ -26,16 +26,17 @@ bool SystemReady_nws_yarp::open(yarp::os::Searchable &config) pos = current_port_property.find(delimiter); token = current_port_property.substr(0, pos); std::string current_port_name = port_list.find(token).asString(); - yCError(SYSTEMREADY_NWS_YARP) << port_list.get(index).asList()[0].toString(); - yCError(SYSTEMREADY_NWS_YARP) << current_port_property; - yCError(SYSTEMREADY_NWS_YARP) << token; - yCError(SYSTEMREADY_NWS_YARP) << port_list.find(token).asString(); yarp::os::Port* current_port = new yarp::os::Port; - current_port->open(current_port_name); + if(!current_port->open(current_port_name)){ + yCError(SYSTEMREADY_NWS_YARP) << "error opening " << current_port_name; + close(); + return false; + } port_pointers_list.push_back(current_port); } return true; } + yCError(SYSTEMREADY_NWS_YARP) << "missing PORT_LIST group"; return false; } @@ -44,6 +45,7 @@ bool SystemReady_nws_yarp::close() { for (auto elem: port_pointers_list){ elem->close(); - delete elem; } + port_pointers_list.clear(); + return true; } diff --git a/src/devices/systemReady/SystemReady_nws_yarp.h b/src/devices/systemReady/SystemReady_nws_yarp.h index 3b830f1f3bf..9194c5f7a01 100644 --- a/src/devices/systemReady/SystemReady_nws_yarp.h +++ b/src/devices/systemReady/SystemReady_nws_yarp.h @@ -6,7 +6,8 @@ #ifndef YARP_SYSTEMREADY_NWS_YARP_H #define YARP_SYSTEMREADY_NWS_YARP_H -#include +#include +#include #include From 17ddc00bfa527bd5acad0b6941295cf26637fe77 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Tue, 7 Dec 2021 10:39:21 +0100 Subject: [PATCH 3/6] fixed doc --- src/devices/systemReady/SystemReady_nws_yarp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/devices/systemReady/SystemReady_nws_yarp.h b/src/devices/systemReady/SystemReady_nws_yarp.h index 9194c5f7a01..5b74525cd21 100644 --- a/src/devices/systemReady/SystemReady_nws_yarp.h +++ b/src/devices/systemReady/SystemReady_nws_yarp.h @@ -12,7 +12,7 @@ /** - * @ingroup dev_impl_network_clients dev_impl_navigation + * @ingroup dev_impl_network_servers * * \section SystemReady_nws_yarp_parameters Device description * \brief `SystemReady_nws_yarp`: A yarp nws to open port for synchronization: From 0ca1eb74898f45df9a15f424ee98bccad3792c01 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Tue, 7 Dec 2021 10:40:11 +0100 Subject: [PATCH 4/6] added awaitSystemReady --- .../AwaitSystemReady_nws_yarp.cpp | 42 ++++++++++++++ .../AwaitSystemReady_nws_yarp.h | 55 +++++++++++++++++++ src/devices/awaitSystemReady/CMakeLists.txt | 47 ++++++++++++++++ 3 files changed, 144 insertions(+) create mode 100644 src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.cpp create mode 100644 src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.h create mode 100644 src/devices/awaitSystemReady/CMakeLists.txt diff --git a/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.cpp b/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.cpp new file mode 100644 index 00000000000..e91615155bf --- /dev/null +++ b/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.cpp @@ -0,0 +1,42 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: LGPL-2.1-or-later + */ + +#include "AwaitSystemReady_nws_yarp.h" +#include +#include +#include + +YARP_LOG_COMPONENT(AWAITSYSTEMREADY_NWS_YARP, "yarp.devices.AwaitSystemReady_nws_yarp") + +AwaitSystemReady_nws_yarp::AwaitSystemReady_nws_yarp() +{ +} + +bool AwaitSystemReady_nws_yarp::open(yarp::os::Searchable &config) +{ + if (config.check("PORT_LIST")) { + // skips the first one since it is PORT_LIST + yarp::os::Bottle port_list = config.findGroup("PORT_LIST").tail(); + for (int index = 0; index < port_list.size(); index++) { + std::string current_port_property = port_list.get(index).toString(); + std::string token; + int pos; + char delimiter = ' '; + pos = current_port_property.find(delimiter); + token = current_port_property.substr(0, pos); + std::string current_port_name = port_list.find(token).asString(); + yarp::os::NetworkBase::waitPort(current_port_name); + } + return true; + } + yCError(AWAITSYSTEMREADY_NWS_YARP) << "missing PORT_LIST group"; + return false; +} + + +bool AwaitSystemReady_nws_yarp::close() +{ + return true; +} diff --git a/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.h b/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.h new file mode 100644 index 00000000000..bea91e75e64 --- /dev/null +++ b/src/devices/awaitSystemReady/AwaitSystemReady_nws_yarp.h @@ -0,0 +1,55 @@ +/* + * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) + * SPDX-License-Identifier: BSD-3-Clause + */ + +#ifndef YARP_AWAITSYSTEMREADY_NWS_YARP_H + +#include + + +/** + * @ingroup dev_impl_network_servers + * + * \section AwaitSystemReady_nws_yarp Device description + * \brief `AwaitSystemReady_nws_yarp`: A yarp nws to wait port for synchronization: + * it waits for the specified ports inside the PORT_LIST group. + * Related to SystemReady_nws_yarp. + * + * Parameters required by this device are: + * | Parameter name | SubParameter | Type | Units | Default Value | Required | Description | + * |:-------------------:|:-----------------------:|:-------:|:--------------:|:-------------:|:-----------------------------: |:-----------------------------------------------------| + * | PORT_NAME | - | group | | | Yes | the group inside which port names are located | + * + * example of xml file with a fake odometer + * + * \code{.unparsed} + * + * + * + * + * + * + * /pippo + * /pluto + * + * + * + * + * \endcode + * + */ + +class AwaitSystemReady_nws_yarp : + public yarp::dev::DeviceDriver +{ +public: + AwaitSystemReady_nws_yarp(); + + // DeviceDriver + bool open(yarp::os::Searchable ¶ms) override; + bool close() override; + +}; + +#endif // YARP_AWAITSYSTEMREADY_NWS_YARP_H diff --git a/src/devices/awaitSystemReady/CMakeLists.txt b/src/devices/awaitSystemReady/CMakeLists.txt new file mode 100644 index 00000000000..f7347ca2f07 --- /dev/null +++ b/src/devices/awaitSystemReady/CMakeLists.txt @@ -0,0 +1,47 @@ +# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-License-Identifier: BSD-3-Clause + + +yarp_prepare_plugin(awaitSystemReady_nws_yarp + CATEGORY device + TYPE AwaitSystemReady_nws_yarp + INCLUDE AwaitSystemReady_nws_yarp.h + EXTRA_CONFIG + WRAPPER=awaitSystemReady_nws_yarp + DEFAULT ON +) + +if(NOT SKIP_awaitSystemReady_nws_yarp) + yarp_add_plugin(yarp_awaitSystemReady_nws_yarp) + + target_sources(yarp_awaitSystemReady_nws_yarp + PRIVATE + AwaitSystemReady_nws_yarp.cpp + AwaitSystemReady_nws_yarp.h + ) + + target_link_libraries(yarp_awaitSystemReady_nws_yarp + PRIVATE + YARP::YARP_os + YARP::YARP_sig + YARP::YARP_dev + ) + list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS + YARP_os + YARP_sig + YARP_dev + ) + + yarp_install( + TARGETS yarp_awaitSystemReady_nws_yarp + EXPORT YARP_${YARP_PLUGIN_MASTER} + COMPONENT ${YARP_PLUGIN_MASTER} + LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR} + ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR} + YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR} + ) + + set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE) + + set_property(TARGET yarp_awaitSystemReady_nws_yarp PROPERTY FOLDER "Plugins/Device/NWS") +endif() From 61ddb7baf0c9e15d7e7ce001bec2f599ada22af4 Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Tue, 7 Dec 2021 14:40:10 +0100 Subject: [PATCH 5/6] update systemready --- src/devices/systemReady/SystemReady_nws_yarp.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/devices/systemReady/SystemReady_nws_yarp.cpp b/src/devices/systemReady/SystemReady_nws_yarp.cpp index c5d842f6656..a4bfb90932a 100644 --- a/src/devices/systemReady/SystemReady_nws_yarp.cpp +++ b/src/devices/systemReady/SystemReady_nws_yarp.cpp @@ -27,12 +27,12 @@ bool SystemReady_nws_yarp::open(yarp::os::Searchable &config) token = current_port_property.substr(0, pos); std::string current_port_name = port_list.find(token).asString(); yarp::os::Port* current_port = new yarp::os::Port; + port_pointers_list.push_back(current_port); if(!current_port->open(current_port_name)){ yCError(SYSTEMREADY_NWS_YARP) << "error opening " << current_port_name; close(); return false; } - port_pointers_list.push_back(current_port); } return true; } @@ -45,6 +45,7 @@ bool SystemReady_nws_yarp::close() { for (auto elem: port_pointers_list){ elem->close(); + delete elem; } port_pointers_list.clear(); return true; From 1081c5bd00dfb5fa33d7ca4e50194dc31c4b1acd Mon Sep 17 00:00:00 2001 From: Stefano Bernagozzi Date: Tue, 7 Dec 2021 14:40:30 +0100 Subject: [PATCH 6/6] update awaitSystemReady --- src/devices/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/devices/CMakeLists.txt b/src/devices/CMakeLists.txt index 7873afe7e13..9c4e0461467 100644 --- a/src/devices/CMakeLists.txt +++ b/src/devices/CMakeLists.txt @@ -102,6 +102,7 @@ yarp_begin_plugin_library(yarpmod add_subdirectory(mobileBaseVelocityControlMsgs) add_subdirectory(odometry2D) add_subdirectory(systemReady) + add_subdirectory(awaitSystemReady) add_subdirectory(portaudio) # DEPRECATED Since YARP 3.2