diff --git a/rclcpp/include/rclcpp/node.hpp b/rclcpp/include/rclcpp/node.hpp index 21aca82966..921f3a0a23 100644 --- a/rclcpp/include/rclcpp/node.hpp +++ b/rclcpp/include/rclcpp/node.hpp @@ -645,15 +645,16 @@ class Node : public std::enable_shared_from_this const NodeOptions & get_node_options() const; - /// Manually assert that this Node is alive (for RMW_QOS_POLICY_MANUAL_BY_NODE) + + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) /** - * If the rmw Liveliness policy is set to RMW_QOS_POLICY_MANUAL_BY_NODE, the creator of this - * Node must manually call `assert_liveliness` periodically to signal that this Node - * is still alive. Must be called at least as often as qos_profile's Liveliness lease_duration + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE, the creator + * of this node may manually call `assert_liveliness` at some point in time to signal to the rest + * of the system that this Node is still alive. */ RCLCPP_PUBLIC - void - assert_liveliness() {} + bool + assert_liveliness() const; protected: /// Construct a sub-node, which will extend the namespace of all entities created with it. diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp index b7470269f4..77379c2858 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base.hpp @@ -86,6 +86,11 @@ class NodeBase : public NodeBaseInterface std::shared_ptr get_shared_rcl_node_handle() const; + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const; + RCLCPP_PUBLIC virtual rclcpp::callback_group::CallbackGroup::SharedPtr diff --git a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp index eb376c0353..ee562e41f8 100644 --- a/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp +++ b/rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp @@ -102,6 +102,12 @@ class NodeBaseInterface std::shared_ptr get_shared_rcl_node_handle() const = 0; + /// Manually assert that this Node is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE) + RCLCPP_PUBLIC + virtual + bool + assert_liveliness() const = 0; + /// Create and return a callback group. RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp index a48316b57b..799c6dad43 100644 --- a/rclcpp/include/rclcpp/publisher.hpp +++ b/rclcpp/include/rclcpp/publisher.hpp @@ -138,15 +138,15 @@ class PublisherBase size_t get_intra_process_subscription_count() const; - /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_MANUAL_BY_TOPIC) + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) /** - * If the rmw Liveliness policy is set to RMW_QOS_POLICY_MANUAL_BY_TOPIC, the creator of this - * Publisher must manually call `assert_liveliness` periodically to signal that this Publisher - * is still alive. Must be called at least as often as qos_profile's Liveliness lease_duration + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator + * of this publisher may manually call `assert_liveliness` at some point in time to signal to the + * rest of the system that this Node is still alive. */ RCLCPP_PUBLIC - void - assert_liveliness() {} + bool + assert_liveliness() const; /// Compare this publisher to a gid. /** @@ -200,7 +200,7 @@ class PublisherBase rcl_publisher_t publisher_handle_ = rcl_get_zero_initialized_publisher(); rcl_publisher_t intra_process_publisher_handle_ = rcl_get_zero_initialized_publisher(); - + std::vector> event_handlers_; using IntraProcessManagerWeakPtr = diff --git a/rclcpp/include/rclcpp/subscription.hpp b/rclcpp/include/rclcpp/subscription.hpp index 982f61f5df..9b6efdfaef 100644 --- a/rclcpp/include/rclcpp/subscription.hpp +++ b/rclcpp/include/rclcpp/subscription.hpp @@ -30,8 +30,6 @@ #include "rcl_interfaces/msg/intra_process_message.hpp" -#include "rclcpp/subscription_options.hpp" -#include "rclcpp/waitable.hpp" #include "rclcpp/any_subscription_callback.hpp" #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" @@ -42,6 +40,7 @@ #include "rclcpp/subscription_traits.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" +#include "rclcpp/waitable.hpp" namespace rclcpp { diff --git a/rclcpp/src/rclcpp/node.cpp b/rclcpp/src/rclcpp/node.cpp index 7935a8caf0..97589442b3 100644 --- a/rclcpp/src/rclcpp/node.cpp +++ b/rclcpp/src/rclcpp/node.cpp @@ -417,3 +417,9 @@ Node::get_node_options() const { return this->node_options_; } + +bool +Node::assert_liveliness() const +{ + return node_base_->assert_liveliness(); +} diff --git a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp index 845dcd7b6d..a9198ccd73 100644 --- a/rclcpp/src/rclcpp/node_interfaces/node_base.cpp +++ b/rclcpp/src/rclcpp/node_interfaces/node_base.cpp @@ -197,6 +197,12 @@ NodeBase::get_shared_rcl_node_handle() const return node_handle_; } +bool +NodeBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_node_assert_liveliness(get_rcl_node_handle()); +} + rclcpp::callback_group::CallbackGroup::SharedPtr NodeBase::create_callback_group(rclcpp::callback_group::CallbackGroupType group_type) { diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp index eebe71c355..3d311962de 100644 --- a/rclcpp/src/rclcpp/publisher.cpp +++ b/rclcpp/src/rclcpp/publisher.cpp @@ -204,6 +204,12 @@ PublisherBase::get_intra_process_subscription_count() const return ipm->get_subscription_count(intra_process_publisher_id_); } +bool +PublisherBase::assert_liveliness() const +{ + return RCL_RET_OK == rcl_publisher_assert_liveliness(&publisher_handle_); +} + bool PublisherBase::operator==(const rmw_gid_t & gid) const { diff --git a/rclcpp_action/test/test_client.cpp b/rclcpp_action/test/test_client.cpp index 6199d29af2..6b8c06d4bf 100644 --- a/rclcpp_action/test/test_client.cpp +++ b/rclcpp_action/test/test_client.cpp @@ -205,7 +205,7 @@ class TestClient : public ::testing::Test action_name, allocator, &status_topic_name); ASSERT_EQ(RCL_RET_OK, ret); status_publisher = server_node->create_publisher(status_topic_name, - nullptr, rclcpp::PublisherOptions<>(rcl_action_qos_profile_status_default)); + rcl_action_qos_profile_status_default); ASSERT_TRUE(status_publisher != nullptr); allocator.deallocate(status_topic_name, allocator.state); server_executor.add_node(server_node);