diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 13a0e35e..d2a64101 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package realtime_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.4.0 (2022-11-19) +------------------ +* bug fix for RealtimePublisher with NON_POLLING (`#85 `_) +* ci: :construction_worker: update rhel container (`#92 `_) +* Make `thread_priority` a shared library (`#91 `_) +* Contributors: Andy Zelenak, Jaron Lundwall, Yoav Fekete, Denis Štogl + 2.3.0 (2022-08-26) ------------------ * Fix source-based branch names (`#90 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index 093019a6..ec6a939d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,11 @@ if(BUILD_TESTING) ament_add_gmock(realtime_clock_tests test/realtime_clock_tests.cpp) target_link_libraries(realtime_clock_tests ${PROJECT_NAME} ${GMOCK_MAIN_LIBRARIES}) + ament_add_gmock(realtime_publisher_tests_non_polling + test/realtime_publisher_non_polling.test test/realtime_publisher_tests_non_polling.cpp) + target_link_libraries(realtime_publisher_tests_non_polling ${PROJECT_NAME} ${test_msgs_LIBRARIES} ${GMOCK_MAIN_LIBRARIES}) + target_include_directories(realtime_publisher_tests_non_polling PRIVATE ${test_msgs_INCLUDE_DIRS}) + ament_add_gmock(realtime_publisher_tests test/realtime_publisher.test test/realtime_publisher_tests.cpp) target_link_libraries(realtime_publisher_tests ${PROJECT_NAME} ${test_msgs_LIBRARIES} ${GMOCK_MAIN_LIBRARIES}) target_include_directories(realtime_publisher_tests PRIVATE ${test_msgs_INCLUDE_DIRS}) diff --git a/include/realtime_tools/realtime_publisher.h b/include/realtime_tools/realtime_publisher.h index 39dc4c12..6b726f72 100644 --- a/include/realtime_tools/realtime_publisher.h +++ b/include/realtime_tools/realtime_publisher.h @@ -166,10 +166,16 @@ class RealtimePublisher Msg outgoing; // Locks msg_ and copies it + +#ifdef NON_POLLING + std::unique_lock lock_(msg_mutex_); +#else lock(); +#endif + while (turn_ != NON_REALTIME && keep_running_) { #ifdef NON_POLLING - updated_cond_.wait(lock); + updated_cond_.wait(lock_); #else unlock(); std::this_thread::sleep_for(std::chrono::microseconds(500)); diff --git a/package.xml b/package.xml index ed6f4b00..86f188c8 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ realtime_tools - 2.3.0 + 2.4.0 Contains a set of tools that can be used from a hard realtime thread, without breaking the realtime behavior. Bence Magyar diff --git a/test/realtime_publisher_non_polling.test b/test/realtime_publisher_non_polling.test new file mode 100644 index 00000000..f66314c7 --- /dev/null +++ b/test/realtime_publisher_non_polling.test @@ -0,0 +1,4 @@ + + + + diff --git a/test/realtime_publisher_tests_non_polling.cpp b/test/realtime_publisher_tests_non_polling.cpp new file mode 100644 index 00000000..92194f69 --- /dev/null +++ b/test/realtime_publisher_tests_non_polling.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2019, ros2_control development team +// +// 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. + +#define NON_POLLING + +#include + +#include +#include +#include +#include + +#include "rclcpp/executors.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/utilities.hpp" +#include "realtime_tools/realtime_publisher.h" +#include "test_msgs/msg/strings.hpp" + +using StringMsg = test_msgs::msg::Strings; +using realtime_tools::RealtimePublisher; + +TEST(RealtimePublisherNonPolling, construct_destruct) { RealtimePublisher rt_pub; } + +struct StringCallback +{ + StringMsg msg_; + std::mutex mtx_; + + void callback(const StringMsg::SharedPtr msg) + { + std::unique_lock lock(mtx_); + msg_ = *msg; + } +}; + +TEST(RealtimePublisherNonPolling, rt_publish) +{ + rclcpp::init(0, nullptr); + const size_t ATTEMPTS = 10; + const std::chrono::milliseconds DELAY(250); + + const char * expected_msg = "Hello World"; + auto node = std::make_shared("construct_move_destruct"); + rclcpp::QoS qos(10); + qos.reliable().transient_local(); + auto pub = node->create_publisher("~/rt_publish", qos); + RealtimePublisher rt_pub(pub); + // publish a latched message + bool lock_is_held = rt_pub.trylock(); + for (size_t i = 0; i < ATTEMPTS && !lock_is_held; ++i) { + lock_is_held = rt_pub.trylock(); + std::this_thread::sleep_for(DELAY); + } + ASSERT_TRUE(lock_is_held); + rt_pub.msg_.string_value = expected_msg; + rt_pub.unlockAndPublish(); + + // make sure subscriber gets it + StringCallback str_callback; + + auto sub = node->create_subscription( + "~/rt_publish", qos, + std::bind(&StringCallback::callback, &str_callback, std::placeholders::_1)); + for (size_t i = 0; i < ATTEMPTS && str_callback.msg_.string_value.empty(); ++i) { + rclcpp::spin_some(node); + std::this_thread::sleep_for(DELAY); + } + EXPECT_STREQ(expected_msg, str_callback.msg_.string_value.c_str()); + rclcpp::shutdown(); +}