diff --git a/data_tamer_cpp/include/data_tamer/sinks/ros2_publisher_sink.hpp b/data_tamer_cpp/include/data_tamer/sinks/ros2_publisher_sink.hpp index c1c4138..f6b5536 100644 --- a/data_tamer_cpp/include/data_tamer/sinks/ros2_publisher_sink.hpp +++ b/data_tamer_cpp/include/data_tamer/sinks/ros2_publisher_sink.hpp @@ -5,22 +5,29 @@ #include "data_tamer_msgs/msg/schemas.hpp" #include "data_tamer_msgs/msg/snapshot.hpp" #include +#include #include #include +#include +#include namespace DataTamer { +using PublisherNodeInterfaces = + rclcpp::node_interfaces::NodeInterfaces; + class ROS2PublisherSink : public DataSinkBase { public: - ROS2PublisherSink(std::shared_ptr node, const std::string& topic_prefix); - - ROS2PublisherSink(std::shared_ptr node, - const std::string& topic_prefix); - template - void create_publishers(NodeT& node, const std::string& topic_prefix) + ROS2PublisherSink(NodeT&& nodelike, const std::string& topic_prefix) + : node_interface_(normalize_node(nodelike)) + { + create_publishers(topic_prefix); + } + + void create_publishers(const std::string& topic_prefix) { rclcpp::QoS schemas_qos{ rclcpp::KeepAll() }; schemas_qos.reliable(); @@ -28,10 +35,10 @@ class ROS2PublisherSink : public DataSinkBase const rclcpp::QoS data_qos{ rclcpp::KeepAll() }; - schema_publisher_ = node->template create_publisher( - topic_prefix + "/schemas", schemas_qos); - data_publisher_ = node->template create_publisher( - topic_prefix + "/data", data_qos); + schema_publisher_ = rclcpp::create_publisher( + node_interface_, topic_prefix + "/schemas", schemas_qos); + data_publisher_ = rclcpp::create_publisher( + node_interface_, topic_prefix + "/data", data_qos); } void addChannel(const std::string& name, const Schema& schema) override; @@ -39,6 +46,20 @@ class ROS2PublisherSink : public DataSinkBase bool storeSnapshot(const Snapshot& snapshot) override; private: + template + static PublisherNodeInterfaces normalize_node(NodeT&& nodelike) + { + using D = std::decay_t; + + if constexpr(std::is_same_v) + return nodelike; + else if constexpr(std::is_same_v> || + std::is_same_v>) + return PublisherNodeInterfaces(*nodelike); + else + return PublisherNodeInterfaces(nodelike); + } + std::unordered_map schemas_; Mutex schema_mutex_; @@ -47,6 +68,9 @@ class ROS2PublisherSink : public DataSinkBase bool schema_changed_ = true; data_tamer_msgs::msg::Snapshot data_msg_; + + // ---- Stored node façade ---- + PublisherNodeInterfaces node_interface_; }; } // namespace DataTamer diff --git a/data_tamer_cpp/src/sinks/ros2_publisher_sink.cpp b/data_tamer_cpp/src/sinks/ros2_publisher_sink.cpp index b49c70b..92eaa32 100644 --- a/data_tamer_cpp/src/sinks/ros2_publisher_sink.cpp +++ b/data_tamer_cpp/src/sinks/ros2_publisher_sink.cpp @@ -2,18 +2,6 @@ namespace DataTamer { -ROS2PublisherSink::ROS2PublisherSink(std::shared_ptr node, - const std::string& topic_prefix) -{ - create_publishers(node, topic_prefix); -} - -ROS2PublisherSink::ROS2PublisherSink( - std::shared_ptr node, - const std::string& topic_prefix) -{ - create_publishers(node, topic_prefix); -} void ROS2PublisherSink::addChannel(const std::string& channel_name, const Schema& schema) { diff --git a/data_tamer_cpp/tests/CMakeLists.txt b/data_tamer_cpp/tests/CMakeLists.txt index 6429aa6..80100ea 100644 --- a/data_tamer_cpp/tests/CMakeLists.txt +++ b/data_tamer_cpp/tests/CMakeLists.txt @@ -7,6 +7,7 @@ if(gtest_vendor_FOUND AND ament_cmake_gtest_FOUND) dt_tests.cpp custom_types_tests.cpp parser_tests.cpp + ros2_publisher_tests.cpp trait_tests.cpp) target_include_directories(datatamer_test @@ -28,6 +29,7 @@ else() add_executable(datatamer_test dt_tests.cpp custom_types_tests.cpp + ros2_publisher_tests.cpp parser_tests.cpp) gtest_discover_tests(datatamer_test DISCOVERY_MODE PRE_TEST) diff --git a/data_tamer_cpp/tests/ros2_publisher_tests.cpp b/data_tamer_cpp/tests/ros2_publisher_tests.cpp new file mode 100644 index 0000000..dd60f3c --- /dev/null +++ b/data_tamer_cpp/tests/ros2_publisher_tests.cpp @@ -0,0 +1,93 @@ +#include "data_tamer/data_tamer.hpp" +#include "data_tamer/sinks/dummy_sink.hpp" +#include "data_tamer/sinks/ros2_publisher_sink.hpp" + +#include + +#include +#include +#include +#include + +using namespace DataTamer; + +TEST(DataTamerROS2Publisher, SharedPointer) +{ + auto node = std::make_shared("test_datatamer_shared_pointer"); + auto ros2_sink = std::make_shared(node, "test_shared_pointer"); + + auto channel = ChannelsRegistry::Global().getChannel("channel_shared_pointer"); + + channel->addDataSink(ros2_sink); + + double const value = 1.; + auto id_value = channel->registerValue("value", &value); + + EXPECT_TRUE(channel->takeSnapshot()); +} + +TEST(DataTamerROS2Publisher, SharedPointerLifeCycle) +{ + auto lifecycle_node = std::make_shared("test_" + "datatamer_" + "shared_" + "pointer_" + "lifecycle"); + auto ros2_sink = std::make_shared(lifecycle_node, "test_shared_" + "pointer_" + "lifecycle"); + + auto channel = ChannelsRegistry::Global().getChannel("channel_shared_pointer_" + "lifecycle"); + + channel->addDataSink(ros2_sink); + + double const value = 1.; + auto id_value = channel->registerValue("value", &value); + + EXPECT_TRUE(channel->takeSnapshot()); +} + +TEST(DataTamerROS2Publisher, Dereference) +{ + auto node = std::make_shared("test_datatamer_dereference"); + auto ros2_sink = std::make_shared(*node, "test_dereference"); + + auto channel = ChannelsRegistry::Global().getChannel("channel_dereference"); + + channel->addDataSink(ros2_sink); + + double const value = 1.; + auto id_value = channel->registerValue("value", &value); + + EXPECT_TRUE(channel->takeSnapshot()); +} + +TEST(DataTamerROS2Publisher, DereferenceLifeCycle) +{ + auto lifecycle_node = std::make_shared("test_" + "datatamer_" + "dereference_" + "lifecycle"); + auto ros2_sink = std::make_shared(*lifecycle_node, "test_" + "dereference_" + "lifecycle"); + + auto channel = ChannelsRegistry::Global().getChannel("channel_dereference_lifecycle"); + + channel->addDataSink(ros2_sink); + + double const value = 1.; + auto id_value = channel->registerValue("value", &value); + + EXPECT_TRUE(channel->takeSnapshot()); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +}