add examples for how to use the new rclcpp::WaitSet classes

rclcpp/wait_set/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.5)
+project(examples_rclcpp_wait_set)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(example_interfaces REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_executable(wait_set wait_set.cpp)
+ament_target_dependencies(wait_set example_interfaces rclcpp std_msgs)
+
+add_executable(static_wait_set static_wait_set.cpp)
+ament_target_dependencies(static_wait_set rclcpp std_msgs)
+
+add_executable(thread_safe_wait_set thread_safe_wait_set.cpp)
+ament_target_dependencies(thread_safe_wait_set example_interfaces rclcpp std_msgs)
+
+install(TARGETS
+  wait_set
+  static_wait_set
+  thread_safe_wait_set
+  DESTINATION lib/${PROJECT_NAME}
+)
+ament_package()

rclcpp/wait_set/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model
+  href="http://download.ros.org/schema/package_format2.xsd"
+  schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>examples_rclcpp_wait_set</name>
+  <version>0.8.2</version>
+  <description>Example of how to use the rclcpp::WaitSet directly.</description>
+  <maintainer email="[email protected]">William Woodall</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>example_interfaces</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <exec_depend>example_interfaces</exec_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <export>
+     <build_type>ament_cmake</build_type>
+  </export>
+</package>

rclcpp/wait_set/static_wait_set.cpp

@@ -0,0 +1,48 @@
+// Copyright 2020 Open Source Robotics Foundation, 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 <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  // auto do_nothing = [](std_msgs::msg::String::UniquePtr){};
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto guard_condition2 = std::make_shared<rclcpp::GuardCondition>();
+
+  rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> static_wait_set(
+    std::array<rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>::SubscriptionEntry, 0>{},
+    std::array<rclcpp::GuardCondition::SharedPtr, 1>{{guard_condition}},
+    std::array<rclcpp::TimerBase::SharedPtr, 0>{},
+    std::array<rclcpp::ClientBase::SharedPtr, 0>{},
+    std::array<rclcpp::ServiceBase::SharedPtr, 0>{},
+    std::array<rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>::WaitableEntry, 0>{});
+  // Note: The following line will result in a compiler error, since the
+  //   static storage policy prevents editing after construction.
+  // static_wait_set.add_guard_condition(guard_condition2);
+  // static_wait_set.remove_guard_condition(guard_condition2);
+  (void)guard_condition2;
+
+  {
+    auto wait_result = static_wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  return 0;
+}

rclcpp/wait_set/thread_safe_wait_set.cpp

@@ -0,0 +1,109 @@
+// Copyright 2020 Open Source Robotics Foundation, 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 <cassert>
+#include <memory>
+// #include <string>
+// #include <thread>
+
+#include "example_interfaces/srv/add_two_ints.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);};
+
+  auto node = std::make_shared<rclcpp::Node>("wait_set_example_node");
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto guard_condition2 = std::make_shared<rclcpp::GuardCondition>();
+
+  auto sub = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
+  rclcpp::SubscriptionOptions so;
+  so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+  auto sub2 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing, so);
+
+  rclcpp::ThreadSafeWaitSet wait_set(
+    std::vector<rclcpp::ThreadSafeWaitSet::SubscriptionEntry>{{sub}},
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition});
+  wait_set.add_subscription(sub2);
+  wait_set.add_guard_condition(guard_condition2);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  guard_condition->trigger();
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[0] != nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[1] == nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] == nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1] == nullptr);
+  }
+
+  wait_set.remove_guard_condition(guard_condition2);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  wait_set.remove_guard_condition(guard_condition);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  wait_set.remove_subscription(sub2);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 1);
+  pub->publish(std_msgs::msg::String().set__data("test"));
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] != nullptr);
+    std_msgs::msg::String msg;
+    rclcpp::MessageInfo msg_info;
+    assert(sub->take(msg, msg_info));
+    assert(msg.data == "test");
+  }
+
+  wait_set.remove_subscription(sub);
+
+  {
+    // still will not fail, because thread-safe policy we are using adds its
+    // own guard condition and therefore it will never be empty
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  return 0;
+}

rclcpp/wait_set/wait_set.cpp

@@ -0,0 +1,123 @@
+// Copyright 2020 Open Source Robotics Foundation, 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 <cassert>
+#include <memory>
+// #include <string>
+// #include <thread>
+
+#include "example_interfaces/srv/add_two_ints.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);};
+  auto add_two_ints =
+    [](
+    const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Request>,
+    const std::shared_ptr<example_interfaces::srv::AddTwoInts::Response>)
+    {assert(false);};
+
+  auto node = std::make_shared<rclcpp::Node>("wait_set_example_node");
+
+  auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
+  auto guard_condition2 = std::make_shared<rclcpp::GuardCondition>();
+
+  auto sub = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
+  rclcpp::SubscriptionOptions so;
+  so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+  auto sub2 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing, so);
+
+  auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 10);
+  rclcpp::PublisherOptions po;
+  po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
+  auto pub2 = node->create_publisher<std_msgs::msg::String>("~/chatter", 10, po);
+
+  auto client = node->create_client<example_interfaces::srv::AddTwoInts>("~/add_two_ints");
+  auto service =
+    node->create_service<example_interfaces::srv::AddTwoInts>("~/add_two_ints", add_two_ints);
+
+  rclcpp::WaitSet wait_set(
+    std::vector<rclcpp::WaitSet::SubscriptionEntry>{{sub}},
+    std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition});
+  wait_set.add_subscription(sub2);
+  wait_set.add_guard_condition(guard_condition2);
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  guard_condition->trigger();
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[0] != nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[1] == nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] == nullptr);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1] == nullptr);
+  }
+
+  wait_set.remove_guard_condition(guard_condition2);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  wait_set.remove_guard_condition(guard_condition);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  wait_set.remove_subscription(sub2);
+
+  {
+    // still fails with timeout
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
+  }
+
+  // auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 1);
+  pub->publish(std_msgs::msg::String().set__data("test"));
+
+  {
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
+    assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] != nullptr);
+    std_msgs::msg::String msg;
+    rclcpp::MessageInfo msg_info;
+    assert(sub->take(msg, msg_info));
+    assert(msg.data == "test");
+  }
+
+  wait_set.remove_subscription(sub);
+
+  {
+    // now fails (fast) with empty
+    auto wait_result = wait_set.wait(std::chrono::seconds(1));
+    assert(wait_result.kind() == rclcpp::WaitResultKind::Empty);
+  }
+
+  return 0;
+}