diff --git a/common/autoware_node/include/autoware_node/autoware_node.hpp b/common/autoware_node/include/autoware_node/autoware_node.hpp index 65a7a2d150..6ba32cfc60 100644 --- a/common/autoware_node/include/autoware_node/autoware_node.hpp +++ b/common/autoware_node/include/autoware_node/autoware_node.hpp @@ -24,6 +24,11 @@ #include "autoware_control_center_msgs/srv/autoware_node_error.hpp" #include "autoware_control_center_msgs/srv/autoware_node_register.hpp" +#include "rclcpp/subscription_options.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/subscription_traits.hpp" +#include "rclcpp/message_memory_strategy.hpp" + #include namespace autoware_node @@ -37,6 +42,52 @@ class AutowareNode : public rclcpp_lifecycle::LifecycleNode const std::string & node_name, const std::string & ns = "", const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> + std::shared_ptr create_monitored_subscription( + const std::string & topic_name, + const float hz, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptions & options = + rclcpp::SubscriptionOptions(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); + + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription> + std::shared_ptr create_simple_monitored_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback); + + template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> + std::shared_ptr + test_create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = + rclcpp_lifecycle::create_default_subscription_options(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = ( + MessageMemoryStrategyT::create_default() + ) + ); + rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_; rclcpp::Client::SharedPtr cli_register_; diff --git a/common/autoware_node/src/autoware_node.cpp b/common/autoware_node/src/autoware_node.cpp index 663c74d332..6f2dd4776d 100644 --- a/common/autoware_node/src/autoware_node.cpp +++ b/common/autoware_node/src/autoware_node.cpp @@ -189,4 +189,90 @@ void AutowareNode::send_state( RCLCPP_INFO(get_logger(), "Send node state"); } +template< +typename MessageT, +typename CallbackT, +typename AllocatorT = std::allocator, +typename SubscriptionT = rclcpp::Subscription, +typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> +std::shared_ptr AutowareNode::create_monitored_subscription( + const std::string & topic_name, + const float hz, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptions & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + // create proper qos based on input parameter + // update lease duration and deadline in qos + RCLCPP_INFO(get_logger(), "Create monitored subscription"); + std::chrono::milliseconds lease_duration = 1.0 / hz * 1000; + lease_duration = 1.1 * lease_duration; // add 10 % gap to lease duration (buffer) + rclcpp::QoS qos_profile = qos; + qos_profile.liveliness(RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) + .liveliness_lease_duration(lease_duration) + .deadline(lease_duration); + + rclcpp::SubscriptionOptions sub_options = options; + sub_options.event_callbacks.deadline_callback = + [=](rclcpp::QOSDeadlineRequestedInfo& event) -> void { + RCLCPP_INFO(get_logger(), "Requested deadline missed - total %d delta %d\n", + event.total_count, event.total_count_change); + // add NodeError service call + std::string msg = "Deadline for topic " + topic_name + " was missed."; + autoware_control_center_msgs::msg::AutowareNodeState node_state; + node_state.status = autoware_control_center_msgs::msg::AutowareNodeState::ERROR; + send_state(node_state, msg); + }; + + return rclcpp::create_subscription ( + *this, + topic_name, + qos_profile, + std::forward(callback), + sub_options, + msg_mem_strat); +} + +template< +typename MessageT, +typename CallbackT, +typename AllocatorT = std::allocator, +typename SubscriptionT = rclcpp::Subscription> +std::shared_ptr AutowareNode::create_simple_monitored_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback) +{ + return rclcpp_lifecycle::LifecycleNode::create_subscription( + topic_name, + qos, + std::forward(callback)); +} + + +template< + typename MessageT, + typename CallbackT, + typename AllocatorT = std::allocator, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> +std::shared_ptr +AutowareNode::test_create_subscription( + const std::string & topic_name, + const rclcpp::QoS & qos, + CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) +{ + RCLCPP_INFO(get_logger(), "call test_create_subscription"); + return rclcpp::create_subscription( + *this, + topic_name, + qos, + std::forward(callback), + options, + msg_mem_strat); +} + } // namespace autoware_node diff --git a/common/test_node/include/test_node/test_node.hpp b/common/test_node/include/test_node/test_node.hpp index 659bdad383..027fe9065f 100644 --- a/common/test_node/include/test_node/test_node.hpp +++ b/common/test_node/include/test_node/test_node.hpp @@ -19,6 +19,8 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "test_node/visibility_control.hpp" + +#include "std_msgs/msg/string.hpp" namespace test_node { @@ -27,6 +29,10 @@ class TestNode : public autoware_node::AutowareNode public: TEST_NODE_PUBLIC explicit TestNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void topic_callback(const std_msgs::msg::String::SharedPtr msg); + rclcpp::Subscription::SharedPtr monitored_subscription_; }; } // namespace test_node diff --git a/common/test_node/src/test_node.cpp b/common/test_node/src/test_node.cpp index 8d20aa7bef..c3344e5d92 100644 --- a/common/test_node/src/test_node.cpp +++ b/common/test_node/src/test_node.cpp @@ -19,12 +19,29 @@ namespace test_node { +using std::placeholders::_1; + + TestNode::TestNode(const rclcpp::NodeOptions & options) : autoware_node::AutowareNode("test_node", "", options) { RCLCPP_INFO( get_logger(), "TestNode constructor with name %s", autoware_node::AutowareNode::self_name.c_str()); + // monitored_subscription_ = autoware_node::AutowareNode::create_monitored_subscription( + // "topic", 10, 10, std::bind(&TestNode::topic_callback, this, _1)); + // monitored_subscription_ = autoware_node::AutowareNode::create_simple_monitored_subscription( + // "topic", 10, std::bind(&TestNode::topic_callback, this, _1) + // ); + + monitored_subscription_ = autoware_node::AutowareNode::test_create_subscription( + "topic", 10, std::bind(&TestNode::topic_callback, this, _1)); + +} + +void TestNode::topic_callback(const std_msgs::msg::String::SharedPtr msg) +{ + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); } } // namespace test_node