From d9b1f1103e7aa426d006f940b859e723fe8bed59 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 18 Nov 2022 18:54:15 +0100 Subject: [PATCH] Design the ROS implementation for the Clock class --- src/System/CMakeLists.txt | 2 +- src/System/RosImplementation/CMakeLists.txt | 14 +++ .../BipedalLocomotion/System/RosClock.h | 114 ++++++++++++++++++ src/System/RosImplementation/src/RosClock.cpp | 69 +++++++++++ 4 files changed, 198 insertions(+), 1 deletion(-) create mode 100644 src/System/RosImplementation/CMakeLists.txt create mode 100644 src/System/RosImplementation/include/BipedalLocomotion/System/RosClock.h create mode 100644 src/System/RosImplementation/src/RosClock.cpp 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; +}