diff --git a/README.md b/README.md index da8fe65d27..7da6e2a2ad 100644 --- a/README.md +++ b/README.md @@ -101,6 +101,13 @@ cmake --build . --config Release --target install # :computer: Some utilities The **bipedal-locomotion-framework** ships also some utilities that can help you in the everyday tests on a real robot. You can find them in the [`utilities` folder](./utilities). Each utility contains a well-documented` README` where you can find further details. +# :robot: Devices +The **bipedal-locomotion-framework** also provides YARP devices in the [`devices` folder](./devices). + +| Device | Description | +| :--------------------------------------------: | :----------------------------------------------------------: | +| [`YarpRobotLoggerDevice`](./devices/YarpRobotLoggerDevice) | Logs robot sensor/actuator data, cameras, and exogenous signals to `.mat` files. Controllable via RPC (`record`, `saveData`, `stopRecording`). | + # :snake: Python **bipedal-locomotion-framework** also provides Python bindings. Only a small set of the components implemented in the library have the corresponding Python bindings. diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index 35f4961431..a62deeda27 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -69,6 +69,8 @@ void CreateVectorsCollectionClient(pybind11::module& module) py::arg("handler")) .def("connect", &VectorsCollectionClient::connect) .def("disconnect", &VectorsCollectionClient::disconnect) + .def("is_connected", &VectorsCollectionClient::isConnected) + .def("check_connection", &VectorsCollectionClient::checkConnection) .def("get_metadata", [](VectorsCollectionClient& impl) -> BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata { diff --git a/devices/YarpRobotLoggerDevice/README.md b/devices/YarpRobotLoggerDevice/README.md index 3b6fe796f4..2981fd2919 100644 --- a/devices/YarpRobotLoggerDevice/README.md +++ b/devices/YarpRobotLoggerDevice/README.md @@ -1,140 +1,151 @@ # YARPRobotLoggerDevice -The **YARPRobotLoggerDevice** is a YARP device based on `YarpSensorBridge` and [`robometry`](https://github.com/robotology/robometry) that allows logging data from robot sensors and actuators in a mat file. +The **YARPRobotLoggerDevice** is a YARP device based on `YarpSensorBridge` and [`robometry`](https://github.com/robotology/robometry) that logs data from robot sensors and actuators into `.mat` files. It also supports logging cameras (as `.mp4` videos or image frames), exogenous signals streamed over YARP ports, text logs, and frame transforms. -## Configuration Parameters -The logger is currently supported only for the robots listed in the [application folder](./app/robots). Each robot folder contains: +## Quick start -- `launch-yarp-robot-logger.xml`: Configuration parameters for the `yarprobotinterface` to launch the logger device and associated devices. -- `yarp-robot-logger.xml`: Configuration parameters for the logger device. -- `blf-yarp-robot-logger-interfaces`: Folder containing all interfaces used by the logger device to log data. - -## How to Use the Logger -To use the logger, launch the `yarprobotinterface` with the `launch-yarp-robot-logger.xml` configuration file: +Launch the logger with `yarprobotinterface`: ```console yarprobotinterface --config launch-yarp-robot-logger.xml ``` -When you close the yarprobotinterface, the logger will save the logged data in a mat file. Additionally, a md file will contain information about the software version in the robot setup. If video recording is enabled, a mp4 file with the video recording will also be generated. All these files will be saved in the working directory in which `yarprobotinterface` has been launched. - -## How to log exogenous data -The `YarpRobotLoggerDevice` can also log exogenous data, i.e., data not directly provided by the robot sensors and actuators. To do this: -1. modify the `yarp-robot-logger.xml` file to specify the exogenous data to log -2. modify the application that streams the exogenous data - -### Modification `yarp-robot-logger.xml` configuration file -Modify the [`ExogenousSignalGroup` in the `yarp-robot-logger.xml` file](https://github.com/ami-iit/bipedal-locomotion-framework/blob/a3a8e9cb8a0c3532db81d814d4851009f8134195/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml#L27-L37) to log the data streamed by an application that allows the robot to balance: - ```xml - - - ("balancing") - () - - - - - "/yarp-robot-logger/exogenous_signals/balancing" - - - "/balancing-controller/logger" - - - "balancing" - - - "udp" - - - ``` - -### Stream exogenous data -You need to modify the application that streams the exogenous data to open a port with the name specified in the `remote` parameter of the `balancing` sub-group. For example, if you want to stream the data from the your `balancing` application you need to use `BipedalLocomotion::YarpUtilities::VectorsCollectionServer` class as follows -#### C++ -If your application is written in C++ you can use the `BipedalLocomotion::YarpUtilities::VectorsCollectionServer` class as follows - -```c++ -#include -class Module -{ - BipedalLocomotion::YarpUtilities::VectorsCollectionServer m_vectorsCollectionServer; /**< Logger server. */ -public: - // all the other functions you need -} -``` -The `m_vectorsCollectionServer` helps you to handle the data you want to send and to populate the metadata. To use this functionality, call `BipedalLocomotion::YarpUtilities::VectorsCollectionServer::populateMetadata` during the configuration phase. Once you have finished populating the metadata you should call `BipedalLocomotion::YarpUtilities::VectorsCollectionServer::finalizeMetadata` -```c++ -//This code should go into the configuration phase -auto loggerOption = std::make_shared(rf); -if (!m_vectorsCollectionServer.initialize(loggerOption->getGroup("LOGGER"))) -{ - log()->error("[BalancingController::configure] Unable to configure the server."); - return false; -} - -m_vectorsCollectionServer.populateMetadata("dcm::position::measured", {"x", "y"}); -m_vectorsCollectionServer.populateMetadata("dcm::position::desired", {"x", "y"}); - -m_vectorsCollectionServer.finalizeMetadata(); // this should be called only once +By default (`auto_start_logging: true`) the logger starts recording immediately. When you close `yarprobotinterface` (e.g. with `Ctrl+C`), the recorded data is **automatically saved** before the device shuts down. All output files (`.mat`, `.mp4`, `.md`) are written to the working directory. + +## RPC commands + +The logger exposes an RPC port at `/commands/rpc:i` (default: `/yarp-robot-logger/commands/rpc:i`). You can control recording with the following commands using `yarp rpc`: + +```console +yarp rpc /yarp-robot-logger/commands/rpc:i ``` -In the main loop, add the following code to prepare and populate the data: -```c++ -m_vectorsCollectionServer.prepareData(); // required to prepare the data to be sent -m_vectorsCollectionServer.clearData(); // optional see the documentation +| Command | Description | +|---|---| +| `record` | Start recording. Initializes sensor bridge, cameras, telemetry buffers and begins the periodic acquisition loop. | +| `saveData` | Save the recorded data to a `.mat` file. Recording **continues** after the save completes. Accepts an optional `tag` string that is appended to the filename (e.g. `saveData "walking_test"`). Can be called multiple times during a single recording session. | +| `saveData ""` | Same as above, with the tag included in the output filename: `robot_logger_device__.mat`. | +| `stopRecording` | Stop recording **without saving**. All buffered data is discarded and the device returns to idle mode. | + +After `stopRecording`, you can call `record` again to start a new recording session. -// DCM -m_vectorsCollectionServer.populateData("dcm::position::measured", ); -m_vectorsCollectionServer.populateData("dcm::position::desired", ); +### Example session -m_vectorsCollectionServer.sendData(); +```console +$ yarp rpc /yarp-robot-logger/commands/rpc:i +>> record +Response: [ok] +# ... robot performs some task ... +>> saveData "walking_trial_1" +Response: [ok] +# data saved to robot_logger_device_walking_trial_1_.mat +# recording continues +>> saveData "walking_trial_2" +Response: [ok] +# data saved to robot_logger_device_walking_trial_2_.mat +# recording still continues +>> stopRecording +Response: [ok] +# recording stopped, no additional data saved ``` -**Note:** Replace `` with the actual data you want to log. +### Auto-start mode +When `auto_start_logging` is set to `true` (the default), `record` is called automatically at device startup. If set to `false`, the device starts in idle mode and waits for an explicit `record` RPC command. -#### Python -If your application is written in Python you can use the `BipedalLocomotion.yarp_utilities.VectorsCollectionServer` class as follows -```python -import bipedal_locomotion_framework as blf +## Configuration + +The logger is currently supported for the robots listed in the [application folder](./app/robots). Each robot folder contains: + +- `launch-yarp-robot-logger.xml`: `yarprobotinterface` configuration that launches the logger and associated devices. +- `yarp-robot-logger.xml`: Logger device parameters. +- `blf-yarp-robot-logger-interfaces/`: Interface configuration files for the sensor bridge. + +### Key parameters + +| Parameter | Default | Description | +|---|---|---| +| `sampling_period_in_s` | `0.01` | Logging period in seconds. | +| `port_prefix` | `/yarp-robot-logger` | Prefix for all YARP ports opened by the logger. | +| `auto_start_logging` | `true` | Start recording automatically on device startup. | +| `log_robot_data` | `true` | Log joint states, motor states, FT sensors, IMUs, etc. | +| `log_cameras` | `true` | Log camera images as video or frames. | +| `log_text` | `true` | Log YARP text logging messages. | +| `maximum_admissible_time_step` | - | Max time step between samples (skip logging on clock resets). | + +## Logging exogenous data -class Module: - def __init__(self): - self.vectors_collection_server = blf.yarp_utilities.VectorsCollectionServer() # Logger server. - # all the other functions you need +The logger can also record exogenous data, i.e., signals not directly from robot sensors. To do this: + +1. Configure the `ExogenousSignals` group in `yarp-robot-logger.xml`. +2. Stream data from your application using `VectorsCollectionServer`. + +### Configuration + +Add an `ExogenousSignals` group to `yarp-robot-logger.xml`: + +```xml + + ("balancing") + () + + + "/yarp-robot-logger/exogenous_signals/balancing" + "/balancing-controller/logger" + "balancing" + "udp" + + ``` -The `vectors_collection_server` helps you to handle the data you want to send and to populate the metadata. To use this functionality, call `BipedalLocomotion.yarp_utilities.VectorsCollectionServer.populate_metadata` during the configuration phase. Once you have finished populating the metadata you should call `BipedalLocomotion.yarp_utilities.VectorsCollectionServer.finalize_metadata` -```python -#This code should go into the configuration phase -logger_option = blf.parameters_handler.StdParametersHandler() -logger_option.set_parameter_string("remote", "/test/log") -if not self.vectors_collection_server.initialize(logger_option): - blf.log().error("[BalancingController::configure] Unable to configure the server.") - raise RuntimeError("Unable to configure the server.") - -# populate the metadata -self.vectors_collection_server.populate_metadata("dcm::position::measured", ["x", "y"]) -self.vectors_collection_server.populate_metadata("dcm::position::desired", ["x", "y"]) - -self.vectors_collection_server.finalize_metadata() # this should be called only once when the metadata are ready + +### Streaming from C++ + +```cpp +#include + +// Configuration phase +BipedalLocomotion::YarpUtilities::VectorsCollectionServer server; +server.initialize(loggerOption->getGroup("LOGGER")); +server.populateMetadata("dcm::position::measured", {"x", "y"}); +server.populateMetadata("dcm::position::desired", {"x", "y"}); +server.finalizeMetadata(); + +// Main loop +server.prepareData(); +server.clearData(); +server.populateData("dcm::position::measured", measuredSignal); +server.populateData("dcm::position::desired", desiredSignal); +server.sendData(); ``` -In the main loop, add the following code to prepare and populate the data: + +### Streaming from Python + ```python -self.vectors_collection_server.prepare_data() # required to prepare the data to be sent -self.vectors_collection_server.clear_data() # optional see the documentation -self.vectors_collection_server.populate_data("dcm::position::measured", ) -self.vectors_collection_server.populate_data("dcm::position::desired", ) -self.vectors_collection_server.send_data() +import bipedal_locomotion_framework as blf + +# Configuration phase +server = blf.yarp_utilities.VectorsCollectionServer() +handler = blf.parameters_handler.StdParametersHandler() +handler.set_parameter_string("remote", "/balancing-controller/logger") +server.initialize(handler) +server.populate_metadata("dcm::position::measured", ["x", "y"]) +server.populate_metadata("dcm::position::desired", ["x", "y"]) +server.finalize_metadata() + +# Main loop +server.prepare_data() +server.clear_data() +server.populate_data("dcm::position::measured", measured_signal) +server.populate_data("dcm::position::desired", desired_signal) +server.send_data() ``` -**Note:** Replace `` with the actual data you want to log. -## How to visualize the logged data -To visualize the logged data you can use [robot-log-visualizer](https://github.com/ami-iit/robot-log-visualizer). To use the `robot-log-visualizer` you can follow the instructions in the [README](https://github.com/ami-iit/robot-log-visualizer/blob/main/README.md) file. +## Visualizing logged data + +Use [robot-log-visualizer](https://github.com/ami-iit/robot-log-visualizer) to explore the `.mat` files: -Once you have installed the `robot-log-visualizer` you can open it from the command line with the following command: ```console robot-log-visualizer ``` -Then, you can open the mat file generated by the logger and explore the logged data as in the following video: -[robot-log-visualizer.webm](https://github.com/ami-iit/robot-log-visualizer/assets/16744101/3fd5c516-da17-4efa-b83b-392b5ce1383b) +Then open the `.mat` file generated by the logger. See the [robot-log-visualizer README](https://github.com/ami-iit/robot-log-visualizer/blob/main/README.md) for details. diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml index 0cc7d91aaf..826ecf73b4 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml @@ -10,12 +10,15 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergoCubGazeboV1/yarprobotinterface") + true ("bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh > {filename}_robotology.txt" "apt list --installed > {filename}_apt.txt") 1.0 true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml index c474a89b3c..a7abd77b57 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -10,12 +10,15 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergoCubGazeboV1/yarprobotinterface") + true ("bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh > {filename}_robotology.txt" "apt list --installed > {filename}_apt.txt") 1.0 true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml index 6ee1e0c371..4bcc9a68d1 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml @@ -18,11 +18,14 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergocub-torso/yarprobotinterface") + true ("ssh ergocub@10.0.2.2 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_2.txt" "ssh ergocub@10.0.2.2 'apt list --installed' > {filename}_apt_10_0_2_2.txt" "ssh ergocub@10.0.2.3 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_3.txt" "ssh ergocub@10.0.2.3 'apt list --installed' > {filename}_apt_10_0_2_3.txt") true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml index fa95f58754..0746b0bb28 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml @@ -18,11 +18,14 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergoCubSN001/yarprobotinterface") + true ("ssh ergocub@10.0.2.2 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_2.txt" "ssh ergocub@10.0.2.2 'apt list --installed' > {filename}_apt_10_0_2_2.txt" "ssh ergocub@10.0.2.3 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_3.txt" "ssh ergocub@10.0.2.3 'apt list --installed' > {filename}_apt_10_0_2_3.txt") true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml index 408809226e..a212728285 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml @@ -18,11 +18,14 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergocub-torso/yarprobotinterface") + true ("ssh ergocub@10.0.2.22 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_22.txt" "ssh ergocub@10.0.2.22 'apt list --installed' > {filename}_apt_10_0_2_22.txt" "ssh ergocub@10.0.2.23 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_2_23.txt" "ssh ergocub@10.0.2.23 'apt list --installed' > {filename}_apt_10_0_2_23.txt") true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN003/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN003/yarp-robot-logger.xml index e142d6a82e..d4683183f8 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN003/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN003/yarp-robot-logger.xml @@ -19,8 +19,11 @@ BSD-3-Clause license. --> /yarp-robot-logger true ("ergocub-torso/yarprobotinterface") + true ("ssh ergocub@10.0.0.2 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_0_2.txt" "ssh ergocub@10.0.0.2 'apt list --installed' > {filename}_apt_10_0_0_2.txt" "ssh ergocub@10.0.0.3 'bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh' > {filename}_robotology_10_0_0_3.txt" "ssh ergocub@10.0.0.3 'apt list --installed' > {filename}_apt_10_0_0_3.txt") + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml index da455539f0..5a43112066 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml @@ -8,11 +8,14 @@ BSD-3-Clause license. --> /yarp-robot-logger 1.0 true + true ("bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh > {filename}_robotology.txt" "apt list --installed > {filename}_apt.txt") true false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml index a9f87d4425..506e8ccab3 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml @@ -15,6 +15,8 @@ BSD-3-Clause license. --> false true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 7ef93b9d04..16657a31a2 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -223,10 +223,13 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool m_logCameras{true}; bool m_logFrames{false}; bool m_logRobot{true}; + bool m_autoStartLogging{true}; + bool m_recordingPrepared{false}; std::vector m_textLoggingSubnames; std::vector m_codeStatusCmds; std::mutex m_bufferManagerMutex; + std::mutex m_rpcMutex; /**< Mutex to serialize concurrent RPC command calls. */ robometry::BufferManager m_bufferManager; const std::string m_rpcPortName{"/commands/rpc:i"}; /**< name of Remote @@ -255,6 +258,11 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, VideoWriter& writer, ExogenousSignal>& signal); void saveCodeStatus(const std::string& logPrefix, const std::string& fileName) const; + virtual bool record() override; + virtual bool isRecording() override; + virtual bool saveData(const std::string& tag = "") override; + virtual bool stopRecording() override; + void stopRecordingThreads(); bool setupRobotSensorBridge(std::weak_ptr params); bool setupRobotCameraBridge(std::weak_ptr params); bool setupTelemetry(std::weak_ptr params, @@ -271,7 +279,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool createFramesFolder(std::shared_ptr imageSaver, const std::string& camera, const std::string& imageType); - bool startLogging(); bool prepareRobotLogging(); bool prepareCameraLogging(); bool prepareExogenousImageLogging(); @@ -331,8 +338,6 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, void waitForAcquisitionThreadsToPause(); void resumeAcquisitionThreads(); - - virtual bool saveData(const std::string& tag = "") override; }; } // namespace BipedalLocomotion diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index 389374818f..d935e62fe5 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -212,6 +212,13 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) m_acceptableStep); } + if (!params->getParameter("auto_start_logging", m_autoStartLogging)) + { + log()->info("{} Unable to get the 'auto_start_logging' parameter. Default value: {}.", + logPrefix, + m_autoStartLogging); + } + if (!params->getParameter("log_robot_data", m_logRobot)) { log()->info("{} Unable to get the 'log_robot_data' parameter for the telemetry. Default " @@ -281,7 +288,7 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) fourccCodecUrl); } else if (m_videoCodecCode.size() != 4) { - constexpr auto fourccCodecUrl = "https://abcavi.kibi.ru/fourcc.php"; + constexpr auto fourccCodecUrl = "https://fourcc.org/codecs.php"; log()->error("{} The parameter 'video_codec_code' must be a string with 4 " "characters. You can find the list of supported parameters at: {}.", logPrefix, @@ -369,7 +376,13 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) log()->info("{} Waiting for the attach phases before starting the logging.", logPrefix); } else { - return startLogging(); + if (m_autoStartLogging) + { + return record(); + } + log()->info("{} auto_start_logging is disabled. Logging will not start automatically. " + "Use the RPC command 'record' to start.", + logPrefix); } return true; @@ -463,36 +476,36 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( return true; }; - inputs.clear(); - if (!ptr->getParameter("vectors_exogenous_inputs", inputs)) - { - log()->error("{} Unable to get the exogenous inputs.", logPrefix); - return false; - } + // Helper to open a group of exogenous signal ports. + // If 'required' is true, failure to find the parameter returns false. + // Otherwise, the missing parameter is just a warning and we continue. + auto openExogenousGroup = [&](const std::string& paramName, + auto& signalsMap, + bool required) -> bool { + inputs.clear(); + if (!ptr->getParameter(paramName, inputs)) + { + if (required) + { + log()->error("{} Unable to get the '{}'.", logPrefix, paramName); + return false; + } + log()->warn("{} Unable to get the '{}'. Assuming none.", logPrefix, paramName); + } + return openExogenousSignals(ptr, inputs, signalsMap); + }; - if (!openExogenousSignals(ptr, inputs, m_vectorSignals)) + if (!openExogenousGroup("vectors_exogenous_inputs", m_vectorSignals, true)) { return false; } - inputs.clear(); - if (!ptr->getParameter("string_exogenous_inputs", inputs)) - { - log()->warn("{} Unable to get the string exogenous inputs. Assuming none.", logPrefix); - } - - if (!openExogenousSignals(ptr, inputs, m_stringSignals)) + if (!openExogenousGroup("string_exogenous_inputs", m_stringSignals, false)) { return false; } - inputs.clear(); - if (!ptr->getParameter("image_exogenous_inputs", inputs)) - { - log()->warn("{} Unable to get the image exogenous inputs. Assuming none.", logPrefix); - } - - if (!openExogenousSignals(ptr, inputs, m_imageSignals)) + if (!openExogenousGroup("image_exogenous_inputs", m_imageSignals, false)) { return false; } @@ -503,34 +516,17 @@ bool YarpRobotLoggerDevice::setupExogenousInputs( return false; } - inputs.clear(); - if (!ptr->getParameter("human_state_exogenous_inputs", inputs)) - { - log()->warn("{} Unable to get the human state exogenous inputs. Assuming none.", logPrefix); - } - if (!openExogenousSignals(ptr, inputs, m_humanStateSignals)) + if (!openExogenousGroup("human_state_exogenous_inputs", m_humanStateSignals, false)) { return false; } - inputs.clear(); - if (!ptr->getParameter("wearable_targets_exogenous_inputs", inputs)) - { - log()->warn("{} Unable to get the wearable targets exogenous inputs. Assuming none.", - logPrefix); - } - if (!openExogenousSignals(ptr, inputs, m_wearableTargetsSignals)) + if (!openExogenousGroup("wearable_targets_exogenous_inputs", m_wearableTargetsSignals, false)) { return false; } - inputs.clear(); - if (!ptr->getParameter("wearable_data_exogenous_inputs", inputs)) - { - log()->warn("{} Unable to get the wearable data exogenous inputs. Assuming none.", - logPrefix); - } - if (!openExogenousSignals(ptr, inputs, m_wearableDataSignals)) + if (!openExogenousGroup("wearable_data_exogenous_inputs", m_wearableDataSignals, false)) { return false; } @@ -763,85 +759,23 @@ bool YarpRobotLoggerDevice::setupRobotSensorBridge( } // Get additional flags required by the device - if (!ptr->getParameter("stream_joint_states", m_streamJointStates)) - { - log()->info("{} The 'stream_joint_states' parameter is not found. The joint states is not " - "logged", - logPrefix); - } - - if (!ptr->getParameter("stream_joint_accelerations", m_streamJointAccelerations)) - { - log()->info("{} The 'stream_joint_accelerations' parameter is not found. Set to true by " - "default", - logPrefix); - } - if (!m_streamJointAccelerations) - { - log()->info("{} The joint accelerations is not logged", logPrefix); - } - - if (!ptr->getParameter("stream_motor_temperature", m_streamMotorTemperature)) - { - log()->info("{} The 'stream_motor_temperature' parameter is not found. The motor " - "temperature is not logged", - logPrefix); - } - if (!m_streamMotorTemperature) - { - log()->info("{} The motor temperature is not logged", logPrefix); - } - - if (!ptr->getParameter("stream_motor_states", m_streamMotorStates)) - { - log()->info("{} The 'stream_motor_states' parameter is not found. The motor states is not " - "logged", - logPrefix); - } - - if (!ptr->getParameter("stream_motor_PWM", m_streamMotorPWM)) - { - log()->info("{} The 'stream_motor_PWM' parameter is not found. The motor PWM is not logged", - logPrefix); - } - - if (!ptr->getParameter("stream_pids", m_streamPIDs)) - { - log()->info("{} The 'stream_pids' parameter is not found. The motor pid values are not " - "logged", - logPrefix); - } - - if (!ptr->getParameter("stream_inertials", m_streamInertials)) - { - log()->info("{} The 'stream_inertials' parameter is not found. The IMU values are not " - "logged", - logPrefix); - } - - if (!ptr->getParameter("stream_cartesian_wrenches", m_streamCartesianWrenches)) - { - log()->info("{} The 'stream_cartesian_wrenches' parameter is not found. The cartesian " - "wrench values are not " - "logged", - logPrefix); - } - - if (!ptr->getParameter("stream_forcetorque_sensors", m_streamFTSensors)) - { - log()->info("{} The 'stream_forcetorque_sensors' parameter is not found. The FT values are " - "not " - "logged", - logPrefix); - } + auto getStreamFlag = [&](const std::string& paramName, bool& flag) { + if (!ptr->getParameter(paramName, flag)) + { + log()->info("{} The '{}' parameter is not found. Not logged.", logPrefix, paramName); + } + }; - if (!ptr->getParameter("stream_temperatures", m_streamTemperatureSensors)) - { - log()->info("{} The 'stream_temperatures' parameter is not found. The temperature sensor " - "values are not " - "logged", - logPrefix); - } + getStreamFlag("stream_joint_states", m_streamJointStates); + getStreamFlag("stream_joint_accelerations", m_streamJointAccelerations); + getStreamFlag("stream_motor_temperature", m_streamMotorTemperature); + getStreamFlag("stream_motor_states", m_streamMotorStates); + getStreamFlag("stream_motor_PWM", m_streamMotorPWM); + getStreamFlag("stream_pids", m_streamPIDs); + getStreamFlag("stream_inertials", m_streamInertials); + getStreamFlag("stream_cartesian_wrenches", m_streamCartesianWrenches); + getStreamFlag("stream_forcetorque_sensors", m_streamFTSensors); + getStreamFlag("stream_temperatures", m_streamTemperatureSensors); return true; } @@ -873,6 +807,12 @@ bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey, std::size_t vectorSize, const std::vector& metadataNames) { + // Skip adding channels if they were already registered in a previous recording session + if (m_recordingPrepared) + { + return true; + } + if (metadataNames.empty() || vectorSize != metadataNames.size()) { log()->warn("The metadata names for channel {} are empty or the size of the metadata names " @@ -1121,20 +1061,26 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) log()->info("{} Attach completed. Starting logger.", logPrefix); if (!this->isRunning()) { - return startLogging(); + if (m_autoStartLogging) + { + return record(); + } + log()->info("{} auto_start_logging is disabled. Logging will not start automatically. " + "Use the RPC command 'record' to start.", + logPrefix); } return true; } -bool BipedalLocomotion::YarpRobotLoggerDevice::startLogging() +bool BipedalLocomotion::YarpRobotLoggerDevice::record() { - constexpr auto logPrefix = "[YarpRobotLoggerDevice::startLogging]"; + std::lock_guard lock(m_rpcMutex); + constexpr auto logPrefix = "[YarpRobotLoggerDevice::record]"; if (this->isRunning()) { - log()->error("{} The logger has already started. This should not have happened.", - logPrefix); + log()->warn("{} Recording is already in progress.", logPrefix); return false; } @@ -1153,6 +1099,29 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::startLogging() m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); } + // Reset data arrival flags so fresh data is collected in this recording session. + // Existing YARP connections are preserved — they were either never established + // (first record()) or cleaned up by stopRecordingThreads() on a previous stop. + auto resetExogenousFlags = [](auto& signals) { + for (auto& [name, signal] : signals) + { + signal.dataArrived = false; + } + }; + resetExogenousFlags(m_vectorsCollectionSignals); + resetExogenousFlags(m_vectorSignals); + resetExogenousFlags(m_stringSignals); + resetExogenousFlags(m_humanStateSignals); + resetExogenousFlags(m_wearableTargetsSignals); + resetExogenousFlags(m_wearableDataSignals); + resetExogenousFlags(m_imageSignals); + + // Clear cached metadata for VectorsCollection signals so it is re-fetched + for (auto& [name, signal] : m_vectorsCollectionSignals) + { + signal.metadata.vectors.clear(); + } + // run the thread for reading the exogenous signals m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); }); @@ -1195,7 +1164,9 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::startLogging() return false; } - log()->info("{} The logger has started.", logPrefix); + m_recordingPrepared = true; + + log()->info("{} Recording started.", logPrefix); return true; } @@ -1377,9 +1348,11 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::prepareCameraLogging() return false; } } - ok = m_bufferManager.addChannel({"camera::" + camera + "::rgb", - {1, 1}, // - {"frame_index"}}); + ok = !m_recordingPrepared + ? m_bufferManager.addChannel({"camera::" + camera + "::rgb", + {1, 1}, // + {"frame_index"}}) + : true; if (!ok) { log()->error("{} Unable to add the channel for the camera named {}.", @@ -1446,14 +1419,17 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::prepareCameraLogging() } } - ok = m_bufferManager.addChannel({"camera::" + camera + "::rgb", - {1, 1}, // - {"frame_index"}}); + if (!m_recordingPrepared) + { + ok = m_bufferManager.addChannel({"camera::" + camera + "::rgb", + {1, 1}, // + {"frame_index"}}); - ok = ok - && m_bufferManager.addChannel({"camera::" + camera + "::depth", - {1, 1}, // - {"frame_index"}}); + ok = ok + && m_bufferManager.addChannel({"camera::" + camera + "::depth", + {1, 1}, // + {"frame_index"}}); + } if (!ok) { @@ -1510,9 +1486,12 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::prepareExogenousImageLogging() signal.signalName); ok = false; } - ok = m_bufferManager.addChannel({"exogenous_images::" + signal.signalName + "::rgb", - {1, 1}, // - {"frame_index"}}); + if (!m_recordingPrepared) + { + ok = m_bufferManager.addChannel({"exogenous_images::" + signal.signalName + "::rgb", + {1, 1}, // + {"frame_index"}}); + } if (!ok) { @@ -1648,6 +1627,29 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() } }; + auto checkExogeneousConnections + = [this]( + std::unordered_map& signals) -> void { + for (auto& [name, signal] : signals) + { + if (!signal.connected) + { + continue; + } + + std::lock_guard lock(signal.mutex); + if (!signal.client.checkConnection()) + { + log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Connection lost " + "for exogenous signal '{}'. Will attempt to reconnect.", + name); + signal.connected = false; + signal.dataArrived = false; + signal.metadata.vectors.clear(); + } + } + }; + while (m_lookForNewExogenousSignalIsRunning) { // detect if a clock has been reset @@ -1670,7 +1672,8 @@ void YarpRobotLoggerDevice::lookForExogenousSignals() connectToExogeneous(m_wearableDataSignals); connectToExogeneous(m_imageSignals); - // TODO check for updated metadata from already connected signals + // check if already connected VectorsCollection signals are still alive + checkExogeneousConnections(m_vectorsCollectionSignals); // Start the logging for exogenous images for (auto& [name, signal] : m_imageSignals) @@ -2768,21 +2771,22 @@ bool YarpRobotLoggerDevice::detachAll() return true; } -bool YarpRobotLoggerDevice::close() +void YarpRobotLoggerDevice::stopRecordingThreads() { - // Wait for the run method to finish - stop(); + constexpr auto logPrefix = "[YarpRobotLoggerDevice::stopRecordingThreads]"; - m_rpcPort.close(); - m_statusPort.close(); + // Stop the periodic thread + if (this->isRunning()) + { + this->stop(); + } - // stop all the video thread + // stop all the video threads for (auto& [cameraName, writer] : m_videoWriters) { writer.recordVideoIsRunning = false; } - // close all the thread associated to the video logging for (auto& [cameraName, writer] : m_videoWriters) { if (writer.videoThread.joinable()) @@ -2792,10 +2796,9 @@ bool YarpRobotLoggerDevice::close() } } - // close all the thread associated to the exogenous image logging + // stop all the exogenous image logging threads for (auto& [name, signal] : m_imageSignals) { - m_exogenousImageWriters[signal.signalName].recordVideoIsRunning = false; signal.port.interrupt(); if (m_exogenousImageWriters[signal.signalName].videoThread.joinable()) @@ -2805,7 +2808,7 @@ bool YarpRobotLoggerDevice::close() } } - // close the thread associated to the text logging polling + // stop the text logging polling thread m_lookForNewLogsIsRunning = false; if (m_lookForNewLogsThread.joinable()) { @@ -2813,6 +2816,10 @@ bool YarpRobotLoggerDevice::close() m_lookForNewLogsThread = std::thread(); } + // close the text logging port so it can be reopened on next record() call + m_textLoggingPort.close(); + + // stop the exogenous signal polling thread m_lookForNewExogenousSignalIsRunning = false; if (m_lookForNewExogenousSignalThread.joinable()) { @@ -2820,6 +2827,75 @@ bool YarpRobotLoggerDevice::close() m_lookForNewExogenousSignalThread = std::thread(); } + // Disconnect all exogenous signals so they can be cleanly reconnected on next record() + auto disconnectExogenousSignals = [](auto& signals) { + for (auto& [name, signal] : signals) + { + signal.disconnect(); + signal.connected = false; + } + }; + disconnectExogenousSignals(m_vectorsCollectionSignals); + disconnectExogenousSignals(m_vectorSignals); + disconnectExogenousSignals(m_stringSignals); + disconnectExogenousSignals(m_humanStateSignals); + disconnectExogenousSignals(m_wearableTargetsSignals); + disconnectExogenousSignals(m_wearableDataSignals); + disconnectExogenousSignals(m_imageSignals); + + // Reset pause state so next record() starts cleanly + m_requestPause = false; + m_paused = false; + m_firstRun = true; + + log()->info("{} All recording threads stopped.", logPrefix); +} + +bool BipedalLocomotion::YarpRobotLoggerDevice::isRecording() +{ + std::lock_guard lock(m_rpcMutex); + return this->isRunning(); +} + +bool BipedalLocomotion::YarpRobotLoggerDevice::stopRecording() +{ + std::lock_guard lock(m_rpcMutex); + constexpr auto logPrefix = "[YarpRobotLoggerDevice::stopRecording]"; + + if (!this->isRunning()) + { + log()->warn("{} Recording is not running.", logPrefix); + return false; + } + + log()->info("{} Stopping recording without saving...", logPrefix); + + stopRecordingThreads(); + + log()->info("{} Recording stopped. Data was not saved. " + "Use the RPC command 'record' to start a new recording.", + logPrefix); + + return true; +} + +bool YarpRobotLoggerDevice::close() +{ + constexpr auto logPrefix = "[YarpRobotLoggerDevice::close]"; + + // If recording is active, auto-save the data before shutting down + if (this->isRunning()) + { + log()->info("{} Recording is active. Auto-saving episode before closing...", logPrefix); + this->saveData(""); + } + + // Always stop recording threads on close (saveData no longer stops them) + stopRecordingThreads(); + + m_rpcPort.close(); + m_statusPort.close(); + return true; } @@ -2893,10 +2969,17 @@ void BipedalLocomotion::YarpRobotLoggerDevice::resumeAcquisitionThreads() bool BipedalLocomotion::YarpRobotLoggerDevice::saveData(const std::string& tag) { + std::lock_guard lock(m_rpcMutex); constexpr auto logPrefix = "[YarpRobotLoggerDevice::saveData]"; + if (!this->isRunning()) + { + log()->warn("{} Recording is not running. Nothing to save.", logPrefix); + return false; + } + std::string actualFileName; - log()->info("{} Saving data to .mat file...", logPrefix); + log()->info("{} Saving episode to .mat file...", logPrefix); auto start = BipedalLocomotion::clock().now(); std::string inputFileName = defaultFilePrefix; bool output = false; @@ -2955,8 +3038,9 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::saveData(const std::string& tag) logPrefix, actualFileName, std::chrono::duration(BipedalLocomotion::clock().now() - start)); - // Calling callback manually since it is called only when the saving is automatic - output = this->saveCallback(actualFileName, robometry::SaveCallbackSaveMethod::periodic); + // Use periodic since recording continues after saving + output = this->saveCallback(actualFileName, + robometry::SaveCallbackSaveMethod::periodic); } // If it lasted less than 1 second we wait a bit to avoid that @@ -2968,7 +3052,9 @@ bool BipedalLocomotion::YarpRobotLoggerDevice::saveData(const std::string& tag) BipedalLocomotion::clock().sleepFor(1s - duration); } - log()->info("{} Saving data took {}.", logPrefix, std::chrono::duration(duration)); + log()->info("{} Episode saved in {}. Recording continues.", + logPrefix, + std::chrono::duration(duration)); return output; } diff --git a/devices/YarpRobotLoggerDevice/tests/CMakeLists.txt b/devices/YarpRobotLoggerDevice/tests/CMakeLists.txt index bc64d34331..99cbb4c9e5 100644 --- a/devices/YarpRobotLoggerDevice/tests/CMakeLists.txt +++ b/devices/YarpRobotLoggerDevice/tests/CMakeLists.txt @@ -5,7 +5,7 @@ add_bipedal_test( NAME YarpRobotLoggerDevice SOURCES YarpRobotLoggerDeviceTest.cpp - LINKS YARP::YARP_robotinterface matioCpp::matioCpp BipedalLocomotion::TextLogging) + LINKS YARP::YARP_robotinterface matioCpp::matioCpp BipedalLocomotion::TextLogging BipedalLocomotion::YarpRobotLoggerDeviceCommands) if(TARGET YarpRobotLoggerDeviceUnitTests) target_compile_definitions(YarpRobotLoggerDeviceUnitTests PRIVATE CMAKE_CURRENT_SOURCE_DIR="${CMAKE_CURRENT_SOURCE_DIR}") diff --git a/devices/YarpRobotLoggerDevice/tests/YarpRobotLoggerDeviceTest.cpp b/devices/YarpRobotLoggerDevice/tests/YarpRobotLoggerDeviceTest.cpp index c72ea4b482..3d2011c103 100644 --- a/devices/YarpRobotLoggerDevice/tests/YarpRobotLoggerDeviceTest.cpp +++ b/devices/YarpRobotLoggerDevice/tests/YarpRobotLoggerDeviceTest.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -29,6 +30,9 @@ // matioCpp #include +// Thrift-generated RPC client +#include + // This function populate the YARP_DATA_DIRS environment variable to // ensure that YARP devices from YARP (from YARP install prefix) and BLF (from the build directory of BLF) // can be correctly found and launched by libYARP_robotinterface @@ -69,42 +73,77 @@ bool ensureYARPAndBLFYARPDevicesCanBeFound() return blf_setenv("YARP_DATA_DIRS", new_yarp_data_dirs_value); } -TEST_CASE("Launch simple logger") +// Helper: remove all .mat and .md files in the current directory, renaming them to .bak +void backupExistingFiles() { - // This test launched a dummy logger and verify that it effectively saves some data - // Locate location of .exe file of ipit-hipac-codesys-embobj-bridge, done in getDirectoryOfExecutable() - - // As a preliminary step, we rename any leftover .md and .mat files contained in the current working directory - // to .md.bak and .mat.bak, to ensure that they are not mistakenly files generated by this tests std::filesystem::path currentPath = std::filesystem::current_path(); - for (const auto& entry : std::filesystem::directory_iterator(currentPath)) { - if (entry.is_regular_file()) { + for (const auto& entry : std::filesystem::directory_iterator(currentPath)) + { + if (entry.is_regular_file()) + { std::filesystem::path filePath = entry.path(); std::string extension = filePath.extension().string(); - - if (extension == ".mat" || extension == ".md") { + if (extension == ".mat" || extension == ".md") + { std::filesystem::path newFilePath = filePath; newFilePath += ".bak"; std::filesystem::rename(filePath, newFilePath); } } } +} - // We will be opening yarp ports in the tests, but we do not want to run the yarpserver, so let's set YARP - // in local mode (everything will work fine as long as all the ports are opened in the same process) - yarp::os::Network network; - yarp::os::NetworkBase::setLocalMode(true); +// Helper: find the first .mat file in the current working directory +std::string findMatFile() +{ + std::filesystem::path currentPath = std::filesystem::current_path(); + for (const auto& entry : std::filesystem::directory_iterator(currentPath)) + { + if (entry.is_regular_file() && entry.path().extension().string() == ".mat") + { + return entry.path().string(); + } + } + return ""; +} - // We ensure that YARP_DATA_DIRS contains the values necessary to find YARP and BLF devices - REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound()); +// Helper: count .mat files in the current working directory +int countMatFiles() +{ + int count = 0; + std::filesystem::path currentPath = std::filesystem::current_path(); + for (const auto& entry : std::filesystem::directory_iterator(currentPath)) + { + if (entry.is_regular_file() && entry.path().extension().string() == ".mat") + { + count++; + } + } + return count; +} + +// Ensure YARP is initialized exactly once across all tests. +// Avoids repeated init/fini cycles that cause port address conflicts and SIGSEGV. +void ensureYarpNetworkInitialized() +{ + static bool initialized = false; + if (!initialized) + { + yarp::os::Network::init(); + yarp::os::NetworkBase::setLocalMode(true); + initialized = true; + } +} - // We hardcode the yarprobotinterface configuration file is in the source directory and passed via - // target_compile_definitions - std::filesystem::path pathToXmlConfigurationFile = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / std::filesystem::path("launch-yarp-robot-logger.xml"); +// Helper: start yarprobotinterface from a given XML file +yarp::robotinterface::XMLReaderResult launchRobotInterface(const std::string& xmlFileName) +{ + std::filesystem::path pathToXmlConfigurationFile + = std::filesystem::path(CMAKE_CURRENT_SOURCE_DIR) / std::filesystem::path(xmlFileName); - BipedalLocomotion::log()->info("Loading yarprobotinterface file from {}", pathToXmlConfigurationFile.string()); + BipedalLocomotion::log()->info("Loading yarprobotinterface file from {}", + pathToXmlConfigurationFile.string()); - // Boilerplate to add enable_tags or disable_tags as necessary yarp::os::Property yarprobotinterfaceConfig; yarp::os::Bottle enableTags; yarp::os::Bottle& enableTagsList = enableTags.addList(); @@ -122,107 +161,325 @@ TEST_CASE("Launch simple logger") } yarp::robotinterface::XMLReader yarprobotinterfaceReader; - yarp::robotinterface::XMLReaderResult yarprobotinterfaceInstance = yarprobotinterfaceReader.getRobotFromFile(pathToXmlConfigurationFile.string(), yarprobotinterfaceConfig); + return yarprobotinterfaceReader.getRobotFromFile( + pathToXmlConfigurationFile.string(), yarprobotinterfaceConfig); +} - REQUIRE(yarprobotinterfaceInstance.parsingIsSuccessful); +// Helper: shut down yarprobotinterface +void shutdownRobotInterface(yarp::robotinterface::XMLReaderResult& instance) +{ + BipedalLocomotion::log()->info("Halting yarprobotinterface."); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1)); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown)); +} - // Enter the startup phase, that will open all the devices and call attach if necessary - REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); +// Helper: validate joint positions in the .mat file match the expected values +void validateJointPositions(const std::string& matFile, + const Eigen::VectorXd& expected, + size_t nrOfJoints, + const std::string& rootVarName = "robot_logger_device") +{ + matioCpp::File savedLog(matFile); + REQUIRE(savedLog.isOpen()); - // At this point, the system is running and the thread that called the - // enterPhase methods does not need to do anything else + auto robotVar = savedLog.read(rootVarName); + REQUIRE(robotVar.isValid()); + matioCpp::Struct robotLoggerDeviceStruct = robotVar.asStruct(); + REQUIRE(robotLoggerDeviceStruct.isValid()); - BipedalLocomotion::log()->info("yarprobotinterface successfully loaded and started."); + auto jointsStateVar = robotLoggerDeviceStruct["joints_state"]; + REQUIRE(jointsStateVar.isValid()); + auto positionsVar = jointsStateVar.asStruct()["positions"]; + REQUIRE(positionsVar.isValid()); + auto dataVar = positionsVar.asStruct()["data"]; + REQUIRE(dataVar.isValid()); - // Sleep 0.2 second to collect some data - std::this_thread::sleep_for(std::chrono::milliseconds(200)); + matioCpp::MultiDimensionalArray jointPosLoggedData + = dataVar.asMultiDimensionalArray(); + REQUIRE(jointPosLoggedData.isValid()); - // Now that some time passed, let's read some values that will compare against the values that we will read from the logger, and then close the robotinterface - double sim_joint_1_pos_read = 0.0; - double accelerometer_z_read = 0.0; + Eigen::VectorXd jointPosLogged; + jointPosLogged.resize(nrOfJoints); + for (size_t i = 0; i < jointPosLogged.size(); i++) + { + jointPosLogged(i) = jointPosLoggedData({i, 0, 0}); + } + + REQUIRE(jointPosLogged.isApprox(expected, 1e-14)); +} + +// Helper: validate accelerometer data in the .mat file +void validateAccelerometer(const std::string& matFile, + const Eigen::VectorXd& expected, + size_t nrOfDirections, + const std::string& rootVarName = "robot_logger_device") +{ + matioCpp::File savedLog(matFile); + REQUIRE(savedLog.isOpen()); + + auto robotVar = savedLog.read(rootVarName); + REQUIRE(robotVar.isValid()); + matioCpp::Struct robotLoggerDeviceStruct = robotVar.asStruct(); + REQUIRE(robotLoggerDeviceStruct.isValid()); + + auto accelerometersVar = robotLoggerDeviceStruct["accelerometers"]; + REQUIRE(accelerometersVar.isValid()); + auto sensorVar = accelerometersVar.asStruct()["sim_imu_sensor"]; + REQUIRE(sensorVar.isValid()); + auto dataVar = sensorVar.asStruct()["data"]; + REQUIRE(dataVar.isValid()); + + matioCpp::MultiDimensionalArray accelerometerLoggedData + = dataVar.asMultiDimensionalArray(); + REQUIRE(accelerometerLoggedData.isValid()); + + Eigen::VectorXd accelerometerLogged; + accelerometerLogged.resize(nrOfDirections); + for (size_t i = 0; i < accelerometerLogged.size(); i++) + { + accelerometerLogged(i) = accelerometerLoggedData({i, 0, 0}); + } + + REQUIRE(accelerometerLogged.isApprox(expected, 1e-14)); + // The z component should be near to -9.8 + CHECK(std::abs(accelerometerLogged(2) - (-9.8)) <= 1.0); +} + +// Helper: read current encoder/accelerometer values from the robotinterface devices +struct SensorReadings +{ + Eigen::VectorXd jointPositions; + Eigen::VectorXd accelerometer; + size_t nrOfJoints{4}; + size_t nrOfAccelDirs{3}; +}; + +SensorReadings readSensorValues(yarp::robotinterface::XMLReaderResult& instance) +{ + SensorReadings readings; - // Make sure that the requires devices are there - // These strings are the name attributes of the device tags specified in launch-yarp-robot-logger.xml - CHECK(yarprobotinterfaceInstance.robot.hasDevice("sim_controlboard")); - CHECK(yarprobotinterfaceInstance.robot.hasDevice("sim_imu")); + CHECK(instance.robot.hasDevice("sim_controlboard")); + CHECK(instance.robot.hasDevice("sim_imu")); // Read encoders - yarp::dev::IEncoders* iencs=nullptr; - CHECK(yarprobotinterfaceInstance.robot.device("sim_controlboard").driver()->view(iencs)); + yarp::dev::IEncoders* iencs = nullptr; + CHECK(instance.robot.device("sim_controlboard").driver()->view(iencs)); REQUIRE(iencs != nullptr); - Eigen::VectorXd jointPosRead; - size_t nrOfJoints = 4; - jointPosRead.resize(nrOfJoints); - CHECK(iencs->getEncoders(jointPosRead.data())); + readings.jointPositions.resize(readings.nrOfJoints); + CHECK(iencs->getEncoders(readings.jointPositions.data())); // Read accelerometer - yarp::dev::IThreeAxisLinearAccelerometers* ilinacc=nullptr; - CHECK(yarprobotinterfaceInstance.robot.device("sim_imu").driver()->view(ilinacc)); + yarp::dev::IThreeAxisLinearAccelerometers* ilinacc = nullptr; + CHECK(instance.robot.device("sim_imu").driver()->view(ilinacc)); REQUIRE(ilinacc != nullptr); yarp::sig::Vector accelerometerReadYARP; - Eigen::VectorXd accelerometerRead; - size_t nrOfDirectionsInAccelerometer = 3; - accelerometerRead.resize(nrOfDirectionsInAccelerometer); - // The device only has one sensor + readings.accelerometer.resize(readings.nrOfAccelDirs); size_t sensIndex = 0; double timestamp; CHECK(ilinacc->getThreeAxisLinearAccelerometerMeasure(sensIndex, accelerometerReadYARP, timestamp)); - accelerometerRead = yarp::eigen::toEigen(accelerometerReadYARP); + readings.accelerometer = yarp::eigen::toEigen(accelerometerReadYARP); + return readings; +} - BipedalLocomotion::log()->info("Halting yarprobotinterface."); - REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseInterrupt1)); - REQUIRE(yarprobotinterfaceInstance.robot.enterPhase(yarp::robotinterface::ActionPhaseShutdown)); +TEST_CASE("Auto-start logger saves on close (Ctrl+C)") +{ + // This test verifies the original behavior: with auto_start_logging=true the logger + // records automatically, and when the device is closed (simulating Ctrl+C / shutdown), + // the data is auto-saved to a .mat file. + backupExistingFiles(); - // This call is commented as otherwise it can result in segfault, see https://github.com/ami-iit/bipedal-locomotion-framework/issues/872 - // BipedalLocomotion::log()->info("yarprobotinterface successfully halted."); + ensureYarpNetworkInitialized(); - // Now that the yarprobotinterface has been halted, let's open our .mat files and check if the log is working - currentPath = std::filesystem::current_path(); - std::string matLogFilename; - for (const auto& entry : std::filesystem::directory_iterator(currentPath)) { - if (entry.is_regular_file()) { - std::filesystem::path filePath = entry.path(); - std::string extension = filePath.extension().string(); + REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound()); - if (extension == ".mat") { - matLogFilename = filePath.string(); - } - } - } + auto instance = launchRobotInterface("launch-yarp-robot-logger.xml"); + REQUIRE(instance.parsingIsSuccessful); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); - // This call is commented as otherwise it can result in segfault, see https://github.com/ami-iit/bipedal-locomotion-framework/issues/872 - // BipedalLocomotion::log()->info("Loading saved log from {}", matLogFilename); - matioCpp::File savedLog(matLogFilename); + BipedalLocomotion::log()->info("yarprobotinterface successfully loaded and started."); - REQUIRE(savedLog.isOpen()); + // Collect some data + std::this_thread::sleep_for(std::chrono::milliseconds(200)); - matioCpp::Struct robotLoggerDeviceStruct = savedLog.read("robot_logger_device").asStruct(); + // Read sensor values for later comparison + auto readings = readSensorValues(instance); - matioCpp::MultiDimensionalArray jointPosLoggedData = robotLoggerDeviceStruct["joints_state"].asStruct()["positions"].asStruct()["data"].asMultiDimensionalArray(); - // The values are constant, so we can just check the first one - // The size of the each vector is the number of joints (nrOfJoints) - Eigen::VectorXd jointPosLogged; - jointPosLogged.resize(nrOfJoints); - for(size_t i=0; i < jointPosLogged.size(); i++) - { - jointPosLogged(i) = jointPosLoggedData({i,0,0}); - } + // Shut down (triggers auto-save in close()) + shutdownRobotInterface(instance); - REQUIRE(jointPosLogged.isApprox(jointPosRead, 1e-14)); + // Wait for YARP port threads to fully terminate before the next test + std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // The values are constant, so we can just check the first one - matioCpp::MultiDimensionalArray accelerometerLoggedData = robotLoggerDeviceStruct["accelerometers"].asStruct()["sim_imu_sensor"].asStruct()["data"].asMultiDimensionalArray(); - // The size of the vector is the number of readings of accelerometers (3) - Eigen::VectorXd accelerometerLogged; - accelerometerLogged.resize(nrOfDirectionsInAccelerometer); - for(size_t i=0; i < accelerometerLogged.size(); i++) - { - accelerometerLogged(i) = accelerometerLoggedData({i,0,0}); - } + // Verify .mat file was created + std::string matFile = findMatFile(); + REQUIRE(!matFile.empty()); - REQUIRE(accelerometerLogged.isApprox(accelerometerRead, 1e-14)); + validateJointPositions(matFile, readings.jointPositions, readings.nrOfJoints); + validateAccelerometer(matFile, readings.accelerometer, readings.nrOfAccelDirs); +} - // The z component should be near to -9.8 - CHECK(std::abs(accelerometerLogged(2) - (-9.8)) <= 1.0); +TEST_CASE("RPC record and saveData") +{ + // This test verifies the RPC-driven workflow: + // 1. Start with auto_start_logging=false → device is idle + // 2. Call record() via RPC → recording starts + // 3. Collect some data + // 4. Call saveData("test_tag") → data saved, recording continues + // 5. Verify the .mat file is created with correct data + // 6. Call stopRecording() to stop before shutdown + backupExistingFiles(); + + ensureYarpNetworkInitialized(); + + REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound()); + + auto instance = launchRobotInterface("launch-yarp-robot-logger-no-autostart.xml"); + REQUIRE(instance.parsingIsSuccessful); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); + + BipedalLocomotion::log()->info("yarprobotinterface loaded (no autostart)."); + + // No .mat file should exist yet (no recording has started) + CHECK(countMatFiles() == 0); + + // Connect RPC client to the logger's RPC port + yarp::os::RpcClient rpcClient; + REQUIRE(rpcClient.open("/test/rpc:o")); + REQUIRE(yarp::os::Network::connect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i")); + + YarpRobotLoggerDeviceCommands rpcInterface; + rpcInterface.yarp().attachAsClient(rpcClient); + + // Start recording via RPC + bool recordOk = rpcInterface.record(); + CHECK(recordOk); + + // Collect some data + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // Read sensor values for comparison + auto readings = readSensorValues(instance); + + // Save episode with a tag via RPC (recording continues after save) + bool saveOk = rpcInterface.saveData("test_tag"); + CHECK(saveOk); + + // Verify the .mat file was created + std::string matFile = findMatFile(); + REQUIRE(!matFile.empty()); + + // The filename should contain the tag + CHECK(matFile.find("test_tag") != std::string::npos); + + validateJointPositions(matFile, readings.jointPositions, readings.nrOfJoints, + "robot_logger_device_test_tag"); + validateAccelerometer(matFile, readings.accelerometer, readings.nrOfAccelDirs, + "robot_logger_device_test_tag"); + + // Stop recording before shutdown to avoid an extra auto-save + CHECK(rpcInterface.stopRecording()); + + // Disconnect and close the RPC client before shutting down the device + yarp::os::Network::disconnect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i"); + rpcClient.close(); + shutdownRobotInterface(instance); + + // Wait for YARP port threads to fully terminate before the next test + std::this_thread::sleep_for(std::chrono::milliseconds(500)); +} + +TEST_CASE("RPC record and stopRecording discards data") +{ + // This test verifies that stopRecording discards data without saving: + // 1. Start with auto_start_logging=false → device is idle + // 2. Call record() via RPC → recording starts + // 3. Collect some data + // 4. Call stopRecording() → recording stops, no .mat saved + // 5. Verify no .mat file was created + backupExistingFiles(); + + ensureYarpNetworkInitialized(); + + REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound()); + + auto instance = launchRobotInterface("launch-yarp-robot-logger-no-autostart.xml"); + REQUIRE(instance.parsingIsSuccessful); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); + + BipedalLocomotion::log()->info("yarprobotinterface loaded (no autostart)."); + + // Connect RPC client + yarp::os::RpcClient rpcClient; + REQUIRE(rpcClient.open("/test/rpc:o")); + REQUIRE(yarp::os::Network::connect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i")); + + YarpRobotLoggerDeviceCommands rpcInterface; + rpcInterface.yarp().attachAsClient(rpcClient); + + // Start and then stop recording without saving + CHECK(rpcInterface.record()); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + CHECK(rpcInterface.stopRecording()); + + // No .mat file should be produced + CHECK(countMatFiles() == 0); + + // Disconnect and close the RPC client before shutting down the device + yarp::os::Network::disconnect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i"); + rpcClient.close(); + shutdownRobotInterface(instance); + + // Wait for YARP port threads to fully terminate before the next test + std::this_thread::sleep_for(std::chrono::milliseconds(500)); +} + +TEST_CASE("RPC record, saveData multiple times") +{ + // This test verifies that saveData can be called multiple times during + // a single recording session (recording continues after each save): + // 1. record() → collect data → saveData("first") → .mat created, recording continues + // 2. collect more data → saveData("second") → second .mat created, recording continues + // 3. stopRecording() to end the session + backupExistingFiles(); + + ensureYarpNetworkInitialized(); + + REQUIRE(ensureYARPAndBLFYARPDevicesCanBeFound()); + + auto instance = launchRobotInterface("launch-yarp-robot-logger-no-autostart.xml"); + REQUIRE(instance.parsingIsSuccessful); + REQUIRE(instance.robot.enterPhase(yarp::robotinterface::ActionPhaseStartup)); + + // Connect RPC client + yarp::os::RpcClient rpcClient; + REQUIRE(rpcClient.open("/test/rpc:o")); + REQUIRE(yarp::os::Network::connect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i")); + + YarpRobotLoggerDeviceCommands rpcInterface; + rpcInterface.yarp().attachAsClient(rpcClient); + + // Start recording + CHECK(rpcInterface.record()); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // First save (recording continues) + CHECK(rpcInterface.saveData("first")); + CHECK(countMatFiles() == 1); + + // Collect more data and save again (recording still continues) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + CHECK(rpcInterface.saveData("second")); + CHECK(countMatFiles() == 2); + + // Stop recording before shutdown + CHECK(rpcInterface.stopRecording()); + + // Disconnect and close the RPC client before shutting down the device + yarp::os::Network::disconnect("/test/rpc:o", "/yarp-robot-logger/commands/rpc:i"); + rpcClient.close(); + shutdownRobotInterface(instance); + + // Wait for YARP port threads to fully terminate before the next test + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } diff --git a/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger-no-autostart.xml b/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger-no-autostart.xml new file mode 100644 index 0000000000..eec626e088 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger-no-autostart.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + ("sim_joint_1","sim_joint_2","sim_joint_3","sim_joint_4") + 4 + + + + + + sim_imu_sensor + true + + + + + + diff --git a/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger-no-autostart.xml b/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger-no-autostart.xml new file mode 100644 index 0000000000..3d91d5b6de --- /dev/null +++ b/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger-no-autostart.xml @@ -0,0 +1,59 @@ + + + + + 0.01 + + /yarp-robot-logger + true + ("ergocub-torso/yarprobotinterface") + ("bash ${ROBOTOLOGY_SUPERBUILD_SOURCE_DIR}/scripts/robotologyGitStatus.sh > {filename}_robotology.txt" "apt list --installed > {filename}_apt.txt") + 1.0 + true + true + + false + + false + + "/yarp-robot-logger/rt_logging" + + + + 20.0 + + + + false + true + true + true + false + false + false + false + false + + + (sim_imu_sensor) + (sim_imu_sensor) + (sim_imu_sensor) + (sim_imu_sensor) + + + + + + + + sim_controlboard + sim_imu + + + + + + + diff --git a/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger.xml index 26b54a72bc..417a5ddf67 100644 --- a/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/tests/yarp-robot-logger.xml @@ -14,6 +14,8 @@ BSD-3-Clause license. --> true true + true + false "/yarp-robot-logger/rt_logging" diff --git a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift index 8e4091d1fc..ff2cb1d81f 100644 --- a/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift +++ b/devices/YarpRobotLoggerDevice/thrifts/YarpRobotLoggerDeviceCommands.thrift @@ -8,5 +8,8 @@ service YarpRobotLoggerDeviceCommands { + bool record(); + bool isRecording(); bool saveData(1: string tag = ""); + bool stopRecording(); } diff --git a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h index f1f63f0ccf..eacd0b63f3 100644 --- a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h +++ b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h @@ -65,6 +65,23 @@ class VectorsCollectionClient */ bool disconnect(); + /** + * Check if the client is currently connected (cached state). + * @return true if the client believes it is connected, false otherwise. + * @note This returns the cached connection state. Use checkConnection() to actively verify + * YARP port connectivity. + */ + bool isConnected() const; + + /** + * Actively verify that the YARP port connections are still alive. + * If any connection (data or RPC) is found to be broken, the internal state is updated to + * disconnected. + * @return true if all connections are still alive, false otherwise. + * @note After this method returns false, call connect() to re-establish the connections. + */ + bool checkConnection(); + /** * Check if new metadata is available. * @return true if new metadata is available, false otherwise. diff --git a/src/YarpUtilities/src/VectorsCollectionClient.cpp b/src/YarpUtilities/src/VectorsCollectionClient.cpp index 1cf972c9f3..893629cbc7 100644 --- a/src/YarpUtilities/src/VectorsCollectionClient.cpp +++ b/src/YarpUtilities/src/VectorsCollectionClient.cpp @@ -123,6 +123,41 @@ bool VectorsCollectionClient::disconnect() return true; } +bool VectorsCollectionClient::isConnected() const +{ + return m_pimpl->isConnected; +} + +bool VectorsCollectionClient::checkConnection() +{ + if (!m_pimpl->isConnected) + { + return false; + } + + // Actively verify that both YARP connections (data and RPC) are still alive. + // The carrier must be specified because Network::isConnected() without a carrier + // defaults to checking for "tcp". If the data connection uses a different carrier + // (e.g. "udp"), the check would incorrectly report the connection as lost, + // causing a continuous disconnect/reconnect cycle. + const bool dataConnected + = yarp::os::Network::isConnected(m_pimpl->remotePortName, m_pimpl->localPortName, m_pimpl->carrier); + const bool rpcConnected + = yarp::os::Network::isConnected(m_pimpl->localRpcPortName, m_pimpl->remoteRpcPortName); + + if (!dataConnected || !rpcConnected) + { + log()->warn("[VectorsCollectionClient::checkConnection] Connection lost. Data port " + "connected: {}, RPC port connected: {}.", + dataConnected, + rpcConnected); + m_pimpl->isConnected = false; + return false; + } + + return true; +} + bool VectorsCollectionClient::connect() { constexpr auto rpcCarrier = "tcp"; diff --git a/src/YarpUtilities/tests/VectorsCollectionTest.cpp b/src/YarpUtilities/tests/VectorsCollectionTest.cpp index b15e97fb3f..4e7e5889a9 100644 --- a/src/YarpUtilities/tests/VectorsCollectionTest.cpp +++ b/src/YarpUtilities/tests/VectorsCollectionTest.cpp @@ -406,6 +406,16 @@ TEST_CASE_METHOD(VectorsCollectionFixture, "VectorsCollectionClient - Connection REQUIRE(client.initialize(clientHandler)); + SECTION("isConnected returns false before connect") { + // Test: Client should report disconnected state before connect + REQUIRE_FALSE(client.isConnected()); + } + + SECTION("checkConnection returns false before connect") { + // Test: checkConnection should return false when not connected + REQUIRE_FALSE(client.checkConnection()); + } + SECTION("Successful connection to available server") { // Test: Client should connect when server is available // Behavior: Establishes both data and RPC port connections @@ -413,6 +423,25 @@ TEST_CASE_METHOD(VectorsCollectionFixture, "VectorsCollectionClient - Connection // Allow server ports to fully open std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(client.connect()); + REQUIRE(client.isConnected()); + } + + SECTION("checkConnection returns true when connected") { + // Test: checkConnection should return true when both ports are alive + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(client.connect()); + REQUIRE(client.checkConnection()); + REQUIRE(client.isConnected()); + } + + SECTION("Graceful connection failure when server unavailable") { + // Test: Client should connect when server is available + // Behavior: Establishes both data and RPC port connections + + // Allow server ports to fully open + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(client.connect()); } @@ -859,3 +888,88 @@ TEST_CASE_METHOD(VectorsCollectionFixture, BipedalLocomotion::YarpUtilities::VectorsCollectionMetadata meta; REQUIRE_FALSE(client.getMetadata(meta)); } + +TEST_CASE_METHOD(VectorsCollectionFixture, + "VectorsCollectionClient - checkConnection detects server shutdown") +{ + // Use a heap-allocated server so we can destroy it mid-test + auto server = std::make_unique(); + REQUIRE(server->initialize(serverHandler)); + server->populateMetadata("sig", {"a", "b"}); + REQUIRE(server->finalizeMetadata()); + + VectorsCollectionClient client; + REQUIRE(client.initialize(clientHandler)); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(client.connect()); + REQUIRE(client.isConnected()); + REQUIRE(client.checkConnection()); + + // Destroy the server, which closes its ports + server.reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + // checkConnection should detect the broken connections + REQUIRE_FALSE(client.checkConnection()); + REQUIRE_FALSE(client.isConnected()); + + // After detecting disconnection, connect should fail (no server) + REQUIRE_FALSE(client.connect()); +} + +TEST_CASE_METHOD(VectorsCollectionFixture, + "VectorsCollectionClient - reconnect after server restart") +{ + // Start a server, connect, then destroy and recreate it + auto server = std::make_unique(); + REQUIRE(server->initialize(serverHandler)); + server->populateMetadata("sig", {"x"}); + REQUIRE(server->finalizeMetadata()); + + VectorsCollectionClient client; + REQUIRE(client.initialize(clientHandler)); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(client.connect()); + REQUIRE(client.checkConnection()); + + // Send some data + server->prepareData(); + REQUIRE(server->populateData("sig", std::vector{1.0})); + server->sendData(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto* data = client.readData(false); + REQUIRE(data != nullptr); + REQUIRE(data->vectors.at("sig")[0] == Catch::Approx(1.0)); + + // Destroy the server + server.reset(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + REQUIRE_FALSE(client.checkConnection()); + + // Create a new server on the same ports + server = std::make_unique(); + REQUIRE(server->initialize(serverHandler)); + server->populateMetadata("sig", {"x"}); + REQUIRE(server->finalizeMetadata()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Reconnect + REQUIRE(client.connect()); + REQUIRE(client.isConnected()); + REQUIRE(client.checkConnection()); + + // Verify data flows again + server->prepareData(); + REQUIRE(server->populateData("sig", std::vector{42.0})); + server->sendData(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + data = client.readData(false); + if (data != nullptr) + { + REQUIRE(data->vectors.at("sig")[0] == Catch::Approx(42.0)); + } +}