Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement assert_liveliness() #5

Merged
merged 3 commits into from
Apr 3, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 7 additions & 6 deletions rclcpp/include/rclcpp/node.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -645,15 +645,16 @@ class Node : public std::enable_shared_from_this<Node>
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.
Expand Down
5 changes: 5 additions & 0 deletions rclcpp/include/rclcpp/node_interfaces/node_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,11 @@ class NodeBase : public NodeBaseInterface
std::shared_ptr<const rcl_node_t>
get_shared_rcl_node_handle() const;

RCLCPP_PUBLIC
virtual
bool
assert_liveliness() const;

RCLCPP_PUBLIC
virtual
rclcpp::callback_group::CallbackGroup::SharedPtr
Expand Down
6 changes: 6 additions & 0 deletions rclcpp/include/rclcpp/node_interfaces/node_base_interface.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,12 @@ class NodeBaseInterface
std::shared_ptr<const rcl_node_t>
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
Expand Down
14 changes: 7 additions & 7 deletions rclcpp/include/rclcpp/publisher.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
/**
Expand Down Expand Up @@ -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<std::shared_ptr<QOSEventHandlerBase>> event_handlers_;

using IntraProcessManagerWeakPtr =
Expand Down
3 changes: 1 addition & 2 deletions rclcpp/include/rclcpp/subscription.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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
{
Expand Down
6 changes: 6 additions & 0 deletions rclcpp/src/rclcpp/node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -417,3 +417,9 @@ Node::get_node_options() const
{
return this->node_options_;
}

bool
Node::assert_liveliness() const
{
return node_base_->assert_liveliness();
}
6 changes: 6 additions & 0 deletions rclcpp/src/rclcpp/node_interfaces/node_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
6 changes: 6 additions & 0 deletions rclcpp/src/rclcpp/publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down
2 changes: 1 addition & 1 deletion rclcpp_action/test/test_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<ActionStatusMessage>(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);
Expand Down