feat(autoware_node): add create_monitored_subscription, build, produc…

…e symbol lookup error

Signed-off-by: Alexey Panferov <lexavtanke@gmail.com>

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 <string>
 
 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<void>,
+  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+  std::shared_ptr<SubscriptionT> 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<void>,
+  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>>
+  std::shared_ptr<SubscriptionT> create_simple_monitored_subscription(
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    CallbackT && callback);
+
+  template<
+    typename MessageT,
+    typename CallbackT,
+    typename AllocatorT = std::allocator<void>,
+    typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+    typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+  std::shared_ptr<SubscriptionT>
+  test_create_subscription(
+    const std::string & topic_name,
+    const rclcpp::QoS & qos,
+    CallbackT && callback,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
+    rclcpp_lifecycle::create_default_subscription_options<AllocatorT>(),
+    typename MessageMemoryStrategyT::SharedPtr msg_mem_strat = (
+      MessageMemoryStrategyT::create_default()
+    )
+  );
+
   rclcpp::CallbackGroup::SharedPtr callback_group_mut_ex_;
 
   rclcpp::Client<autoware_control_center_msgs::srv::AutowareNodeRegister>::SharedPtr cli_register_;

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<void>,
+typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+std::shared_ptr<SubscriptionT> 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<MessageT>  (
+      *this,
+      topic_name,
+      qos_profile,
+      std::forward<CallbackT>(callback),
+      sub_options,
+      msg_mem_strat);
+}
+
+template<
+typename MessageT,
+typename CallbackT,
+typename AllocatorT = std::allocator<void>,
+typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>>
+std::shared_ptr<SubscriptionT> AutowareNode::create_simple_monitored_subscription(
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback)
+{
+  return rclcpp_lifecycle::LifecycleNode::create_subscription<MessageT>(
+    topic_name,
+    qos,
+    std::forward<CallbackT>(callback));
+}
+
+
+template<
+  typename MessageT,
+  typename CallbackT,
+  typename AllocatorT = std::allocator<void>,
+  typename SubscriptionT = rclcpp::Subscription<MessageT, AllocatorT>,
+  typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType>
+std::shared_ptr<SubscriptionT>
+AutowareNode::test_create_subscription(
+  const std::string & topic_name,
+  const rclcpp::QoS & qos,
+  CallbackT && callback,
+  const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+  typename MessageMemoryStrategyT::SharedPtr msg_mem_strat)
+{
+  RCLCPP_INFO(get_logger(), "call test_create_subscription");
+  return rclcpp::create_subscription<MessageT>(
+    *this,
+    topic_name,
+    qos,
+    std::forward<CallbackT>(callback),
+    options,
+    msg_mem_strat);
+}
+
 }  // namespace autoware_node

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<std_msgs::msg::String>::SharedPtr monitored_subscription_;
 };
 
 }  // namespace test_node

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<std_msgs::msg::String>(
+  //   "topic", 10, 10, std::bind(&TestNode::topic_callback, this, _1));
+  // monitored_subscription_ = autoware_node::AutowareNode::create_simple_monitored_subscription<std_msgs::msg::String>(
+  //   "topic", 10, std::bind(&TestNode::topic_callback, this, _1)
+  // );
+
+  monitored_subscription_  = autoware_node::AutowareNode::test_create_subscription<std_msgs::msg::String>(
+    "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