From 0d6b39707ce9b7bc48a2ecbf2086240d9ffd9892 Mon Sep 17 00:00:00 2001 From: Yoav Fekete <59432952+YoavFekete@users.noreply.github.com> Date: Sat, 1 Oct 2022 11:37:46 +0300 Subject: [PATCH] bug fix for RealtimePublisher with NON_POLLING (#85) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Co-authored-by: Denis Štogl --- CMakeLists.txt | 5 + include/realtime_tools/realtime_publisher.h | 8 +- test/realtime_publisher_non_polling.test | 4 + test/realtime_publisher_tests_non_polling.cpp | 95 +++++++++++++++++++ 4 files changed, 111 insertions(+), 1 deletion(-) create mode 100644 test/realtime_publisher_non_polling.test create mode 100644 test/realtime_publisher_tests_non_polling.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3beb8f10..9695a73e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,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/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(); +}