diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b7109a8a3b..aeb0c3d1ea 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -13,7 +13,7 @@ on: env: vcpkg_robotology_TAG: v0.11.0 YCM_TAG: v0.15.3 - YARP_TAG: v3.8.0 + YARP_TAG: v3.10.1 iDynTree_TAG: v12.3.3 CasADi_TAG: 3.5.5.2 manif_TAG: 0.0.5 diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index 9a571a511a..5342f8c4e6 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.github/workflows/conda-forge-ci.yml @@ -64,6 +64,9 @@ jobs: if: contains(matrix.os, 'windows') shell: cmd /C CALL {0} run: | + :: We delete all the Python executables installed by GitHub Actions as a workaround for https://github.com/ami-iit/bipedal-locomotion-framework/pull/910#issuecomment-2589862552 + :: once that is not necessary anymore, remove this + rmdir /s /q C:\hostedtoolcache\windows\Python :: Due to this https://github.com/conda-forge/icub-models-feedstock/issues/18 :: pcl is removed as a workaround for https://github.com/ami-iit/bipedal-locomotion-framework/pull/695#issuecomment-1632208836 :: pcl can be re-added once we have a ros humble build compatible with PCL 1.13.0 diff --git a/CHANGELOG.md b/CHANGELOG.md index 57b0cb54de..a7462cab84 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,8 +4,11 @@ All notable changes to this project are documented in this file. ## [unreleased] ### Added ### Changed +- Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910) +- Removed ROS1 device publisher and the corresponding example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910) ### Fixed - Add missing `ContinuousDynamicalSystem` row to `Exported components` in the README (https://github.com/ami-iit/bipedal-locomotion-framework/pull/919) +- Avoid using ``iCubGazeboV3`` model in ``BaseEstimatorFromFootIMU`` test (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910) ## [0.20.0] - 2024-12-16 ### Added diff --git a/CMakeLists.txt b/CMakeLists.txt index 677ebe8c4f..db9146fb65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,6 @@ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) option(BUILD_TESTING "Create tests using CMake" OFF) include(CTest) -option(BUILD_DEVICE_EXAMPLES "Create example devices using CMake" OFF) - # Check BipedalLocomotionFramework dependencies, find necessary libraries. include(BipedalLocomotionFrameworkDependencies) diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 10fb7b3d49..529d21c161 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -50,7 +50,7 @@ if(FRAMEWORK_COMPILE_PYTHON_BINDINGS) add_subdirectory(python) # Create the __init__.py file - file(WRITE ${CMAKE_CURRENT_BINARY_DIR}/__init__.py "") + file(WRITE ${BLF_PYTHON_PACKAGE}/__init__.py "") # If we are on Windows and BUILD_SHARED_LIBS is ON, handle the fact that # the Python interpreter does not look into PATH to find dll (see https://docs.python.org/3.8/library/os.html#os.add_dll_directory) @@ -68,6 +68,12 @@ if(FRAMEWORK_COMPILE_PYTHON_BINDINGS) file(APPEND "${BLF_PYTHON_PACKAGE}/__init__.py" "if(library_dll_path != os.path.join(os.environ.get('CONDA_PREFIX', ''),'Library','bin') and library_dll_path != os.path.join(os.environ.get('CONDA_PREFIX', ''),'bin')):${NEW_LINE}") file(APPEND "${BLF_PYTHON_PACKAGE}/__init__.py" " if(os.path.exists(library_dll_path)):${NEW_LINE}") file(APPEND "${BLF_PYTHON_PACKAGE}/__init__.py" " os.add_dll_directory(library_dll_path)${NEW_LINE}") + + # For running tests, we need to add also the path where the dependecies are installed. + # We add idyntree to update the dll path for the tests. + file(APPEND "${BLF_PYTHON_PACKAGE}/__init__.py" "import idyntree${NEW_LINE}") + + endif() file(APPEND "${BLF_PYTHON_PACKAGE}/__init__.py" "from .bindings import *${NEW_LINE}from . import utils${NEW_LINE}") diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 9cb854001f..2ba3685e3b 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -49,7 +49,7 @@ target_link_libraries(pybind11_blf PRIVATE # # The generated Python dynamic module must have the same name as the pybind11 # # module, i.e. `bindings`. set_target_properties(pybind11_blf PROPERTIES - LIBRARY_OUTPUT_DIRECTORY "${BLF_PYTHON_PACKAGE}" + LIBRARY_OUTPUT_DIRECTORY $<1:${BLF_PYTHON_PACKAGE}> # Prevent appending Release/Debug to the output path in Windows OUTPUT_NAME "bindings") if(FRAMEWORK_TEST_PYTHON_BINDINGS) diff --git a/cmake/AddBipedalLocomotionYARPDevice.cmake b/cmake/AddBipedalLocomotionYARPDevice.cmake index 78a703380e..e3df07c62e 100644 --- a/cmake/AddBipedalLocomotionYARPDevice.cmake +++ b/cmake/AddBipedalLocomotionYARPDevice.cmake @@ -2,11 +2,6 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. -framework_dependent_option(FRAMEWORK_COMPILE_example_devices - "Compile example devices?" ON - "BUILD_DEVICE_EXAMPLES" OFF) - - function(add_bipedal_yarp_device) set(options ) set(oneValueArgs NAME) diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index e744c35e07..31f1c01dea 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -141,10 +141,6 @@ framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_USE_YARP" OFF) -framework_dependent_option(FRAMEWORK_COMPILE_Ros1Publisher - "Compile YarpUtilities::RosPublisher class?" ON - "FRAMEWORK_USE_YARP" OFF) - framework_dependent_option(FRAMEWORK_COMPILE_RosImplementation "Compile All the ROS implementations?" ON "FRAMEWORK_USE_rclcpp" OFF) @@ -157,10 +153,6 @@ framework_dependent_option(FRAMEWORK_COMPILE_TomlImplementation "Compile All the TOML implementations?" ON "FRAMEWORK_USE_tomlplusplus" OFF) -framework_dependent_option(FRAMEWORK_COMPILE_Math - "Compile Math library?" ON - "FRAMEWORK_USE_manif" OFF) - framework_dependent_option(FRAMEWORK_COMPILE_Estimators "Compile Estimators library?" ON "" OFF) @@ -181,6 +173,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_System "Compile System library?" ON "" OFF) +framework_dependent_option(FRAMEWORK_COMPILE_Math + "Compile Math library?" ON + "FRAMEWORK_USE_manif;FRAMEWORK_COMPILE_System" OFF) + framework_dependent_option(FRAMEWORK_COMPILE_Unicycle "Compile the Unicycle Planner library?" ON "FRAMEWORK_USE_UnicyclePlanner;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_Planners;FRAMEWORK_COMPILE_Contact" OFF) diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt index 9196e24973..8ee9c8e73d 100644 --- a/devices/CMakeLists.txt +++ b/devices/CMakeLists.txt @@ -7,8 +7,3 @@ add_subdirectory(JointTorqueControlDevice) add_subdirectory(RobotDynamicsEstimatorDevice) add_subdirectory(YarpRobotLoggerDevice) add_subdirectory(VectorsCollectionWrapper) - -if (FRAMEWORK_COMPILE_example_devices) - add_subdirectory(examples) -endif() - diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml index 47905bd907..4afea40e6d 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml @@ -9,6 +9,7 @@ BSD-3-Clause license. --> (30) mp4v /yarp-robot-logger + true ("ergoCubGazeboV1/yarprobotinterface") 1.0 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 6caefa65ff..fe1ca8cc8e 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -9,6 +9,7 @@ BSD-3-Clause license. --> (30) mp4v /yarp-robot-logger + true ("ergoCubGazeboV1/yarprobotinterface") 1.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml index 02e1137531..675b34c01d 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml @@ -17,6 +17,7 @@ BSD-3-Clause license. --> mp4v /yarp-robot-logger + true ("ergocub-torso/yarprobotinterface") ("ssh ergocub@10.0.2.2" "ssh ergocub@10.0.2.3") diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml index 43ea7684ff..cea3faba55 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml @@ -17,6 +17,7 @@ BSD-3-Clause license. --> mp4v /yarp-robot-logger + true ("ergoCubSN001/yarprobotinterface") ("ssh ergocub@10.0.2.2" "ssh ergocub@10.0.2.3") diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml index 61a448d824..036be2cf78 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml @@ -17,6 +17,7 @@ BSD-3-Clause license. --> mp4v /yarp-robot-logger + true ("ergocub-torso/yarprobotinterface") ("ssh ergocub@10.0.2.22" "ssh ergocub@10.0.2.23") diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml index 380340677f..5f9de5786c 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml @@ -8,6 +8,7 @@ BSD-3-Clause license. --> 0.01 /yarp-robot-logger 1.0 + true 600.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml index 19c500b63c..80067ce0e1 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml @@ -9,6 +9,7 @@ BSD-3-Clause license. --> (30) mp4v /yarp-robot-logger + true ("icub-head/yarprobotinterface") ("ssh icub@10.0.0.2" "ssh icub@10.0.0.150") diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index 598e543c09..33e16a129c 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h @@ -174,6 +174,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver, bool m_streamCartesianWrenches{false}; bool m_streamFTSensors{false}; bool m_streamTemperatureSensors{false}; + bool m_logText{true}; std::vector m_textLoggingSubnames; std::vector m_codeStatusCmdPrefixes; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index cdcc546654..9654b51d85 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp @@ -152,13 +152,24 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config) this->setPeriod(devicePeriod); } - if (!params->getParameter("text_logging_subnames", m_textLoggingSubnames)) + if (!params->getParameter("log_text", m_logText)) { - log()->info("{} Unable to get the 'text_logging_subnames' parameter for the telemetry. All " - "the ports related to the text logging will be considered.", - logPrefix); + log()->info("{} Unable to get the 'log_text' parameter for the telemetry. Default value: {}.", + logPrefix, + m_logText); + } + + if (m_logText) + { + if (!params->getParameter("text_logging_subnames", m_textLoggingSubnames)) + { + log()->info("{} Unable to get the 'text_logging_subnames' parameter for the telemetry. " + "All the ports related to the text logging will be considered.", + logPrefix); + } } + if (!params->getParameter("code_status_cmd_prefixes", m_codeStatusCmdPrefixes)) { log()->info("{} Unable to get the 'code_status_cmd_prefixes' parameter. No prefix will be " @@ -672,8 +683,8 @@ bool YarpRobotLoggerDevice::addChannel(const std::string& nameKey, { if (metadataNames.empty() || vectorSize != metadataNames.size()) { - log()->warn("The metadata names are empty or the size of the metadata names is different " - "from the vector size. The default metadata will be used."); + log()->warn("The metadata names for channel {} are empty or the size of the metadata names " + "is different from the vector size. The default metadata will be used.", nameKey); if (!m_bufferManager.addChannel({nameKey, {vectorSize, 1}})) { log()->error("Failed to add the channel in buffer manager named: {}", nameKey); @@ -725,10 +736,14 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly) constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]"; bool ok = true; - // open the TextLogging port - ok = ok && m_textLoggingPort.open(m_textLoggingPortName); - // run the thread - m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); + + if (m_logText) + { + // open the TextLogging port + ok = ok && m_textLoggingPort.open(m_textLoggingPortName); + // run the thread + m_lookForNewLogsThread = std::thread([this] { this->lookForNewLogs(); }); + } // run the thread for reading the exogenous signals m_lookForNewExogenousSignalThread = std::thread([this] { this->lookForExogenousSignals(); }); @@ -1346,6 +1361,13 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit wakeUpTime = time; } wakeUpTime += recordVideoPeriod; + if (wakeUpTime < time) + { + // Before acquiring the image we already spent more time than expected. + // At this point the next frame should be acquired considering the nominal period + // starting from the current time, as if we reset the clock. + wakeUpTime = time + recordVideoPeriod; + } // get the frame from the camera if (writer.rgb != nullptr) @@ -1435,13 +1457,16 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit // release the CPU BipedalLocomotion::clock().yield(); - - if (wakeUpTime < BipedalLocomotion::clock().now()) + auto endTime = BipedalLocomotion::clock().now(); + if (wakeUpTime < endTime) { log()->info("{} The video thread spent more time than expected to save the camera " - "named: {}.", + "named: {}. Expected: {}. Measured: {}. Nominal: {}.", logPrefix, - cameraName); + cameraName, + std::chrono::duration(wakeUpTime - time), + std::chrono::duration(endTime - time), + std::chrono::duration(recordVideoPeriod)); } // sleep @@ -1730,39 +1755,42 @@ void YarpRobotLoggerDevice::run() } } - int bufferportSize = m_textLoggingPort.getPendingReads(); - BipedalLocomotion::TextLoggingEntry msg; - - while (bufferportSize > 0) + if (m_logText) { - yarp::os::Bottle* b = m_textLoggingPort.read(false); - if (b != nullptr) + int bufferportSize = m_textLoggingPort.getPendingReads(); + BipedalLocomotion::TextLoggingEntry msg; + + while (bufferportSize > 0) { - msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, - std::to_string(time)); - if (msg.isValid) + yarp::os::Bottle* b = m_textLoggingPort.read(false); + if (b != nullptr) { - signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName - + "::p" + msg.processPID; + msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, + std::to_string(time)); + if (msg.isValid) + { + signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName + + "::p" + msg.processPID; - // matlab does not support the character - as a key of a struct - findAndReplaceAll(signalFullName, "-", "_"); + // matlab does not support the character - as a key of a struct + findAndReplaceAll(signalFullName, "-", "_"); - // if it is the first time this signal is seen by the device the channel is - // added - if (m_textLogsStoredInManager.find(signalFullName) - == m_textLogsStoredInManager.end()) - { - m_bufferManager.addChannel({signalFullName, {1, 1}}); - m_textLogsStoredInManager.insert(signalFullName); + // if it is the first time this signal is seen by the device the channel is + // added + if (m_textLogsStoredInManager.find(signalFullName) + == m_textLogsStoredInManager.end()) + { + m_bufferManager.addChannel({signalFullName, {1, 1}}); + m_textLogsStoredInManager.insert(signalFullName); + } + // Not using logData here because we don't want to stream the data to RT + m_bufferManager.push_back(msg, time, signalFullName); } - //Not using logData here because we don't want to stream the data to RT - m_bufferManager.push_back(msg, time, signalFullName); + bufferportSize = m_textLoggingPort.getPendingReads(); + } else + { + break; } - bufferportSize = m_textLoggingPort.getPendingReads(); - } else - { - break; } } @@ -1835,12 +1863,22 @@ bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, // save the video if there is any for (const auto& camera : m_rgbCamerasList) { + log()->info("{} Saving the rgb camera named {}.", logPrefix, camera); + + auto start = BipedalLocomotion::clock().now(); + if (!saveVideo(m_videoWriters[camera].rgb, camera, "rgb")) { log()->error("{} Unable to save the rgb for the camera named {}", logPrefix, camera); return false; } + log()->info("{} Saved video {}_{}_{} in {}.", + fileName, + camera, + "rgb", + std::chrono::duration(BipedalLocomotion::clock().now() - start)); + if (method != robometry::SaveCallbackSaveMethod::periodic) { continue; @@ -1873,18 +1911,38 @@ bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, for (const auto& camera : m_rgbdCamerasList) { + log()->info("{} Saving the rgb camera named {}.", logPrefix, camera); + + auto start = BipedalLocomotion::clock().now(); + if (!saveVideo(m_videoWriters[camera].rgb, camera, "rgb")) { log()->error("{} Unable to save the rgb for the camera named {}", logPrefix, camera); return false; } + log()->info("{} Saved video {}_{}_{} in {}.", + fileName, + camera, + "rgb", + std::chrono::duration(BipedalLocomotion::clock().now() - start)); + + log()->info("{} Saving the depth camera named {}.", logPrefix, camera); + + start = BipedalLocomotion::clock().now(); + if (!saveVideo(m_videoWriters[camera].depth, camera, "depth")) { log()->error("{} Unable to save the depth for the camera named {}", logPrefix, camera); return false; } + log()->info("{} Saved video {}_{}_{} in {}.", + fileName, + camera, + "depth", + std::chrono::duration(BipedalLocomotion::clock().now() - start)); + if (method != robometry::SaveCallbackSaveMethod::periodic) { continue; @@ -1943,6 +2001,9 @@ bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, } // save the status of the code + log()->info("{} Saving the status of the code...", logPrefix); + auto start = BipedalLocomotion::clock().now(); + std::ofstream file(fileName + ".md"); file << "# " << fileName << std::endl; file << "File containing all the installed software required to replicate the experiment. " @@ -1969,6 +2030,10 @@ bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName, } file.close(); + log()->info("{} Status of the code saved to file {} in {}.", + logPrefix, + fileName + ".md", + std::chrono::duration(BipedalLocomotion::clock().now() - start)); return true; } @@ -2021,7 +2086,17 @@ bool YarpRobotLoggerDevice::close() bool YarpRobotLoggerDevice::saveData() { + constexpr auto logPrefix = "[YarpRobotLoggerDevice::saveData]"; + std::string fileName; + log()->info("{} Saving data to .mat file...", logPrefix); + auto start = BipedalLocomotion::clock().now(); + m_bufferManager.saveToFile(fileName); + + log()->info("{} Data saved to file {}.mat in {}.", + logPrefix, + fileName, + std::chrono::duration(BipedalLocomotion::clock().now() - start)); return this->saveCallback(fileName, robometry::SaveCallbackSaveMethod::periodic); } diff --git a/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger.xml index b98cc92f52..e3d77c902b 100644 --- a/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/tests/launch-yarp-robot-logger.xml @@ -12,7 +12,7 @@ BSD-3-Clause license. --> - "sim_joint_1" "sim_joint_2" "sim_joint_3" "sim_joint_4" + ("sim_joint_1","sim_joint_2","sim_joint_3","sim_joint_4") 4 diff --git a/devices/examples/CMakeLists.txt b/devices/examples/CMakeLists.txt deleted file mode 100644 index d65eb24c9d..0000000000 --- a/devices/examples/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -add_subdirectory(ROSPublisherTestDevice) - diff --git a/devices/examples/ROSPublisherTestDevice/CMakeLists.txt b/devices/examples/ROSPublisherTestDevice/CMakeLists.txt deleted file mode 100644 index 88bce95198..0000000000 --- a/devices/examples/ROSPublisherTestDevice/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_YarpUtilities AND FRAMEWORK_COMPILE_Ros1Publisher) -# Warning: the option of yarp_configure_plugins_installation should be different from the plugin name -add_bipedal_yarp_device( - NAME ROSPublisherTestDevice - TYPE BipedalLocomotion::ROSPublisherTestDevice - SOURCES src/ROSPublisherTestDevice.cpp - PUBLIC_HEADERS include/BipedalLocomotion/ROSPublisherTestDevice.h - PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation - CONFIGURE_PACKAGE_NAME ros_publisher_test_device) -endif() - diff --git a/devices/examples/ROSPublisherTestDevice/README.md b/devices/examples/ROSPublisherTestDevice/README.md deleted file mode 100644 index 8f8a0b27bb..0000000000 --- a/devices/examples/ROSPublisherTestDevice/README.md +++ /dev/null @@ -1,28 +0,0 @@ -### ROS Publisher Test Device - -- Run the servers - - ```bash - roscore - yarpserver --ros - yarpdev --device transformServer --name tfServer --ROS::enable_ros_publisher true --ROS::enable_ros_subscriber true - ``` - - (This step can also be done using [ros-publisher-test-runner.xml](./ros-publisher-test-runner.xml) through the yarpmanager). - -- Run the device using - - ``` bash - YARP_ROBOT_NAME=iCubGazeboV2_5 yarprobotinterface --config launch-ros-publisher-test.xml - ``` - -- Check the outputs of the test device by running, - - ``` bash - rostopic echo /joint_states # to check the published joint states - name: [hello] position: [20] - rostopic echo /wrench/right # to check the published wrench - frame: right force: x:0.0 y:1.0 z:2.0 torque: x:0.0 y:0.0 z:0.0 - rostopic echo /tf # to check the published pose between /world and /dummy as identity (zero position and unit quaternion) - ``` - - - diff --git a/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt b/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt deleted file mode 100644 index 8397da4302..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -# Get list of models -subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) -# Install each model -foreach (dir ${subdirs}) - yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) -endforeach () - diff --git a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml b/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml deleted file mode 100644 index bb9a274e01..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - diff --git a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml b/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml deleted file mode 100644 index ab2c0f6abf..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - /joint_states - /tfServer - - ("right", "left") - ("/wrench/right", "/wrench/left") - - diff --git a/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h b/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h deleted file mode 100644 index f013ff2f6d..0000000000 --- a/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - * @file ROSPublisherTestDevice.h - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H -#define BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H - -#include -#include -#include -#include - -#include - -namespace BipedalLocomotion -{ - class ROSPublisherTestDevice; -} - -class BipedalLocomotion::ROSPublisherTestDevice : public yarp::dev::DeviceDriver, - public yarp::os::PeriodicThread -{ -public: - ROSPublisherTestDevice(double period, - yarp::os::ShouldUseSystemClock useSystemClock - = yarp::os::ShouldUseSystemClock::No); - ROSPublisherTestDevice(); - ~ROSPublisherTestDevice(); - - virtual bool open(yarp::os::Searchable& config) final; - virtual bool close() final; - virtual void run() final; - - std::unique_ptr pub; -}; - - - -#endif //BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H diff --git a/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml b/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml deleted file mode 100644 index e7ac6cfc11..0000000000 --- a/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml +++ /dev/null @@ -1,18 +0,0 @@ - - ros-publisher-test-launcher - - roscore - - - - yarpserver - --ros - - - - yarpdev - --device transformServer --name tfServer --ROS::enable_ros_publisher true --ROS::enable_ros_subscriber true - - - - diff --git a/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp b/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp deleted file mode 100644 index 2a99b80cc0..0000000000 --- a/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file ROSPublisherTestDevice.cpp - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include -#include -#include -#include - -BipedalLocomotion::ROSPublisherTestDevice::ROSPublisherTestDevice(double period, - yarp::os::ShouldUseSystemClock useSystemClock) - : yarp::os::PeriodicThread(period, useSystemClock) -{ - pub = std::make_unique("/PublisherTest"); -} - -BipedalLocomotion::ROSPublisherTestDevice::ROSPublisherTestDevice() - : yarp::os::PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) -{ - pub = std::make_unique("/PublisherTest"); -} - -BipedalLocomotion::ROSPublisherTestDevice::~ROSPublisherTestDevice() -{ -} - -bool BipedalLocomotion::ROSPublisherTestDevice::open(yarp::os::Searchable& config) -{ - std::shared_ptr configHandler = std::make_shared(); - configHandler->set(config); - - if (!pub->initialize(configHandler)) - { - return false; - } - - this->start(); - return true; -} - -void BipedalLocomotion::ROSPublisherTestDevice::run() -{ - std::vector jointList{"hello"}; - std::vector jointPos{20.0}; - - pub->publishJointStates(jointList, jointPos); - - std::vector wrench{0.0, 1.0, 2.0,0.0, 0.0, 0.0}; - pub->publishWrench("right", wrench); - - - std::vector pose{1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1.}; - pub->publishTransform("/world", "/dummy", pose); -} - - -bool BipedalLocomotion::ROSPublisherTestDevice::close() -{ - this->stop(); - pub->stop(); - return true; -} - diff --git a/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp b/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp index 298229f146..026aa5e2da 100644 --- a/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp +++ b/src/Estimators/tests/FloatingBaseEstimators/BaseEstimatorFromFootIMUTest.cpp @@ -51,7 +51,7 @@ TEST_CASE("BaseEstimatorFromFootIMU") REQUIRE(populateConfig(parameterHandler, footFrameName)); // Load the reduced iDynTree model to be passed to the estimator - const std::string model_path = iCubModels::getModelFile("iCubGazeboV3"); + const std::string model_path = iCubModels::getModelFile("iCubGazeboV2_5"); std::vector joints_list = {"neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", @@ -88,9 +88,15 @@ TEST_CASE("BaseEstimatorFromFootIMU") baseVelocity.setZero(); Eigen::Vector3d gravity; gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - manif::SE3d I = manif::SE3d::Identity(); + manif::SE3d basePose = manif::SE3d::Identity(); + Eigen::Matrix3d alignedRoot; + alignedRoot << -1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0; + Eigen::Quaterniond alignedRootQuaternion(alignedRoot); + basePose.quat(alignedRootQuaternion); - REQUIRE(kinDyn->setRobotState(I.transform(), encoders, baseVelocity, encoder_speeds, gravity)); + REQUIRE(kinDyn->setRobotState(basePose.transform(), encoders, baseVelocity, encoder_speeds, gravity)); BaseEstimatorFromFootIMUInput input; input.jointPositions = encoders; @@ -105,5 +111,5 @@ TEST_CASE("BaseEstimatorFromFootIMU") REQUIRE(estimator.isOutputValid()); constexpr double tolerance = 1e-3; - REQUIRE(estimator.getOutput().basePose.coeffs().isApprox(I.coeffs(), tolerance)); + REQUIRE(estimator.getOutput().basePose.coeffs().isApprox(basePose.coeffs(), tolerance)); } diff --git a/src/YarpUtilities/CMakeLists.txt b/src/YarpUtilities/CMakeLists.txt index a6eccba05b..0a382934e3 100644 --- a/src/YarpUtilities/CMakeLists.txt +++ b/src/YarpUtilities/CMakeLists.txt @@ -5,17 +5,10 @@ # set target name if(FRAMEWORK_COMPILE_YarpUtilities) - set(YarpUtilities_Ros1Publisher_PUBLIC_HEADERS "") - set(YarpUtilities_Ros1Publisher_SOURCES "") - if(FRAMEWORK_COMPILE_Ros1Publisher) - list(APPEND YarpUtilities_Ros1Publisher_PUBLIC_HEADERS "include/BipedalLocomotion/YarpUtilities/RosPublisher.h") - list(APPEND YarpUtilities_Ros1Publisher_SOURCES "src/RosPublisher.cpp") - endif() - add_bipedal_locomotion_library( NAME YarpUtilities - SOURCES src/Helper.cpp ${YarpUtilities_Ros1Publisher_SOURCES} - PUBLIC_HEADERS include/BipedalLocomotion/YarpUtilities/Helper.h include/BipedalLocomotion/YarpUtilities/Helper.tpp ${YarpUtilities_Ros1Publisher_PUBLIC_HEADERS} + SOURCES src/Helper.cpp + PUBLIC_HEADERS include/BipedalLocomotion/YarpUtilities/Helper.h include/BipedalLocomotion/YarpUtilities/Helper.tpp PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::GenericContainer BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging SUBDIRECTORIES tests) diff --git a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h deleted file mode 100644 index 26cfb801e7..0000000000 --- a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h +++ /dev/null @@ -1,154 +0,0 @@ -/** - * @file RosPublisher.h - * @authors Prashanth Ramadoss - * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H -#define BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H - -#include -#include -#include -#include -#include -namespace BipedalLocomotion -{ - -/** - * Helper for YARP library. - */ -namespace YarpUtilities -{ - -/** - * The class internally contains a YARP based ROS node and a set of publishers. - * Current implementation consists of - * - Joint states publisher - * - Wrenches publisher - * - Transform broadcaster - * Although the class might be ROS independent, in order to run the code, ROS is required and usual YARP-ROS connections need to be made. - */ -class [[deprecated("The ROS 1-based BipedalLocomotion::YarpUtilities::RosPublisher is deprecated, use ROS 2 instead.")]] RosPublisher -{ -public: - /** - * Constructor - * @param[in] nodeName Name of the ROS publisher node - */ - RosPublisher(const std::string& nodeName); - - /** - * Destructor - */ - ~RosPublisher(); - - /** - * Configures the publishers and ROS topics for publishing messages - * The parameters used to configure the RosPublisher, - * - "joint_states_topic" name of the topic over which the joint states need to be published - * - "transform_server_port" name of the already running transform server's remote port - * - "WrenchPublishers" is group with the following parameters, - * - "frame_names" a list containing the frames at which the published wrenches should be expressed - * - "topics" a list containing the topics over which the wrenches need to be published. Must be the same size and order as frame_names. - * @param[in] handler Parameter handler - * @note this method needs to be called after construction and before publishing - */ - bool initialize(std::weak_ptr handler); - - /** - * Clear internal configuration and close the node - */ - void stop(); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - * @param[in] jointVelocities list of joint velocities in m/s or rad/s with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions, - const BipedalLocomotion::GenericContainer::Vector::Ref jointVelocities); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - * @param[in] jointVelocities list of joint velocities in m/s or rad/s with the same size as the joints list - * @param[in] jointPositions list of joint torques/forces in Nm or N with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions, - const BipedalLocomotion::GenericContainer::Vector::Ref jointVelocities, - const BipedalLocomotion::GenericContainer::Vector::Ref jointEfforts); - - /** - * Publish the wrenches over the configured wrench topic associated to the frame - * @param[in] frameName frame at which the wrench will be expressed - * @param[in] wrench6d 6d wrench as force-torque in N-Nm with serialziation as fx fy fz tx ty tz - */ - bool publishWrench(const std::string& frameName, - BipedalLocomotion::GenericContainer::Vector::Ref wrench6d); - - /** - * Publish transforms to the transform server - * @param[in] target target frame for the transform - * @param[in] source source frame of the transform - * @param[in] transformAsVector16d 4x4 transform as a vector data in row-major order - */ - bool publishTransform(const std::string& target, const std::string& source, - const BipedalLocomotion::GenericContainer::Vector::Ref transformAsVector16d); - - /** - * Configure which transform server to connect to - * @param[in] transformServerPort name of the transform server port - */ - bool configureTransformServer(const std::string& transformServerPort); - - /** - * Configure the topic over which joint states are published - * @param[in] topicName topic name - */ - bool configureJointStatePublisher(const std::string& topicName); - - /** - * Add or reconfigure a wrench publisher - * @param[in] frameName frame name - * @param[in] topicName topic name - */ - bool configureWrenchPublisher(const std::string& frameName, - const std::string& topicName); - - /** - * Remove wrench publisher - * @param[in] frameName frame name - */ - bool removeWrenchPublisher(const std::string& frameName); - - RosPublisher(const RosPublisher& other) = delete; /**< delete copy constructor */ - RosPublisher(RosPublisher&& other) = delete; /**< delete move constructor */ - RosPublisher& operator=(const RosPublisher& other) = delete; /**< delete copy assignment operator */ - RosPublisher& operator=(RosPublisher&& other) = delete; /**< delete move assignment operator */ -private: - class Impl; - std::unique_ptr m_pimpl; - -}; - -} // namespace YarpUtilities -} // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H - diff --git a/src/YarpUtilities/src/RosPublisher.cpp b/src/YarpUtilities/src/RosPublisher.cpp deleted file mode 100644 index 4162746b4e..0000000000 --- a/src/YarpUtilities/src/RosPublisher.cpp +++ /dev/null @@ -1,497 +0,0 @@ -/** - * @file RosPublisher.cpp - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include "BipedalLocomotion/YarpUtilities/RosPublisher.h" - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include - -using namespace BipedalLocomotion::YarpUtilities; -using namespace BipedalLocomotion; - -template -struct PublisherDetails -{ - PublisherPtr ptr{nullptr}; /**< YARP object for a ROS publisher */ - std::string topic{"/topic"}; /**< topic over which the message is published */ - Msg msg; /**< YARP object for a ROS message */ -}; - -using JointStateMsg = yarp::rosmsg::sensor_msgs::JointState; -using JointStatePublisherPtr = std::unique_ptr< yarp::os::Publisher >; -using WrenchStampedMsg = yarp::rosmsg::geometry_msgs::WrenchStamped; -using WrenchStampedPublisherPtr = std::unique_ptr< yarp::os::Publisher >; -using WrenchPublisherDetails = PublisherDetails; -using JointStatePublisherDetails = PublisherDetails; - -class RosPublisher::Impl -{ -public: - /** - * Get ROS timestamp from Yarp time now - */ - yarp::rosmsg::TickTime getTimeStampFromYarp(); - - /** - * Open the ROS topic for the corresponding publisher - * In ROS terms, advertise the topic over which the node is going to publish - */ - template - bool openPublisher(Publisher* pub, const std::string& topicName) - { - std::string_view printPrefix = "[RosPublisher::Impl::openPublisher] "; - if (!pub->topic(topicName)) - { - std::cerr << printPrefix << "Could not open ROS topic " << topicName << std::endl; - return false; - } - return true; - } - - - template - void configurePublisher(std::unique_ptr >& ptr) - { - if (ptr == nullptr) - { - ptr = std::make_unique< yarp::os::Publisher >(); - } - - ptr->close(); - } - - - /** - * Utility function to load vector parameters - */ - template - bool setupParamV(std::weak_ptr handler, - const std::string& param, std::vector& vec, const std::string& prefix) - { - auto handle = handler.lock(); - if (handle == nullptr) - { - return false; - } - - if (!handle->getParameter(param, vec)) - { - std::cerr << prefix << "The parameter handler could not find \"" << param << "\" in the configuration file. This is a required parameter." << std::endl; - return false; - } - return true; - } - - void resizeJointStateBuffer(const std::size_t& size); - - std::string nodeName; /**< name of the ROS node */ - std::unique_ptr node; /**< YARP object for a ROS node */ - - JointStatePublisherDetails jointStatePublisher; - std::unordered_map wrenchPublisherMap; /**< map of wrench frame and corresponding publisher*/ - - yarp::dev::PolyDriver transformBroadcaster; /**< YARP Polydriver object to open a transform client */ - yarp::dev::IFrameTransform* transformInterface{nullptr}; /**< YARP IFrameTransform interface to publish transforms */ - - bool publishTF{false}; /**< flag to enable publishing transforms to YARP transform server */ - bool initialized{false}; /**< flag to chekc if the publisher was initialized */ - - // placeholder variables - yarp::sig::Matrix pose; /**< placeholder variable for publishing transform data*/ - std::vector jointStateZero; /**< placeholder variable for zero joint state data*/ -}; - -RosPublisher::RosPublisher(const std::string& nodeName) : m_pimpl(std::make_unique()) -{ - m_pimpl->nodeName = nodeName; - m_pimpl->node = std::make_unique(m_pimpl->nodeName); - - m_pimpl->pose.resize(4, 4); - - std::cout << "[RosPublisher] Ensure roscore is running and yarpserver was run with --ros option." << std::endl; -} - -RosPublisher::~RosPublisher() = default; - -bool RosPublisher::initialize(std::weak_ptr handler) -{ - std::string printPrefix{"[RosPublisher::initialize] "}; - bool ok{true}; - - auto handle = handler.lock(); - if (handle == nullptr) - { - std::cerr << printPrefix << "The parameter handler has expired. Please check its scope." << std::endl; - return false; - } - - // configure joint state publisher if it exists - if (handle->getParameter("joint_states_topic", m_pimpl->jointStatePublisher.topic)) - { - if (!configureJointStatePublisher(m_pimpl->jointStatePublisher.topic)) - { - std::cerr << printPrefix << "Could not configure joint states publisher." << std::endl; - return false; - } - } - - // configure transform publisher - std::string tfServerPort; - if (handle->getParameter("transform_server_port", tfServerPort)) - { - m_pimpl->publishTF = true; - if (!configureTransformServer(tfServerPort)) - { - return false; - } - } - - auto wrenchHandle = handle->getGroup("WrenchPublishers"); - auto wrenchHandler = wrenchHandle.lock(); - if (wrenchHandler != nullptr) - { - std::vector frames; - if (!m_pimpl->setupParamV(wrenchHandler, "frame_names", frames, printPrefix)) - { - return false; - } - - std::vector topics; - if (!m_pimpl->setupParamV(wrenchHandler, "topics", topics, printPrefix)) - { - return false; - } - - if (frames.size() != topics.size()) - { - std::cerr << printPrefix << " WrenchPublishers group: frame_names and topics must be of same size." << std::endl; - return false; - } - - for (std::size_t idx = 0; idx < frames.size(); idx++) - { - if (!configureWrenchPublisher(frames[idx], topics[idx])) - { - std::cerr << printPrefix << "Could not configure wrench publisher for frame: " - << frames[idx] << " and topic: " << topics[idx] << std::endl; - return false; - } - } - } - - if (!ok) - { - std::cerr << printPrefix << "Failed to initialize the ROSPublisher" << std::endl; - return false; - } - - m_pimpl->initialized = true; - - return true; -} - -bool RosPublisher::configureJointStatePublisher(const std::string& topicName) -{ - m_pimpl->configurePublisher(m_pimpl->jointStatePublisher.ptr); - - m_pimpl->jointStatePublisher.topic = topicName; - if (!m_pimpl->openPublisher(m_pimpl->jointStatePublisher.ptr.get(), m_pimpl->jointStatePublisher.topic)) - { - return false; - } - - return true; -} - -bool RosPublisher::configureWrenchPublisher(const std::string& frameName, - const std::string& topicName) -{ - std::string_view printPrefix = "[RosPublisher::configureWrenchPublisher] "; - if (m_pimpl->wrenchPublisherMap.find(frameName) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Wrench publisher does not already exist. Adding a wrench publisher for " << frameName << "." << std::endl; - m_pimpl->wrenchPublisherMap[frameName] = WrenchPublisherDetails(); - } - - m_pimpl->configurePublisher(m_pimpl->wrenchPublisherMap.at(frameName).ptr); - m_pimpl->wrenchPublisherMap.at(frameName).topic = topicName; - if (!m_pimpl->openPublisher(m_pimpl->wrenchPublisherMap.at(frameName).ptr.get(), m_pimpl->wrenchPublisherMap.at(frameName).topic)) - { - return false; - } - - return true; -} - -bool RosPublisher::removeWrenchPublisher(const std::string& frameName) -{ - std::string_view printPrefix = "[RosPublisher::removeWrenchPublisher] "; - if (m_pimpl->wrenchPublisherMap.find(frameName) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Wrench publisher does not already exist." << std::endl; - return false; - } - - m_pimpl->wrenchPublisherMap.at(frameName).ptr->close(); - m_pimpl->wrenchPublisherMap.at(frameName).ptr = nullptr; - m_pimpl->wrenchPublisherMap.at(frameName).msg.clear(); - - m_pimpl->wrenchPublisherMap.erase(frameName); - return true; -} - -bool RosPublisher::configureTransformServer(const std::string& transformServerPort) -{ - if (m_pimpl->transformBroadcaster.isValid()) - { - m_pimpl->transformBroadcaster.close(); - m_pimpl->transformInterface = nullptr; - } - - yarp::os::Property tfBroadcasterSettings{{"device", yarp::os::Value("transformClient")}, - {"remote", yarp::os::Value(transformServerPort)}, - {"local", yarp::os::Value(m_pimpl->nodeName + "/transformClient")}}; - - std::string_view printPrefix = "[RosPublisher::openTransformBroadcaster] "; - if (!m_pimpl->transformBroadcaster.open(tfBroadcasterSettings)) - { - std::cerr << printPrefix << "Unable to open transform broadcaster. Ensure transform server is already running." << std::endl; - return false; - } - - if (!m_pimpl->transformBroadcaster.view(m_pimpl->transformInterface)) - { - std::cerr << printPrefix << "Unable to access transform interface." << std::endl; - return false; - } - - return true; -} - -bool RosPublisher::publishTransform(const std::string& target, - const std::string& source, - const BipedalLocomotion::GenericContainer::Vector::Ref transformAsVector16d) -{ - std::string_view printPrefix = "[RosPublisher::publishTransform] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - if (!m_pimpl->publishTF || m_pimpl->transformInterface == nullptr) - { - std::cerr << printPrefix << "Transform broadcaster was not configured. Unable to publish transforms." << std::endl; - return false; - } - - if (transformAsVector16d.size() != 16) - { - std::cerr << printPrefix << "Malformed Transform data. Expecting a 16d vector." << std::endl; - return false; - } - - for (size_t i = 0; i < 4; i++) - { - for (size_t j = 0; j < 4; j++) - { - m_pimpl->pose(i, j) = transformAsVector16d( (i*4) + j); - } - } - - if (!m_pimpl->transformInterface->setTransform(target, source, m_pimpl->pose)) - { - std::cerr << printPrefix << "Could not publish transform for source: " - << source << " and target: " << target << " frames." << std::endl; - return false; - } - - return true; -} - -void RosPublisher::Impl::resizeJointStateBuffer(const std::size_t& size) -{ - if (jointStateZero.size() != size) - { - jointStateZero.resize(size, 0.0); - } -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions) -{ - m_pimpl->resizeJointStateBuffer(jointList.size()); - return publishJointStates(jointList, jointPositions, m_pimpl->jointStateZero, m_pimpl->jointStateZero); -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions, - const GenericContainer::Vector::Ref jointVelocities) -{ - m_pimpl->resizeJointStateBuffer(jointList.size()); - return publishJointStates(jointList, jointPositions, jointVelocities, m_pimpl->jointStateZero); -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions, - const GenericContainer::Vector::Ref jointVelocities, - const GenericContainer::Vector::Ref jointEfforts) -{ - std::string_view printPrefix = "[RosPublisher::publishJointStates] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - auto& pub = m_pimpl->jointStatePublisher; - if (pub.ptr == nullptr) - { - std::cerr << printPrefix << "Joint state publisher was not configured. Unable to publish joint states." << std::endl; - return false; - } - - if ( (jointList.size() != jointPositions.size()) || - (jointList.size() != jointVelocities.size()) || - (jointList.size() != jointEfforts.size())) - { - std::cerr << printPrefix << "Joint states size mismatch. Unable to publish joint states." << std::endl; - return false; - } - - pub.msg.header.seq++; - pub.msg.header.stamp = m_pimpl->getTimeStampFromYarp(); - - pub.msg.name.clear(); - pub.msg.position.clear(); - pub.msg.velocity.clear(); - pub.msg.effort.clear(); - - for (std::size_t idx = 0; idx < jointList.size(); idx++) - { - pub.msg.name.emplace_back(jointList(idx)); - pub.msg.position.emplace_back(jointPositions(idx)); - pub.msg.velocity.emplace_back(jointVelocities(idx)); - pub.msg.effort.emplace_back(jointEfforts(idx)); - } - - pub.ptr->write(pub.msg); - return true; -} - -bool RosPublisher::publishWrench(const std::string& frame, - BipedalLocomotion::GenericContainer::Vector::Ref wrench6d) -{ - std::string_view printPrefix = "[RosPublisher::publishWrench] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - if (m_pimpl->wrenchPublisherMap.find(frame) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Frame does not exist. Please add wrench publisher before publishing." << std::endl; - return false; - } - - auto& pub = m_pimpl->wrenchPublisherMap.at(frame); - if (pub.ptr == nullptr) - { - std::cerr << printPrefix << "Wrench publisher was not configured. Unable to publish wrench stamped messages." << std::endl; - return false; - } - - if (wrench6d.size() != 6) - { - std::cerr << printPrefix << "Expecting a 6d input vector. Unable to publish wrench." << std::endl; - return false; - } - - pub.msg.header.seq++; - pub.msg.header.frame_id = frame; - pub.msg.header.stamp = m_pimpl->getTimeStampFromYarp(); - pub.msg.wrench.force.x = wrench6d(0); - pub.msg.wrench.force.y = wrench6d(1); - pub.msg.wrench.force.z = wrench6d(2); - - pub.msg.wrench.torque.x = wrench6d(3); - pub.msg.wrench.torque.y = wrench6d(4); - pub.msg.wrench.torque.z = wrench6d(5); - pub.ptr->write(pub.msg); - - return true; -} - -yarp::rosmsg::TickTime RosPublisher::Impl::getTimeStampFromYarp() -{ - std::string_view printPrefix = "[RosPublisher::Impl::getTimeStampFromYarp] "; - yarp::rosmsg::TickTime rosTickTime; - - double yarpTimeNow{yarp::os::Time::now()}; - std::chrono::duration timeStamp(yarpTimeNow); - uint64_t secPart = std::chrono::duration_cast(timeStamp).count(); - uint64_t nsecPart = std::chrono::duration_cast(std::chrono::duration(yarpTimeNow - secPart)).count(); - - if (secPart > std::numeric_limits::max()) { - std::cerr << printPrefix - << "Timestamp exceeded the 64 bit representation, resetting it to 0" << std::endl; - secPart = 0; - } - - rosTickTime.sec = static_cast(secPart); - rosTickTime.nsec = static_cast(nsecPart); - - return rosTickTime; -} - - -void RosPublisher::stop() -{ - if (!m_pimpl->wrenchPublisherMap.empty()) - { - std::vector frames; - // use two loops to avoid segfaults due to map::erase in a for loop - // avoid iterator invalidation - for (auto& pair : m_pimpl->wrenchPublisherMap) - { - frames.push_back(pair.first); - } - - for (auto& frame : frames) - { - removeWrenchPublisher(frame); - } - } - - m_pimpl->wrenchPublisherMap.clear(); - if (m_pimpl->jointStatePublisher.ptr != nullptr) - { - m_pimpl->jointStatePublisher.ptr->close(); - m_pimpl->jointStatePublisher.ptr = nullptr; - } - - if (m_pimpl->transformBroadcaster.isValid()) - { - m_pimpl->transformBroadcaster.close(); - } -}