diff --git a/CMakeLists.txt b/CMakeLists.txt index 8540aa63..9d95905c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,7 +31,6 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(realtime_tools SHARED - src/realtime_clock.cpp src/realtime_helpers.cpp ) target_compile_features(realtime_tools PUBLIC cxx_std_17) @@ -74,9 +73,6 @@ if(BUILD_TESTING) ament_add_gmock(realtime_buffer_tests test/realtime_buffer_tests.cpp) target_link_libraries(realtime_buffer_tests realtime_tools) - ament_add_gmock(realtime_clock_tests test/realtime_clock_tests.cpp) - target_link_libraries(realtime_clock_tests realtime_tools) - ament_add_gmock(realtime_publisher_tests_non_polling test/realtime_publisher_non_polling.test test/realtime_publisher_tests_non_polling.cpp) diff --git a/include/realtime_tools/realtime_clock.h b/include/realtime_tools/realtime_clock.h deleted file mode 100644 index 253df9ae..00000000 --- a/include/realtime_tools/realtime_clock.h +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2024 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef REALTIME_TOOLS__REALTIME_CLOCK_H_ -#define REALTIME_TOOLS__REALTIME_CLOCK_H_ - -#include "realtime_tools/realtime_clock.hpp" - -// Deprecation notice -#ifdef _WIN32 -#pragma message( \ - "This header include is deprecated. Please update your code to use 'realtime_clock.hpp' header.") //NOLINT -#else -#warning \ - "This header include is deprecated. Please update your code to use 'realtime_clock.hpp' header." //NOLINT -#endif - -#endif // REALTIME_TOOLS__REALTIME_CLOCK_H_ diff --git a/include/realtime_tools/realtime_clock.hpp b/include/realtime_tools/realtime_clock.hpp deleted file mode 100644 index 8f4ca26a..00000000 --- a/include/realtime_tools/realtime_clock.hpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright (c) 2013, hiDOF, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/* - * Author: Wim Meeussen - */ - -#ifndef REALTIME_TOOLS__REALTIME_CLOCK_HPP_ -#define REALTIME_TOOLS__REALTIME_CLOCK_HPP_ - -#include -#include - -#include "rclcpp/clock.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/time.hpp" - -namespace realtime_tools -{ -class [[deprecated("Use rclcpp::Clock or std::chrono::steady_clock instead")]] RealtimeClock -{ -public: - /** - * Default constructor creates an instance that always returns zero time. - */ - RealtimeClock(); - - /** - * Create a realtime-safe wrapper around a clock object. - */ - explicit RealtimeClock(rclcpp::Clock::SharedPtr clock); - - /** - * Create a realtime-safe wrapper around a clock object with a specified logger. - */ - RealtimeClock(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger); - - ~RealtimeClock(); - - /** - * Get the current time from the clock. - * \deprecated use now() instead. - */ - [[deprecated]] rclcpp::Time getSystemTime(const rclcpp::Time & realtime_time = rclcpp::Time()); - - /** - * Get the current time from the clock. - * \return current time, or - * \return zero if RealtimeClock was not given a valid clock object or time is uninitialized. - */ - rclcpp::Time now(const rclcpp::Time & realtime_time = rclcpp::Time()); - -private: - void loop(); - - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_; - unsigned int lock_misses_ = 0; - rclcpp::Time system_time_; - rclcpp::Duration clock_offset_{0, 0u}; - - rclcpp::Time last_realtime_time_; - bool running_ = false; - bool initialized_ = false; - std::mutex mutex_; - std::thread thread_; -}; // class - -} // namespace realtime_tools -#endif // REALTIME_TOOLS__REALTIME_CLOCK_HPP_ diff --git a/src/realtime_clock.cpp b/src/realtime_clock.cpp deleted file mode 100644 index d794fc31..00000000 --- a/src/realtime_clock.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// Copyright (c) 2013, hiDOF, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the hiDOF, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -/* - * Publishing ROS messages is difficult, as the publish function is - * not realtime safe. This class provides the proper locking so that - * you can call publish in realtime and a separate (non-realtime) - * thread will ensure that the message gets published over ROS. - * - * Author: Wim Meeussen - */ - -#include "realtime_tools/realtime_clock.hpp" - -#include - -#include "rclcpp/logging.hpp" -#include "rclcpp/rate.hpp" - -namespace realtime_tools -{ -RealtimeClock::RealtimeClock() : RealtimeClock(nullptr, rclcpp::get_logger("realtime_tools")) {} - -RealtimeClock::RealtimeClock(rclcpp::Clock::SharedPtr clock) -: RealtimeClock(clock, rclcpp::get_logger("realtime_tools")) -{ -} - -RealtimeClock::RealtimeClock(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger) -: clock_(clock), logger_(logger), running_(true), thread_(std::thread(&RealtimeClock::loop, this)) -{ -} - -RealtimeClock::~RealtimeClock() -{ - if (thread_.joinable()) { - running_ = false; - thread_.join(); - } -} - -rclcpp::Time RealtimeClock::now(const rclcpp::Time & realtime_time) -{ - // Default constructed or given invalid clock, so return zero - if (!clock_) { - return rclcpp::Time(); - } - - std::unique_lock guard(mutex_, std::try_to_lock); - if (guard.owns_lock()) { - // update time offset when we have a new system time measurement in the last cycle - if (lock_misses_ == 0 && system_time_ != rclcpp::Time()) { - // get additional offset caused by period of realtime loop - rclcpp::Duration period_offset(0, 0u); - if (last_realtime_time_ != rclcpp::Time()) { - period_offset = (realtime_time - last_realtime_time_) * 0.5; - } - - if (!initialized_) { - clock_offset_ = system_time_ + period_offset - realtime_time; - initialized_ = true; - } else { - clock_offset_ = - clock_offset_ * 0.9999 + (system_time_ + period_offset - realtime_time) * 0.0001; - } - } - system_time_ = rclcpp::Time(); - lock_misses_ = 0; - } else { - lock_misses_++; - } - - last_realtime_time_ = realtime_time; - - // return time - return realtime_time + clock_offset_; -} - -void RealtimeClock::loop() -{ - rclcpp::Rate r(750); - while (running_) { -#ifdef NON_POLLING - std::lock_guard guard(mutex_); -#else - std::unique_lock guard(mutex_, std::defer_lock); - while (!guard.try_lock()) { - std::this_thread::sleep_for(std::chrono::microseconds(500)); - } -#endif - - // store system time - system_time_ = clock_->now(); - - // warning, using non-locked 'lock_misses_', but it's just for debugging - if (lock_misses_ > 100) { - static rclcpp::Time last_warn_time = system_time_; - if ((system_time_ - last_warn_time).seconds() > 1.0) { - RCLCPP_WARN(logger_, "Time estimator has trouble transferring data between non-RT and RT"); - } - } - - // release lock - guard.unlock(); - r.sleep(); - } -} -} // namespace realtime_tools diff --git a/test/realtime_clock_tests.cpp b/test/realtime_clock_tests.cpp deleted file mode 100644 index ed986e45..00000000 --- a/test/realtime_clock_tests.cpp +++ /dev/null @@ -1,67 +0,0 @@ -// Copyright (c) 2019, Open Source Robotics Foundation, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the Open Source Robotics Foundation, Inc. nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include - -#include -#include - -#include "rclcpp/utilities.hpp" -#include "realtime_tools/realtime_clock.hpp" - -// Disable deprecated warnings -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - -using realtime_tools::RealtimeClock; - -TEST(RealtimeClock, get_system_time) -{ - // initialize the global context - rclcpp::init(0, nullptr); - const int ATTEMPTS = 10; - const std::chrono::milliseconds DELAY(1); - - rclcpp::Clock::SharedPtr clock(new rclcpp::Clock()); - { - RealtimeClock rt_clock(clock); - // Wait for time to be available - rclcpp::Time last_rt_time; - for (int i = 0; i < ATTEMPTS && rclcpp::Time() == last_rt_time; ++i) { - std::this_thread::sleep_for(DELAY); - last_rt_time = rt_clock.now(rclcpp::Time()); - } - ASSERT_NE(rclcpp::Time(), last_rt_time); - - // This test assumes system time will not jump backwards during it - EXPECT_GT(rt_clock.now(last_rt_time), last_rt_time); - } - rclcpp::shutdown(); -} - -#pragma GCC diagnostic pop