From 76722b3eea821d32a3da921fb64690b3ec445f4e Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 20 Jan 2025 18:20:47 +0900 Subject: [PATCH] feat(agnocastlib): implement Agnocast-ROS2 functionality (#326) Signed-off-by: veqcc --- src/agnocast_bridge/CMakeLists.txt | 25 --------- src/agnocast_bridge/launch/bridge.launch.xml | 5 -- src/agnocast_bridge/package.xml | 18 ------ src/agnocast_bridge/src/bridge_node.cpp | 40 -------------- src/agnocastlib/include/agnocast.hpp | 49 +++++++++++------ .../include/agnocast_publisher.hpp | 27 +++++++-- .../include/agnocast_subscription.hpp | 24 ++++---- src/agnocastlib/package.xml | 1 + src/agnocastlib/src/agnocast_subscription.cpp | 7 ++- .../test/test_agnocast_publisher.cpp | 27 +++++---- .../src/callback_group_test_listener.cpp | 4 +- .../src/callback_group_test_talker.cpp | 6 +- .../src/minimal_publisher.cpp | 32 ++++++++++- src/sample_application/src/minimal_pubsub.cpp | 5 +- .../src/minimal_subscriber.cpp | 55 ++++++++++++++++++- 15 files changed, 177 insertions(+), 148 deletions(-) delete mode 100644 src/agnocast_bridge/CMakeLists.txt delete mode 100644 src/agnocast_bridge/launch/bridge.launch.xml delete mode 100644 src/agnocast_bridge/package.xml delete mode 100644 src/agnocast_bridge/src/bridge_node.cpp diff --git a/src/agnocast_bridge/CMakeLists.txt b/src/agnocast_bridge/CMakeLists.txt deleted file mode 100644 index 9f3a81eb..00000000 --- a/src/agnocast_bridge/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(agnocast_bridge) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) - -find_package(agnocastlib REQUIRED) -find_package(sample_interfaces REQUIRED) - -add_executable(bridge_node src/bridge_node.cpp) -ament_target_dependencies(bridge_node rclcpp agnocastlib sample_interfaces) -target_include_directories(bridge_node PRIVATE - ${agnocastlib_INCLUDE_DIRS} -) - -install(TARGETS - bridge_node - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY launch - DESTINATION share/${PROJECT_NAME}/ -) - -ament_package() diff --git a/src/agnocast_bridge/launch/bridge.launch.xml b/src/agnocast_bridge/launch/bridge.launch.xml deleted file mode 100644 index 4cb7ffda..00000000 --- a/src/agnocast_bridge/launch/bridge.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/src/agnocast_bridge/package.xml b/src/agnocast_bridge/package.xml deleted file mode 100644 index c37c4691..00000000 --- a/src/agnocast_bridge/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - agnocast_bridge - 0.0.1 - Bridge Agnocast communication and ROS 2 communication - sykwer - Apache-2.0 - - ament_cmake - - rclcpp - sample_interfaces - agnocastlib - - - ament_cmake - - diff --git a/src/agnocast_bridge/src/bridge_node.cpp b/src/agnocast_bridge/src/bridge_node.cpp deleted file mode 100644 index 11db9769..00000000 --- a/src/agnocast_bridge/src/bridge_node.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "agnocast.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sample_interfaces/msg/dynamic_size_array.hpp" - -using std::placeholders::_1; - -class BridgeNode : public rclcpp::Node -{ - agnocast::Subscription::SharedPtr agnocast_subscriber_; - rclcpp::Publisher::SharedPtr publisher_; - - void agnocast_topic_callback( - const agnocast::ipc_shared_ptr & message) - { - RCLCPP_INFO(this->get_logger(), "bridge message agnocast->ros2 address: %p", message.get()); - const sample_interfaces::msg::DynamicSizeArray * raw = message.get(); - publisher_->publish(*raw); - } - -public: - BridgeNode() : Node("bridge_node") - { - auto publisher_options = rclcpp::PublisherOptions(); - publisher_options.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - publisher_ = this->create_publisher( - "/mytopic", 10, publisher_options); - - agnocast_subscriber_ = agnocast::create_subscription( - get_node_base_interface(), "/mytopic", 10, - std::bind(&BridgeNode::agnocast_topic_callback, this, _1)); - } -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/agnocastlib/include/agnocast.hpp b/src/agnocastlib/include/agnocast.hpp index dec4a06b..d0091771 100644 --- a/src/agnocastlib/include/agnocast.hpp +++ b/src/agnocastlib/include/agnocast.hpp @@ -28,25 +28,40 @@ bool ok(); template typename Publisher::SharedPtr create_publisher( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const rclcpp::QoS & qos) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos) { - return std::make_shared>(node, topic_name, qos); + return std::make_shared>(node, topic_name, qos, true); } template typename Publisher::SharedPtr create_publisher( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const size_t qos_history_depth) + rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth) { return std::make_shared>( - node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth))); + node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), true); +} + +template +typename Publisher::SharedPtr create_publisher( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, + const bool do_always_ros2_publish) +{ + return std::make_shared>(node, topic_name, qos, do_always_ros2_publish); +} + +template +typename Publisher::SharedPtr create_publisher( + rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth, + const bool do_always_ros2_publish) +{ + return std::make_shared>( + node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth)), do_always_ros2_publish); } template typename Subscription::SharedPtr create_subscription( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const rclcpp::QoS & qos, std::function &)> callback) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, + std::function &)> callback) { const agnocast::SubscriptionOptions options; return std::make_shared>(node, topic_name, qos, callback, options); @@ -54,8 +69,7 @@ typename Subscription::SharedPtr create_subscription( template typename Subscription::SharedPtr create_subscription( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const size_t qos_history_depth, + rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth, std::function &)> callback) { const agnocast::SubscriptionOptions options; @@ -65,8 +79,8 @@ typename Subscription::SharedPtr create_subscription( template typename Subscription::SharedPtr create_subscription( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const rclcpp::QoS & qos, std::function &)> callback, + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, + std::function &)> callback, agnocast::SubscriptionOptions options) { return std::make_shared>(node, topic_name, qos, callback, options); @@ -74,8 +88,7 @@ typename Subscription::SharedPtr create_subscription( template typename Subscription::SharedPtr create_subscription( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const size_t qos_history_depth, + rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth, std::function &)> callback, agnocast::SubscriptionOptions options) { @@ -85,17 +98,17 @@ typename Subscription::SharedPtr create_subscription( template typename PollingSubscriber::SharedPtr create_subscription( - const std::string & topic_name, const size_t qos_history_depth) + rclcpp::Node * node, const std::string & topic_name, const size_t qos_history_depth) { return std::make_shared>( - topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth))); + node, topic_name, rclcpp::QoS(rclcpp::KeepLast(qos_history_depth))); } template typename PollingSubscriber::SharedPtr create_subscription( - const std::string & topic_name, const rclcpp::QoS & qos) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos) { - return std::make_shared>(topic_name, qos); + return std::make_shared>(node, topic_name, qos); } } // namespace agnocast diff --git a/src/agnocastlib/include/agnocast_publisher.hpp b/src/agnocastlib/include/agnocast_publisher.hpp index 9f7cc88f..5213bd33 100644 --- a/src/agnocastlib/include/agnocast_publisher.hpp +++ b/src/agnocastlib/include/agnocast_publisher.hpp @@ -41,15 +41,26 @@ class Publisher // TODO(Koichi98): The mq should be closed when a subscriber unsubscribes the topic, but this is // not currently implemented. std::unordered_map opened_mqs_; + bool do_always_ros2_publish_; // For transient local. + typename rclcpp::Publisher::SharedPtr ros2_publisher_; public: using SharedPtr = std::shared_ptr>; Publisher( - [[maybe_unused]] rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, /* for CARET */ - const std::string & topic_name, const rclcpp::QoS & qos) - : topic_name_(topic_name), publisher_pid_(getpid()), qos_(qos) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, + const bool do_always_ros2_publish) + : topic_name_(node->get_node_topics_interface()->resolve_topic_name(topic_name)), + publisher_pid_(getpid()), + qos_(qos), + ros2_publisher_(node->create_publisher(topic_name_, qos)) { + if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + do_always_ros2_publish_ = do_always_ros2_publish; + } else { + do_always_ros2_publish_ = false; + } + initialize_publisher(publisher_pid_, topic_name_); } @@ -82,11 +93,19 @@ class Publisher exit(EXIT_FAILURE); } + if (do_always_ros2_publish_ || ros2_publisher_->get_subscription_count() > 0) { + const MessageT * raw = message.get(); + ros2_publisher_->publish(*raw); + } + publish_core(topic_name_, publisher_pid_, message.get_timestamp(), opened_mqs_); message.reset(); } - uint32_t get_subscription_count() const { return get_subscription_count_core(topic_name_); } + uint32_t get_subscription_count() const + { + return ros2_publisher_->get_subscription_count() + get_subscription_count_core(topic_name_); + } }; } // namespace agnocast diff --git a/src/agnocastlib/include/agnocast_subscription.hpp b/src/agnocastlib/include/agnocast_subscription.hpp index 3c215e1b..8067022d 100644 --- a/src/agnocastlib/include/agnocast_subscription.hpp +++ b/src/agnocastlib/include/agnocast_subscription.hpp @@ -54,7 +54,9 @@ class SubscriptionBase union ioctl_subscriber_args initialize(bool is_take_sub); public: - SubscriptionBase(const pid_t subscriber_pid, std::string topic_name, const rclcpp::QoS & qos); + SubscriptionBase( + rclcpp::Node * node, const pid_t subscriber_pid, const std::string & topic_name, + const rclcpp::QoS & qos); }; template @@ -66,17 +68,17 @@ class Subscription : public SubscriptionBase using SharedPtr = std::shared_ptr>; Subscription( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node, const std::string & topic_name, - const rclcpp::QoS & qos, + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos, std::function &)> callback, agnocast::SubscriptionOptions options) - : SubscriptionBase(getpid(), topic_name, qos) + : SubscriptionBase(node, getpid(), topic_name, qos) { union ioctl_subscriber_args subscriber_args = initialize(false); - mqd_t mq = open_mq_for_subscription(topic_name, subscriber_pid_, mq_subscription); - rclcpp::CallbackGroup::SharedPtr callback_group = get_valid_callback_group(node, options); + mqd_t mq = open_mq_for_subscription(topic_name_, subscriber_pid_, mq_subscription); + auto node_base = node->get_node_base_interface(); + rclcpp::CallbackGroup::SharedPtr callback_group = get_valid_callback_group(node_base, options); agnocast::register_callback( - callback, topic_name, static_cast(qos.depth()), mq, callback_group); + callback, topic_name_, static_cast(qos.depth()), mq, callback_group); // If there are messages available and the transient local is enabled, immediately call the // callback. @@ -101,8 +103,8 @@ class TakeSubscription : public SubscriptionBase public: using SharedPtr = std::shared_ptr>; - TakeSubscription(const std::string & topic_name, const rclcpp::QoS & qos) - : SubscriptionBase(getpid(), topic_name, qos) + TakeSubscription(rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos) + : SubscriptionBase(node, getpid(), topic_name, qos) { if (qos.durability() == rclcpp::DurabilityPolicy::TransientLocal) { RCLCPP_WARN( @@ -145,10 +147,10 @@ class PollingSubscriber using SharedPtr = std::shared_ptr>; explicit PollingSubscriber( - const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) : data_(agnocast::ipc_shared_ptr()) { - subscriber_ = std::make_shared>(topic_name, qos); + subscriber_ = std::make_shared>(node, topic_name, qos); }; const agnocast::ipc_shared_ptr takeData() diff --git a/src/agnocastlib/package.xml b/src/agnocastlib/package.xml index a533a56d..43ca06b0 100644 --- a/src/agnocastlib/package.xml +++ b/src/agnocastlib/package.xml @@ -11,6 +11,7 @@ rclcpp rclcpp_components + std_msgs ament_cmake_gmock ament_lint_auto diff --git a/src/agnocastlib/src/agnocast_subscription.cpp b/src/agnocastlib/src/agnocast_subscription.cpp index 81752ebb..14b0077e 100644 --- a/src/agnocastlib/src/agnocast_subscription.cpp +++ b/src/agnocastlib/src/agnocast_subscription.cpp @@ -8,8 +8,11 @@ mqd_t mq_new_publisher = -1; extern std::vector threads; SubscriptionBase::SubscriptionBase( - const pid_t subscriber_pid, std::string topic_name, const rclcpp::QoS & qos) -: subscriber_pid_(subscriber_pid), topic_name_(std::move(topic_name)), qos_(qos) + rclcpp::Node * node, const pid_t subscriber_pid, const std::string & topic_name, + const rclcpp::QoS & qos) +: subscriber_pid_(subscriber_pid), + topic_name_(node->get_node_topics_interface()->resolve_topic_name(topic_name)), + qos_(qos) { validate_ld_preload(); } diff --git a/src/agnocastlib/test/test_agnocast_publisher.cpp b/src/agnocastlib/test/test_agnocast_publisher.cpp index e80054cc..0ae909c4 100644 --- a/src/agnocastlib/test/test_agnocast_publisher.cpp +++ b/src/agnocastlib/test/test_agnocast_publisher.cpp @@ -1,6 +1,8 @@ #include "agnocast.hpp" #include "agnocast_publisher.hpp" +#include "std_msgs/msg/int32.hpp" + #include #include @@ -25,18 +27,18 @@ class AgnocastPublisherTest : public ::testing::Test { rclcpp::init(0, nullptr); pid = getpid(); - dummy_tn = "dummy"; - node = std::make_shared(dummy_tn); + dummy_tn = "/dummy"; + node = std::make_shared("dummy_node"); dummy_qd = 10; EXPECT_GLOBAL_CALL(initialize_publisher, initialize_publisher(pid, dummy_tn)).Times(1); dummy_publisher = - agnocast::create_publisher(node->get_node_base_interface(), dummy_tn, dummy_qd); + agnocast::create_publisher(node.get(), dummy_tn, dummy_qd); } void TearDown() override { rclcpp::shutdown(); } std::shared_ptr node; - agnocast::Publisher::SharedPtr dummy_publisher; + agnocast::Publisher::SharedPtr dummy_publisher; uint32_t pid; std::string dummy_tn; uint32_t dummy_qd; @@ -48,7 +50,7 @@ TEST_F(AgnocastPublisherTest, test_publish_normal) borrow_loaned_message_core, borrow_loaned_message_core(dummy_tn, pid, dummy_qd, _, _)) .WillOnce(testing::Return(std::vector())); EXPECT_GLOBAL_CALL(publish_core, publish_core(dummy_tn, pid, _, _)).Times(1); - agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); + agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); dummy_publisher->publish(std::move(message)); } @@ -56,7 +58,7 @@ TEST_F(AgnocastPublisherTest, test_publish_normal) TEST_F(AgnocastPublisherTest, test_publish_null_message) { EXPECT_GLOBAL_CALL(publish_core, publish_core(dummy_tn, pid, _, _)).Times(0); - agnocast::ipc_shared_ptr message; + agnocast::ipc_shared_ptr message; EXPECT_EXIT( dummy_publisher->publish(std::move(message)), ::testing::ExitedWithCode(EXIT_FAILURE), @@ -69,7 +71,7 @@ TEST_F(AgnocastPublisherTest, test_publish_already_published_message) borrow_loaned_message_core, borrow_loaned_message_core(dummy_tn, pid, dummy_qd, _, _)) .WillOnce(testing::Return(std::vector())); EXPECT_GLOBAL_CALL(publish_core, publish_core(dummy_tn, pid, _, _)).Times(1); - agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); + agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); dummy_publisher->publish(std::move(message)); @@ -80,15 +82,16 @@ TEST_F(AgnocastPublisherTest, test_publish_already_published_message) TEST_F(AgnocastPublisherTest, test_publish_different_message) { - std::string diff_dummy_tn = "dummy2"; + std::string diff_dummy_tn = "/dummy2"; EXPECT_GLOBAL_CALL(initialize_publisher, initialize_publisher(pid, diff_dummy_tn)).Times(1); EXPECT_GLOBAL_CALL(borrow_loaned_message_core, borrow_loaned_message_core(_, pid, _, _, _)) .WillRepeatedly(testing::Return(std::vector())); EXPECT_GLOBAL_CALL(publish_core, publish_core(dummy_tn, pid, _, _)).Times(0); - agnocast::Publisher::SharedPtr diff_publisher = - agnocast::create_publisher(node->get_node_base_interface(), diff_dummy_tn, 10); - agnocast::ipc_shared_ptr diff_message = diff_publisher->borrow_loaned_message(); - agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); + agnocast::Publisher::SharedPtr diff_publisher = + agnocast::create_publisher(node.get(), diff_dummy_tn, 10); + agnocast::ipc_shared_ptr diff_message = + diff_publisher->borrow_loaned_message(); + agnocast::ipc_shared_ptr message = dummy_publisher->borrow_loaned_message(); EXPECT_EXIT( dummy_publisher->publish(std::move(diff_message)), ::testing::ExitedWithCode(EXIT_FAILURE), diff --git a/src/sample_application/src/callback_group_test_listener.cpp b/src/sample_application/src/callback_group_test_listener.cpp index 2c2b42f0..aa56071f 100644 --- a/src/sample_application/src/callback_group_test_listener.cpp +++ b/src/sample_application/src/callback_group_test_listener.cpp @@ -23,12 +23,12 @@ class CallbackGroupTestListener : public rclcpp::Node sub_options.callback_group = mutex_callback_group_; sub1_ = agnocast::create_subscription( - get_node_base_interface(), "/topic1", rclcpp::QoS(10), + this, "/topic1", rclcpp::QoS(10), std::bind(&CallbackGroupTestListener::sub_callback1, this, std::placeholders::_1), sub_options); sub2_ = agnocast::create_subscription( - get_node_base_interface(), "/topic2", 10, + this, "/topic2", 10, std::bind(&CallbackGroupTestListener::sub_callback2, this, std::placeholders::_1), sub_options); diff --git a/src/sample_application/src/callback_group_test_talker.cpp b/src/sample_application/src/callback_group_test_talker.cpp index 8291ffb8..a5c9fa11 100644 --- a/src/sample_application/src/callback_group_test_talker.cpp +++ b/src/sample_application/src/callback_group_test_talker.cpp @@ -14,10 +14,8 @@ class CallbackGroupTestTalker : public rclcpp::Node public: CallbackGroupTestTalker() : Node("callback_group_test_talker") { - pub1_ = - agnocast::create_publisher(get_node_base_interface(), "/topic1", 10); - pub2_ = - agnocast::create_publisher(get_node_base_interface(), "/topic2", 10); + pub1_ = agnocast::create_publisher(this, "/topic1", 10); + pub2_ = agnocast::create_publisher(this, "/topic2", 10); timer_ = this->create_wall_timer( 5000ms, std::bind(&CallbackGroupTestTalker::publish_test_messages, this)); diff --git a/src/sample_application/src/minimal_publisher.cpp b/src/sample_application/src/minimal_publisher.cpp index 6e024f49..5546ce40 100644 --- a/src/sample_application/src/minimal_publisher.cpp +++ b/src/sample_application/src/minimal_publisher.cpp @@ -63,6 +63,10 @@ class MinimalPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; agnocast::Publisher::SharedPtr publisher_dynamic_; agnocast::Publisher::SharedPtr publisher_static_; + agnocast::Publisher::SharedPtr + publisher_transient_local_; + agnocast::Publisher::SharedPtr + publisher_transient_local_with_flag_; int count_; std::vector timestamps_; @@ -74,14 +78,38 @@ class MinimalPublisher : public rclcpp::Node { timer_ = this->create_wall_timer(100ms, std::bind(&MinimalPublisher::timer_callback, this)); publisher_dynamic_ = agnocast::create_publisher( - get_node_base_interface(), "/my_dynamic_topic", 10); + this, "/my_dynamic_topic", 10); publisher_static_ = agnocast::create_publisher( - get_node_base_interface(), "/my_static_topic", 10); + this, "/my_static_topic", 10); + publisher_transient_local_ = + agnocast::create_publisher( + this, "/my_transient_local_topic", rclcpp::QoS(1).transient_local()); + publisher_transient_local_with_flag_ = + agnocast::create_publisher( + this, "/my_transient_local_topic_with_flag", rclcpp::QoS(1).transient_local(), false); count_ = 0; timestamps_.resize(10000, 0); timestamp_ids_.resize(10000, 0); timestamp_idx_ = 0; + + { + agnocast::ipc_shared_ptr message = + publisher_transient_local_->borrow_loaned_message(); + message->id = count_; + message->timestamp = agnocast_get_timestamp(); + assign_data(*message); + publisher_transient_local_->publish(std::move(message)); + } + + { + agnocast::ipc_shared_ptr message = + publisher_transient_local_with_flag_->borrow_loaned_message(); + message->id = count_; + message->timestamp = agnocast_get_timestamp(); + assign_data(*message); + publisher_transient_local_with_flag_->publish(std::move(message)); + } } ~MinimalPublisher() diff --git a/src/sample_application/src/minimal_pubsub.cpp b/src/sample_application/src/minimal_pubsub.cpp index 0e32931c..50ab5728 100644 --- a/src/sample_application/src/minimal_pubsub.cpp +++ b/src/sample_application/src/minimal_pubsub.cpp @@ -55,10 +55,9 @@ class MinimalPubSub : public rclcpp::Node MinimalPubSub() : Node("minimal_pubsub") { publisher_dynamic_ = agnocast::create_publisher( - get_node_base_interface(), "/my_dynamic_topic", 10); + this, "/my_dynamic_topic", 10); sub_static_ = agnocast::create_subscription( - get_node_base_interface(), "/my_static_topic", 10, - std::bind(&MinimalPubSub::topic_callback, this, _1)); + this, "/my_static_topic", 10, std::bind(&MinimalPubSub::topic_callback, this, _1)); count_ = 0; diff --git a/src/sample_application/src/minimal_subscriber.cpp b/src/sample_application/src/minimal_subscriber.cpp index 4bf1b434..e8766830 100644 --- a/src/sample_application/src/minimal_subscriber.cpp +++ b/src/sample_application/src/minimal_subscriber.cpp @@ -25,6 +25,13 @@ class MinimalSubscriber : public rclcpp::Node agnocast::PollingSubscriber::SharedPtr sub_dynamic_; agnocast::Subscription::SharedPtr sub_static_; + agnocast::Subscription::SharedPtr sub_transient_local_; + rclcpp::Subscription::SharedPtr + sub_transient_local_ros2_; + agnocast::Subscription::SharedPtr + sub_transient_local_with_flag_; + rclcpp::Subscription::SharedPtr + sub_transient_local_with_flag_ros2_; void callback_static( const agnocast::ipc_shared_ptr & message) @@ -53,6 +60,32 @@ class MinimalSubscriber : public rclcpp::Node reinterpret_cast(message.get())); } + void callback_transient_local( + [[maybe_unused]] const agnocast::ipc_shared_ptr & + message) + { + RCLCPP_INFO(this->get_logger(), "I heard transient_local message through Agnocast"); + } + + void callback_transient_local_ros2( + [[maybe_unused]] const sample_interfaces::msg::StaticSizeArray & message) + { + RCLCPP_INFO(this->get_logger(), "I heard transient_local message through ROS"); + } + + void callback_transient_local_with_flag( + [[maybe_unused]] const agnocast::ipc_shared_ptr & + message) + { + RCLCPP_INFO(this->get_logger(), "I heard transient_local_with_flag message through Agnocast"); + } + + void callback_transient_local_with_flag_ros2( + [[maybe_unused]] const sample_interfaces::msg::StaticSizeArray & message) + { + RCLCPP_INFO(this->get_logger(), "I heard transient_local_with_flag message through ROS"); + } + public: explicit MinimalSubscriber(const rclcpp::NodeOptions & options) : Node("minimal_subscriber", options) @@ -67,11 +100,29 @@ class MinimalSubscriber : public rclcpp::Node agnocast_options.callback_group = group; sub_dynamic_ = agnocast::create_subscription( - "/my_dynamic_topic", 10); + this, "/my_dynamic_topic", 10); sub_static_ = agnocast::create_subscription( - get_node_base_interface(), "/my_static_topic", rclcpp::QoS(10).transient_local(), + this, "/my_static_topic", rclcpp::QoS(10).transient_local(), std::bind(&MinimalSubscriber::callback_static, this, _1), agnocast_options); + + sub_transient_local_ = agnocast::create_subscription( + this, "/my_transient_local_topic", rclcpp::QoS(1).transient_local(), + std::bind(&MinimalSubscriber::callback_transient_local, this, _1)); + + sub_transient_local_ros2_ = create_subscription( + "/my_transient_local_topic", rclcpp::QoS(1).transient_local(), + std::bind(&MinimalSubscriber::callback_transient_local_ros2, this, _1)); + + sub_transient_local_with_flag_ = + agnocast::create_subscription( + this, "/my_transient_local_topic_with_flag", rclcpp::QoS(1).transient_local(), + std::bind(&MinimalSubscriber::callback_transient_local_with_flag, this, _1)); + + sub_transient_local_with_flag_ros2_ = + create_subscription( + "/my_transient_local_topic_with_flag", rclcpp::QoS(1).transient_local(), + std::bind(&MinimalSubscriber::callback_transient_local_with_flag_ros2, this, _1)); } ~MinimalSubscriber()