Realtime_tools Realtimepublisher trylock () часто выходит из строя в однопользовом скромном узле Ros2 ⇐ C++

[Anonymous]

Я использую realtime_tools в Ros2 Humble, я создал несколько Realtimepublisher s, и я называю их последовательно в потоке, используя следующий код:
src/main.cpp:

Код: Выделить всё

#include 
#include 
#include 
#include 

class MyNode : public rclcpp_lifecycle::LifecycleNode
{
public:
MyNode(rclcpp::NodeOptions &options)
: rclcpp_lifecycle::LifecycleNode("test_node", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
{
rt_pub1_ = std::make_shared(this->create_publisher("/test_topic1", 10));
rt_pub2_ = std::make_shared(this->create_publisher("/test_topic2", 10));
rt_pub3_ = std::make_shared(this->create_publisher("/test_topic3", 10));
rt_pub4_ = std::make_shared(this->create_publisher("/test_topic4", 10));

timer_ = this->create_wall_timer(
std::chrono::milliseconds(1), [this]()
{
publishMessage(rt_pub1_, "Hello from topic 1!");
publishMessage(rt_pub2_, "Hello from topic 2!");
publishMessage(rt_pub3_, "Hello from topic 3!");
publishMessage(rt_pub4_, "Hello from topic 4!"); });
}

~MyNode() {}

private:
void publishMessage(realtime_tools::RealtimePublisherSharedPtr &pub, const std::string &message)
{
if (pub->trylock())
{
pub->msg_.data = message;
pub->unlockAndPublish();
}
else
{
RCLCPP_WARN(this->get_logger(), "Failed to lock publisher");
}
}

rclcpp::TimerBase::SharedPtr timer_;
realtime_tools::RealtimePublisherSharedPtr rt_pub1_;
realtime_tools::RealtimePublisherSharedPtr rt_pub2_;
realtime_tools::RealtimePublisherSharedPtr rt_pub3_;
realtime_tools::RealtimePublisherSharedPtr rt_pub4_;
};

int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto node = std::make_shared(options);
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}
< /code>
package.xml: 


rt_test
0.0.0
A simple ROS2 lifecycle node using realtime_tools.
Your Name
Apache License 2.0

ament_cmake

rclcpp
rclcpp_lifecycle
realtime_tools
std_msgs

ament_lint_auto
ament_lint_common


ament_cmake


< /code>
cmakelists.txt:
cmake_minimum_required(VERSION 3.8)
project(rt_test)

if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(realtime_tools REQUIRED)
find_package(std_msgs REQUIRED)

include_directories(
include
${rclcpp_INCLUDE_DIRS}
${rclcpp_lifecycle_INCLUDE_DIRS}
${realtime_tools_INCLUDE_DIRS}
${std_msgs_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME} src/main.cpp)

ament_target_dependencies(${PROJECT_NAME}
rclcpp
rclcpp_lifecycle
realtime_tools
std_msgs
)

install(TARGETS ${PROJECT_NAME}
DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
< /code>
Структура каталога: < /p>
❯ tree realtime_pub_test/
realtime_pub_test/
├── CMakeLists.txt
├── package.xml
└── src
└── main.cpp

1 directory, 3 files
< /code>
Запуск Результат: < /pbr />  < /pbr /> 

Подробнее здесь: [url]https://stackoverflow.com/questions/79680324/realtime-tools-realtimepublisher-trylock-failing-frequently-in-single-threaded[/url]