diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bdadd724d7..a8dd45909b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -93,10 +93,6 @@ jobs: cd C:/robotology/vcpkg ./bootstrap-vcpkg.bat - # Install Catch2 - cd C:/robotology/vcpkg - ./vcpkg.exe --overlay-ports=C:/robotology/robotology-vcpkg-ports install catch2:x64-windows - # Install tomlplusplus ./vcpkg.exe --overlay-ports=C:/robotology/robotology-vcpkg-ports install tomlplusplus:x64-windows @@ -453,6 +449,23 @@ jobs: cmake --build . --config ${{ matrix.build_type }} --target install + - name: Catch2 Dependency [Windows] + if: matrix.os == 'windows-2019' + shell: bash + run: | + # Catch2 + git clone -b ${Catch2_TAG} https://github.com/catchorg/Catch2.git + cd Catch2 + mkdir -p build + cd build + cmake -A x64 -DCMAKE_TOOLCHAIN_FILE=${VCPKG_INSTALLATION_ROOT}/scripts/buildsystems/vcpkg.cmake \ + -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps \ + -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} \ + -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps \ + -DBUILD_TESTING=OFF .. + + cmake --build . --config ${{ matrix.build_type }} --target install + - name: Source-based Dependencies [Ubuntu] if: steps.cache-source-deps.outputs.cache-hit != 'true' && startsWith(matrix.os, 'ubuntu') shell: bash diff --git a/.github/workflows/conda-forge-ci.yml b/.github/workflows/conda-forge-ci.yml index ca605d2d0b..6f3e2e5563 100644 --- a/.github/workflows/conda-forge-ci.yml +++ b/.github/workflows/conda-forge-ci.yml @@ -26,7 +26,7 @@ jobs: - uses: conda-incubator/setup-miniconda@v2 with: mamba-version: "*" - channels: conda-forge,robotology + channels: conda-forge,robotology,robostack-humble channel-priority: true - name: Dependencies @@ -38,7 +38,7 @@ jobs: mamba install cmake compilers make ninja pkg-config # Actual dependencies # We ask for numpy 1.22.4 because of https://github.com/ami-iit/bipedal-locomotion-framework/issues/545 - mamba install idyntree "yarp>=3.5.0" libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog "catch2=2" nlohmann_json manif manifpy pybind11 "numpy=1.22.4" pytest scipy opencv pcl tomlplusplus unicycle-footstep-planner "icub-models>=1.23.4" + mamba install idyntree "yarp>=3.5.0" libmatio matio-cpp lie-group-controllers eigen qhull "casadi>=3.5.5" cppad spdlog "catch2=2" nlohmann_json manif manifpy pybind11 "numpy=1.22.4" pytest scipy opencv pcl tomlplusplus unicycle-footstep-planner "icub-models>=1.23.4" ros-humble-desktop - name: Linux-only Dependencies [Linux] if: contains(matrix.os, 'ubuntu') diff --git a/CHANGELOG.md b/CHANGELOG.md index d7a25fe464..7b378c87df 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ All notable changes to this project are documented in this file. ## [unreleased] ### Added - Log the status of the system in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/571) +- Add the `ROS2` implementation for Clock class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/575) ### Changed - YARP devices are now enabled by default if YARP is found (https://github.com/ami-iit/bipedal-locomotion-framework/pull/576). diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index affd3eacbe..24c30b4a1f 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -32,6 +32,13 @@ else () endif() mark_as_advanced(FRAMEWORK_COMPILE_BINDINGS_YarpSystem) +if (FRAMEWORK_COMPILE_System AND FRAMEWORK_COMPILE_RosImplementation) + set(FRAMEWORK_COMPILE_BINDINGS_RosSystem ON) +else () + set(FRAMEWORK_COMPILE_BINDINGS_RosSystem OFF) +endif() +mark_as_advanced(FRAMEWORK_COMPILE_BINDINGS_RosSystem) + configure_file_with_cmakeif(${CMAKE_CURRENT_SOURCE_DIR}/bipedal_locomotion_framework.cpp.in ${CMAKE_CURRENT_BINARY_DIR}/bipedal_locomotion_framework.cpp @ONLY) diff --git a/bindings/python/System/CMakeLists.txt b/bindings/python/System/CMakeLists.txt index 22c754dfe9..772615aa14 100644 --- a/bindings/python/System/CMakeLists.txt +++ b/bindings/python/System/CMakeLists.txt @@ -25,4 +25,16 @@ if(FRAMEWORK_COMPILE_System) endif() + if(FRAMEWORK_COMPILE_RosImplementation) + + add_bipedal_locomotion_python_module( + NAME SystemRosImplementationBindings + SOURCES src/RosClock.cpp src/RosModule.cpp + HEADERS ${H_PREFIX}/RosClock.h ${H_PREFIX}/RosModule.h + LINK_LIBRARIES BipedalLocomotion::SystemRosImplementation + ) + + endif() + + endif() diff --git a/bindings/python/System/include/BipedalLocomotion/bindings/System/RosClock.h b/bindings/python/System/include/BipedalLocomotion/bindings/System/RosClock.h new file mode 100644 index 0000000000..95bda38bc6 --- /dev/null +++ b/bindings/python/System/include/BipedalLocomotion/bindings/System/RosClock.h @@ -0,0 +1,27 @@ +/** + * @file RosClock.h + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_CLOCK_H +#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_CLOCK_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace System +{ + +void CreateRosClock(pybind11::module& module); +void CreateRosClockFactory(pybind11::module& module); + +} // namespace System +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_CLOCK_H diff --git a/bindings/python/System/include/BipedalLocomotion/bindings/System/RosModule.h b/bindings/python/System/include/BipedalLocomotion/bindings/System/RosModule.h new file mode 100644 index 0000000000..6b268ffa8e --- /dev/null +++ b/bindings/python/System/include/BipedalLocomotion/bindings/System/RosModule.h @@ -0,0 +1,26 @@ +/** + * @file RosModule.h + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_MODULE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_MODULE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace System +{ + +void CreateRosModule(pybind11::module& module); + +} // namespace System +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_ROS_MODULE_H diff --git a/bindings/python/System/src/RosClock.cpp b/bindings/python/System/src/RosClock.cpp new file mode 100644 index 0000000000..7306d57f4e --- /dev/null +++ b/bindings/python/System/src/RosClock.cpp @@ -0,0 +1,45 @@ +/** + * @file RosClock.cpp + * @authors Giulio Romualdi + * @copyright 2022 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 + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace System +{ + +void CreateRosClock(pybind11::module& module) +{ + namespace py = ::pybind11; + + py::class_<::BipedalLocomotion::System::RosClock, + ::BipedalLocomotion::System::IClock>(module, "RosClock"); +} + +void CreateRosClockFactory(pybind11::module& module) +{ + namespace py = ::pybind11; + + py::class_<::BipedalLocomotion::System::RosClockFactory, + ::BipedalLocomotion::System::ClockFactory, + std::shared_ptr<::BipedalLocomotion::System::RosClockFactory>>(module, + "RosClockFactory") + .def(py::init&>(), py::arg("args")); + ; +} + +} // namespace System +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/System/src/RosModule.cpp b/bindings/python/System/src/RosModule.cpp new file mode 100644 index 0000000000..f4d31a28fc --- /dev/null +++ b/bindings/python/System/src/RosModule.cpp @@ -0,0 +1,26 @@ +/** + * @file RosModule.cpp + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace System +{ +void CreateRosModule(pybind11::module& module) +{ + CreateRosClock(module); + CreateRosClockFactory(module); +} +} // namespace System +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index d789e54bf3..161d381142 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -29,6 +29,10 @@ #include @endcmakeif FRAMEWORK_COMPILE_BINDINGS_YarpSystem +@cmakeif FRAMEWORK_COMPILE_BINDINGS_RosSystem +#include +@endcmakeif FRAMEWORK_COMPILE_BINDINGS_RosSystem + @cmakeif FRAMEWORK_COMPILE_Contact #include @endcmakeif FRAMEWORK_COMPILE_Contact @@ -104,6 +108,10 @@ PYBIND11_MODULE(bindings, m) bindings::System::CreateYarpModule(systemModule); @endcmakeif FRAMEWORK_COMPILE_BINDINGS_YarpSystem + @cmakeif FRAMEWORK_COMPILE_BINDINGS_RosSystem + bindings::System::CreateRosModule(systemModule); + @endcmakeif FRAMEWORK_COMPILE_BINDINGS_RosSystem + @cmakeif FRAMEWORK_COMPILE_Contact py::module contactsModule = m.def_submodule("contacts"); bindings::Contacts::CreateModule(contactsModule); diff --git a/cmake/AddUninstallTarget.cmake b/cmake/AddUninstallTarget.cmake index 4d7811a4a5..63b3fce8c3 100644 --- a/cmake/AddUninstallTarget.cmake +++ b/cmake/AddUninstallTarget.cmake @@ -1,44 +1,58 @@ -#.rst: -# AddUninstallTarget -# ------------------ -# -# Add the "uninstall" target for your project:: -# -# include(AddUninstallTarget) -# -# -# will create a file cmake_uninstall.cmake in the build directory and add a -# custom target uninstall that will remove the files installed by your package -# (using install_manifest.txt) - -#============================================================================= -# Copyright 2008-2013 Kitware, Inc. -# Copyright 2013 iCub Facility, Istituto Italiano di Tecnologia -# Authors: Daniele E. Domenichelli -# -# Distributed under the OSI-approved BSD License (the "License"); -# see accompanying file Copyright.txt for details. -# -# This software is distributed WITHOUT ANY WARRANTY; without even the -# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -# See the License for more information. -#============================================================================= -# (To distribute this file outside of CMake, substitute the full -# License text for the above reference.) - - -if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) +# SPDX-FileCopyrightText: 2012-2021 Istituto Italiano di Tecnologia (IIT) +# SPDX-FileCopyrightText: 2008-2013 Kitware Inc. +# SPDX-License-Identifier: BSD-3-Clause + +#[=======================================================================[.rst: +AddUninstallTarget +------------------ + +Add the "uninstall" target for your project:: + + include(AddUninstallTarget) + + +will create a file ``cmake_uninstall.cmake`` in the build directory and add a +custom target ``uninstall`` (or ``UNINSTALL`` on Visual Studio and Xcode) that +will remove the files installed by your package (using +``install_manifest.txt``). +See also +https://gitlab.kitware.com/cmake/community/wikis/FAQ#can-i-do-make-uninstall-with-cmake + +The :module:`AddUninstallTarget` module must be included in your main +``CMakeLists.txt``. If included in a subdirectory it does nothing. +This allows you to use it safely in your main ``CMakeLists.txt`` and include +your project using ``add_subdirectory`` (for example when using it with +:cmake:module:`FetchContent`). + +If the ``uninstall`` target already exists, the module does nothing. +#]=======================================================================] + + +# AddUninstallTarget works only when included in the main CMakeLists.txt +if(NOT "${CMAKE_CURRENT_BINARY_DIR}" STREQUAL "${CMAKE_BINARY_DIR}") + return() +endif() + +# The name of the target is uppercase in MSVC and Xcode (for coherence with the +# other standard targets) +if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") + set(_uninstall "UNINSTALL") +else() + set(_uninstall "uninstall") +endif() + +# If target is already defined don't do anything +if(TARGET ${_uninstall}) return() endif() -set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) -set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) +set(_filename cmake_uninstall.cmake) -file(WRITE ${_filename} - "if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") - message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") - return() +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/${_filename}" +"if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") + message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") + return() endif() file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) @@ -46,25 +60,33 @@ string(STRIP \"\${files}\" files) string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\") list(REVERSE files) foreach(file \${files}) + if(IS_SYMLINK \"\$ENV{DESTDIR}\${file}\" OR EXISTS \"\$ENV{DESTDIR}\${file}\") message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") - if(EXISTS \"\$ENV{DESTDIR}\${file}\") - execute_process( - COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" - OUTPUT_VARIABLE rm_out - RESULT_VARIABLE rm_retval) - if(NOT \"\${rm_retval}\" EQUAL 0) - message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") - endif() - else() - message(STATUS \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\") + execute_process( + COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" + OUTPUT_VARIABLE rm_out + RESULT_VARIABLE rm_retval) + if(NOT \"\${rm_retval}\" EQUAL 0) + message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") endif() + else() + message(STATUS \"Not-found: \$ENV{DESTDIR}\${file}\") + endif() endforeach(file) ") -if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") - set(_uninstall "UNINSTALL") +set(_desc "Uninstall the project...") +if(CMAKE_GENERATOR STREQUAL "Unix Makefiles") + set(_comment COMMAND \$\(CMAKE_COMMAND\) -E cmake_echo_color --switch=$\(COLOR\) --cyan "${_desc}") else() - set(_uninstall "uninstall") + set(_comment COMMENT "${_desc}") endif() -add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") +add_custom_target(${_uninstall} + ${_comment} + COMMAND ${CMAKE_COMMAND} -P ${_filename} + USES_TERMINAL + BYPRODUCTS uninstall_byproduct + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") +set_property(SOURCE uninstall_byproduct PROPERTY SYMBOLIC 1) + set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index 2ac17f9fff..cdd17b46ef 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -19,6 +19,10 @@ dependency_classifier(spdlog MINIMUM_VERSION 1.5.0 IS_USED TRUE PUBLIC) ########################## Optional dependencies ############################## +find_package(rclcpp 16.0.0 QUIET) +checkandset_dependency(rclcpp MINIMUM_VERSION 16.0.0) +dependency_classifier(rclcpp MINIMUM_VERSION 16.0.0 IS_USED ${FRAMEWORK_USE_rclcpp} PUBLIC) + find_package(YARP 3.7.0 COMPONENTS companion profiler dev os idl_tools QUIET) checkandset_dependency(YARP MINIMUM_VERSION 3.7.0) dependency_classifier(YARP MINIMUM_VERSION 3.7.0 IS_USED ${FRAMEWORK_USE_YARP} @@ -108,6 +112,10 @@ framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_USE_YARP" OFF) +framework_dependent_option(FRAMEWORK_COMPILE_RosImplementation + "Compile All the ROS implementations?" ON + "FRAMEWORK_USE_rclcpp" OFF) + framework_dependent_option(FRAMEWORK_COMPILE_YarpImplementation "Compile All the YARP implementations?" ON "FRAMEWORK_COMPILE_YarpUtilities" OFF) diff --git a/src/System/CMakeLists.txt b/src/System/CMakeLists.txt index e6c72480b5..2d9acb06dd 100644 --- a/src/System/CMakeLists.txt +++ b/src/System/CMakeLists.txt @@ -22,7 +22,7 @@ if(FRAMEWORK_COMPILE_System) src/StdClock.cpp src/Clock.cpp src/QuitHandler.cpp src/Barrier.cpp src/ConstantWeightProvider.cpp PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler Eigen3::Eigen - SUBDIRECTORIES tests YarpImplementation + SUBDIRECTORIES tests YarpImplementation RosImplementation ) endif() diff --git a/src/System/RosImplementation/CMakeLists.txt b/src/System/RosImplementation/CMakeLists.txt new file mode 100644 index 0000000000..284774e503 --- /dev/null +++ b/src/System/RosImplementation/CMakeLists.txt @@ -0,0 +1,14 @@ +# 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_RosImplementation) + + add_bipedal_locomotion_library( + NAME SystemRosImplementation + SOURCES src/RosClock.cpp + PUBLIC_HEADERS include/BipedalLocomotion/System/RosClock.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::System rclcpp::rclcpp + INSTALLATION_FOLDER System) + +endif() diff --git a/src/System/RosImplementation/include/BipedalLocomotion/System/RosClock.h b/src/System/RosImplementation/include/BipedalLocomotion/System/RosClock.h new file mode 100644 index 0000000000..99fd18d3ac --- /dev/null +++ b/src/System/RosImplementation/include/BipedalLocomotion/System/RosClock.h @@ -0,0 +1,114 @@ +/** + * @file RosClock.h + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_SYSTEM_ROS_CLOCK_H +#define BIPEDAL_LOCOMOTION_SYSTEM_ROS_CLOCK_H + +#include + +#include + +#include + +namespace BipedalLocomotion +{ +namespace System +{ + +/** + * RosClock implements the IClock interface using ros functions. + * The clock can be easily used as follows + * \code{.cpp} + * #include + * #include + * + * #include + * #include + * + * int main(int argc, char *argv[]) + * { + * // Change the clock + * BipedalLocomotion::System::ClockBuilder::setFactory(std::make_shared(argc, argv)); + * + * // Add a sleep + * BipedalLocomotion::clock().sleepFor(2000ms); + * + * auto start = BipedalLocomotion::clock().now(); + * foo(); + * auto end = BipedalLocomotion::clock().now(); + * std::chrono::duration elapsed = end - start; + * + * return 0; + * } + * \endcode + */ +class RosClock final : public IClock +{ + + rclcpp::Clock m_clock; /**< Ros2 clock */ + +public: + /** + * Get ROS current time + * @return the current time computed from `rclcpp::Clock::now()` + * @note `BipedalLocomotion::clock().now().count()` returns a double containing the seconds + * since epoch. + */ + std::chrono::duration now() final; + + /** + * Blocks the execution of the current thread for at least the specified sleepDuration. + * @param time duration to sleep + */ + void sleepFor(const std::chrono::duration& sleepDuration) final; + + /** + * Blocks the execution of the current thread until specified sleepTime has been reached. + * @param time to block until + */ + void sleepUntil(const std::chrono::duration& sleepTime) final; + + /** + * Provides a hint to the implementation to reschedule the execution of threads, allowing other + * threads to run. + */ + void yield() final; +}; + +class RosClockFactory final : public ClockFactory +{ +public: + /** + * Constructor of the factory. + * @param argc number of command-line arguments to parse. + * @param argv array of command-line arguments to parse. + * @note This function will call `rclcpp::init(argc, argv)`. Please check + * [here](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/function_namespacerclcpp_1a026b2ac505c383735117de5d1679ed80.html?highlight=init) + * for further details. + */ + RosClockFactory(int argc, char const* argv[]); + + /** + * Constructor of the factory. + * @param args array of command-line arguments to parse. + * @note This function will call `rclcpp::init(argc, argv)`. Please check + * [here](https://docs.ros.org/en/ros2_packages/rolling/api/rclcpp/generated/function_namespacerclcpp_1a026b2ac505c383735117de5d1679ed80.html?highlight=init) + * for further details. + */ + RosClockFactory(const std::vector& args); + + /** + * Create the ROS clock as a singleton + * @return the reference to a System::RosClock + */ + IClock& createClock() final; +}; + +} // namespace System +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_SYSTEM_ROS_CLOCK_H diff --git a/src/System/RosImplementation/src/RosClock.cpp b/src/System/RosImplementation/src/RosClock.cpp new file mode 100644 index 0000000000..bd9550d9bb --- /dev/null +++ b/src/System/RosImplementation/src/RosClock.cpp @@ -0,0 +1,69 @@ +/** + * @file RosClock.cpp + * @authors Giulio Romualdi + * @copyright 2022 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 + +using namespace BipedalLocomotion::System; + +std::chrono::duration RosClock::now() +{ + return std::chrono::duration(m_clock.now().seconds()); +} + +void RosClock::sleepFor(const std::chrono::duration& sleepDuration) +{ + // std::chrono::duration store the time in second + m_clock.sleep_for(sleepDuration); +} + +void RosClock::sleepUntil(const std::chrono::duration& sleepTime) +{ + m_clock.sleep_for(sleepTime - this->now()); +} + +void RosClock::yield() +{ +} + +RosClockFactory::RosClockFactory(int argc, char const* argv[]) + : ClockFactory() +{ + try + { + rclcpp::init(argc, argv); + } catch (const rclcpp::ContextAlreadyInitialized& except) + { + }; +} + +RosClockFactory::RosClockFactory(const std::vector& args) + : ClockFactory() +{ + std::vector rargs(args.size(), 0); + for (int i = 0; i < args.size(); ++i) + { + rargs[i] = args[i].c_str(); + } + try + { + rclcpp::init(rargs.size(), rargs.data()); + } catch (const rclcpp::ContextAlreadyInitialized& except) + { + }; +} + +IClock& RosClockFactory::createClock() +{ + // Create the singleton. Meyers' implementation. It is automatically threadsafe + static RosClock clock; + return clock; +}