Add wait set examples

rclcpp/topics/minimal_subscriber/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 
 add_executable(subscriber_lambda lambda.cpp)
@@ -32,6 +33,31 @@ ament_target_dependencies(subscriber_member_function_with_unique_network_flow_en
 add_executable(subscriber_not_composable not_composable.cpp)
 ament_target_dependencies(subscriber_not_composable rclcpp std_msgs)
 
+
+add_library(wait_set_subscriber_library SHARED
+    wait_set_subscriber.cpp
+    static_wait_set_subscriber.cpp
+    time_triggered_wait_set_subscriber.cpp)
+ament_target_dependencies(wait_set_subscriber_library rclcpp rclcpp_components std_msgs)
+
+rclcpp_components_register_node(wait_set_subscriber_library
+    PLUGIN "WaitSetSubscriber"
+    EXECUTABLE wait_set_subscriber)
+
+rclcpp_components_register_node(wait_set_subscriber_library
+    PLUGIN "StaticWaitSetSubscriber"
+    EXECUTABLE static_wait_set_subscriber)
+
+rclcpp_components_register_node(wait_set_subscriber_library
+    PLUGIN "TimeTriggeredWaitSetSubscriber"
+    EXECUTABLE time_triggered_wait_set_subscriber)
+
+install(TARGETS
+    wait_set_subscriber_library
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin)
+
 install(TARGETS
   subscriber_lambda
   subscriber_member_function

rclcpp/topics/minimal_subscriber/README.md

@@ -4,11 +4,19 @@ This package contains a few different strategies for creating nodes which receiv
  * `lambda.cpp` uses a C++11 lambda function
  * `member_function.cpp` uses a C++ member function callback
  * `not_composable.cpp` uses a global function callback without a Node subclass
-
+ * `wait_set_subscriber.cpp` uses a `rclcpp::WaitSet` to wait and poll for data
+ * `static_wait_set_subscriber.cpp` uses a `rclcpp::StaticWaitSet` to wait and poll for data
+ * `time_triggered_wait_set_subscriber.cpp` uses a `rclcpp::Waitset` and a timer to poll for data
+   periodically
+
 Note that `not_composable.cpp` instantiates a `rclcpp::Node` _without_ subclassing it.
 This was the typical usage model in ROS 1, but this style of coding is not compatible with composing multiple nodes into a single process.
 Thus, it is no longer the recommended style for ROS 2.
 
-All of these nodes do the same thing: they create a node called `minimal_listener` and subscribe to a topic named `topic` which is of datatype `std_msgs/String`.
+All of these nodes do the same thing: they create a node called `minimal_subscriber` and subscribe to a topic named `topic` which is of datatype `std_msgs/String`.
 When a message arrives on that topic, the node prints it to the screen.
 We provide multiple examples of different coding styles which achieve this behavior in order to demonstrate that there are many ways to do this in ROS 2.
+
+The following examples `wait_set_subscriber.cpp`, `static_wait_set_subscriber.cpp` and `time_triggered_wait_set_subscriber.cpp` show how to use a subscription in a node using a `rclcpp` wait-set.
+This is not a common use case in ROS 2 so this is not the recommended strategy to  use by-default.
+This strategy makes sense in some specific situations, for example when the developer needs to have more control over callback order execution, to create custom triggering conditions or to use the timeouts provided by the  wait-sets.   

rclcpp/topics/minimal_subscriber/package.xml

@@ -14,9 +14,11 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <build_depend>rclcpp</build_depend>
+  <build_depend>rclcpp_components</build_depend>
   <build_depend>std_msgs</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
   <exec_depend>std_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp

@@ -0,0 +1,96 @@
+// Copyright 2021, Apex.AI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+/* This example creates a subclass of Node and uses static a wait-set based loop to wait on
+ * a subscription to have messages available and then handles them manually without an executor */
+
+class StaticWaitSetSubscriber : public rclcpp::Node
+{
+  using MyStaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>;
+
+public:
+  explicit StaticWaitSetSubscriber(rclcpp::NodeOptions options)
+  : Node("static_wait_set_subscriber", options),
+    subscription_(
+      [this]()
+      {
+        // create subscription with a callback-group not added to the executor
+        rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group(
+          rclcpp::CallbackGroupType::MutuallyExclusive, false);
+        auto subscription_options = rclcpp::SubscriptionOptions();
+        subscription_options.callback_group = cb_group_waitset;
+        auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) {
+          RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+        };
+        return this->create_subscription<std_msgs::msg::String>(
+          "topic",
+          10,
+          subscription_callback,
+          subscription_options);
+      } ()
+    ),
+    wait_set_(std::array<MyStaticWaitSet::SubscriptionEntry, 1>{{{subscription_}}}),
+    thread_(std::thread([this]() -> void {spin_wait_set();}))
+  {
+  }
+
+  ~StaticWaitSetSubscriber()
+  {
+    if (thread_.joinable()) {
+      thread_.join();
+    }
+  }
+
+  void spin_wait_set()
+  {
+    while (rclcpp::ok()) {
+      // Wait for the subscriber event to trigger. Set a 1 ms margin to trigger a timeout.
+      const auto wait_result = wait_set_.wait(std::chrono::milliseconds(501));
+      switch (wait_result.kind()) {
+        case rclcpp::WaitResultKind::Ready:
+          {
+            std_msgs::msg::String msg;
+            rclcpp::MessageInfo msg_info;
+            if (subscription_->take(msg, msg_info)) {
+              std::shared_ptr<void> type_erased_msg = std::make_shared<std_msgs::msg::String>(msg);
+              subscription_->handle_message(type_erased_msg, msg_info);
+            }
+            break;
+          }
+        case rclcpp::WaitResultKind::Timeout:
+          if (rclcpp::ok()) {
+            RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time");
+          }
+          break;
+        default:
+          RCLCPP_ERROR(this->get_logger(), "Error. Wait-set failed.");
+      }
+    }
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  MyStaticWaitSet wait_set_;
+  std::thread thread_;
+};
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(StaticWaitSetSubscriber)

rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp

@@ -0,0 +1,98 @@
+// Copyright 2021, Apex.AI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+/* This example creates a subclass of Node and uses a wait-set based loop to periodically poll
+ * for messages in a timer callback using a wait-set based loop. */
+
+class TimeTriggeredWaitSetSubscriber : public rclcpp::Node
+{
+public:
+  explicit TimeTriggeredWaitSetSubscriber(rclcpp::NodeOptions options)
+  : Node("time_triggered_wait_set_subscriber", options)
+  {
+    rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+
+    auto subscription_options = rclcpp::SubscriptionOptions();
+    subscription_options.callback_group = cb_group_waitset;
+    auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) {
+        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+      };
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "topic",
+      10,
+      subscription_callback,
+      subscription_options);
+    auto timer_callback = [this]() -> void {
+        std_msgs::msg::String msg;
+        rclcpp::MessageInfo msg_info;
+        if (subscription_->take(msg, msg_info)) {
+          std::shared_ptr<void> type_erased_msg = std::make_shared<std_msgs::msg::String>(msg);
+          subscription_->handle_message(type_erased_msg, msg_info);
+        } else {
+          RCLCPP_WARN(this->get_logger(), "No message available");
+        }
+      };
+    timer_ = create_wall_timer(500ms, timer_callback, cb_group_waitset);
+    wait_set_.add_timer(timer_);
+    thread_ = std::thread([this]() -> void {spin_wait_set();});
+  }
+
+  ~TimeTriggeredWaitSetSubscriber()
+  {
+    thread_.join();
+  }
+
+  void spin_wait_set()
+  {
+    while (rclcpp::ok()) {
+      // Wait for the timer event to trigger. Set a 1 ms margin to trigger a timeout.
+      const auto wait_result = wait_set_.wait(501ms);
+      switch (wait_result.kind()) {
+        case rclcpp::WaitResultKind::Ready:
+          {
+            if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) {
+              timer_->execute_callback();
+            }
+            break;
+          }
+        case rclcpp::WaitResultKind::Timeout:
+          if (rclcpp::ok()) {
+            RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time");
+          }
+          break;
+        default:
+          RCLCPP_ERROR(this->get_logger(), "Error. Wait-set failed.");
+      }
+    }
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::WaitSet wait_set_;
+  std::thread thread_;
+};
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(TimeTriggeredWaitSetSubscriber)

rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp

@@ -0,0 +1,88 @@
+// Copyright 2021, Apex.AI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+/* This example creates a subclass of Node and uses a wait-set based loop to wait on
+ * a subscription to have messages available and then handles them manually without an executor */
+
+class WaitSetSubscriber : public rclcpp::Node
+{
+public:
+  explicit WaitSetSubscriber(rclcpp::NodeOptions options)
+  : Node("wait_set_subscriber", options)
+  {
+    rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive, false);
+    auto subscription_options = rclcpp::SubscriptionOptions();
+    subscription_options.callback_group = cb_group_waitset;
+    auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) {
+        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+      };
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "topic",
+      10,
+      subscription_callback,
+      subscription_options);
+    wait_set_.add_subscription(subscription_);
+    thread_ = std::thread([this]() -> void {spin_wait_set();});
+  }
+
+  ~WaitSetSubscriber()
+  {
+    if (thread_.joinable()) {
+      thread_.join();
+    }
+  }
+
+  void spin_wait_set()
+  {
+    while (rclcpp::ok()) {
+      // Wait for the subscriber event to trigger. Set a 1 ms margin to trigger a timeout.
+      const auto wait_result = wait_set_.wait(std::chrono::milliseconds(501));
+      switch (wait_result.kind()) {
+        case rclcpp::WaitResultKind::Ready:
+          {
+            std_msgs::msg::String msg;
+            rclcpp::MessageInfo msg_info;
+            if (subscription_->take(msg, msg_info)) {
+              std::shared_ptr<void> type_erased_msg = std::make_shared<std_msgs::msg::String>(msg);
+              subscription_->handle_message(type_erased_msg, msg_info);
+            }
+            break;
+          }
+        case rclcpp::WaitResultKind::Timeout:
+          if (rclcpp::ok()) {
+            RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time");
+          }
+          break;
+        default:
+          RCLCPP_ERROR(this->get_logger(), "Error. Wait-set failed.");
+      }
+    }
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  rclcpp::WaitSet wait_set_;
+  std::thread thread_;
+};
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+RCLCPP_COMPONENTS_REGISTER_NODE(WaitSetSubscriber)