diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index d9221c48..2edca97f 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/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 diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index e11b22c4..5caa8006 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/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. diff --git a/rclcpp/topics/minimal_subscriber/package.xml b/rclcpp/topics/minimal_subscriber/package.xml index 9a95ed6d..f07e7b16 100644 --- a/rclcpp/topics/minimal_subscriber/package.xml +++ b/rclcpp/topics/minimal_subscriber/package.xml @@ -14,9 +14,11 @@ ament_cmake rclcpp + rclcpp_components std_msgs rclcpp + rclcpp_components std_msgs ament_lint_auto diff --git a/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp new file mode 100644 index 00000000..d72c7ab3 --- /dev/null +++ b/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 +#include + +#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( + "topic", + 10, + subscription_callback, + subscription_options); + } () + ), + wait_set_(std::array{{{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 type_erased_msg = std::make_shared(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::SharedPtr subscription_; + MyStaticWaitSet wait_set_; + std::thread thread_; +}; + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(StaticWaitSetSubscriber) diff --git a/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp new file mode 100644 index 00000000..7b5d85dc --- /dev/null +++ b/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 +#include + +#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( + "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 type_erased_msg = std::make_shared(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::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) diff --git a/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp new file mode 100644 index 00000000..79dddce2 --- /dev/null +++ b/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 +#include + +#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( + "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 type_erased_msg = std::make_shared(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::SharedPtr subscription_; + rclcpp::WaitSet wait_set_; + std::thread thread_; +}; + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(WaitSetSubscriber) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt new file mode 100644 index 00000000..db6c6c6f --- /dev/null +++ b/rclcpp/wait_set/CMakeLists.txt @@ -0,0 +1,81 @@ +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(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) + +include_directories(include) + +add_library(talker SHARED src/talker.cpp) +target_compile_definitions(talker PRIVATE WAIT_SET_DLL) +ament_target_dependencies(talker rclcpp rclcpp_components std_msgs) +rclcpp_components_register_node( + talker + PLUGIN "Talker" + EXECUTABLE wait_set_talker) + +add_library(listener SHARED src/listener.cpp) +target_compile_definitions(listener PRIVATE WAIT_SET_DLL) +ament_target_dependencies(listener rclcpp rclcpp_components std_msgs) +rclcpp_components_register_node( + listener + PLUGIN "Listener" + EXECUTABLE wait_set_listener) + +add_executable(wait_set src/wait_set.cpp) +ament_target_dependencies(wait_set example_interfaces rclcpp std_msgs) + +add_executable(static_wait_set src/static_wait_set.cpp) +ament_target_dependencies(static_wait_set rclcpp std_msgs) + +add_executable(thread_safe_wait_set src/thread_safe_wait_set.cpp) +ament_target_dependencies(thread_safe_wait_set example_interfaces rclcpp std_msgs) + +add_executable(wait_set_topics_and_timer src/wait_set_topics_and_timer.cpp) +ament_target_dependencies(wait_set_topics_and_timer rclcpp std_msgs) + +add_executable(wait_set_random_order src/wait_set_random_order.cpp) +ament_target_dependencies(wait_set_random_order rclcpp std_msgs) + +add_executable(executor_random_order src/executor_random_order.cpp) +ament_target_dependencies(executor_random_order rclcpp std_msgs) + +add_executable(wait_set_topics_with_different_rates src/wait_set_topics_with_different_rates.cpp) +ament_target_dependencies(wait_set_topics_with_different_rates rclcpp std_msgs) + +add_executable(wait_set_composed src/wait_set_composed.cpp) +target_link_libraries(wait_set_composed talker listener) +ament_target_dependencies(wait_set_composed rclcpp) + +install(TARGETS + talker + listener + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + wait_set + static_wait_set + thread_safe_wait_set + wait_set_topics_and_timer + wait_set_random_order + executor_random_order + wait_set_topics_with_different_rates + wait_set_composed + DESTINATION lib/${PROJECT_NAME} +) +ament_package() diff --git a/rclcpp/wait_set/README.md b/rclcpp/wait_set/README.md new file mode 100644 index 00000000..bf497dd7 --- /dev/null +++ b/rclcpp/wait_set/README.md @@ -0,0 +1,22 @@ +# Minimal rclcpp wait-set cookbook recipes + +This package contains a few different strategies for creating nodes which use `rclcpp::waitset` +to wait and handle ROS entities, that is, subscribers, timers, clients, services, guard +conditions and waitables. + + +* `wait_set.cpp`: Simple example showing how to use the default wait-set with a dynamic + storage policy and a sequential (no thread-safe) synchronization policy. +* `static_wait_set.cpp`: Simple example showing how to use the static wait-set with a static + storage policy. +* `thread_safe_wait_set.cpp`: Simple example showing how to use the thread-safe wait-set with a + thread-safe synchronization policy. +* `wait_set_topics_and_timer.cpp`: Simple example using multiple subscriptions, + publishers, and a timer. +* `wait_set_random_order.cpp`: An example showing user-defined + data handling and a random publisher. `executor_random_order.cpp` run the same node logic + using `SingleThreadedExecutor` to compare the data handling order. +* `wait_set_and_executor_composition.cpp`: An example showing how to combine a + `SingleThreadedExecutor` and a wait-set. +* `wait_set_topics_with_different_rate.cpp`: An example showing how to use a custom trigger + condition to handle topics with different topic rates. \ No newline at end of file diff --git a/rclcpp/wait_set/include/wait_set/listener.hpp b/rclcpp/wait_set/include/wait_set/listener.hpp new file mode 100644 index 00000000..f4d9d7c5 --- /dev/null +++ b/rclcpp/wait_set/include/wait_set/listener.hpp @@ -0,0 +1,37 @@ +// 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. + +#ifndef WAIT_SET__LISTENER_HPP_ +#define WAIT_SET__LISTENER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "wait_set/visibility.h" + +class Listener : public rclcpp::Node +{ +public: + WAIT_SET_PUBLIC explicit Listener(rclcpp::NodeOptions options); + WAIT_SET_PUBLIC ~Listener() override; + +private: + void spin_wait_set(); + + rclcpp::Subscription::SharedPtr subscription1_; + rclcpp::Subscription::SharedPtr subscription2_; + rclcpp::WaitSet wait_set_; + std::thread thread_; +}; + +#endif // WAIT_SET__LISTENER_HPP_ diff --git a/rclcpp/wait_set/include/wait_set/random_listener.hpp b/rclcpp/wait_set/include/wait_set/random_listener.hpp new file mode 100644 index 00000000..79589ad7 --- /dev/null +++ b/rclcpp/wait_set/include/wait_set/random_listener.hpp @@ -0,0 +1,50 @@ +// 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. + +#ifndef WAIT_SET__RANDOM_LISTENER_HPP_ +#define WAIT_SET__RANDOM_LISTENER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +class RandomListener : public rclcpp::Node +{ + using subscription_list = std::vector::SharedPtr>; + +public: + RandomListener() + : Node("random_listener") + { + auto print_msg = [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }; + sub1_ = this->create_subscription("topicA", 10, print_msg); + sub2_ = this->create_subscription("topicB", 10, print_msg); + sub3_ = this->create_subscription("topicC", 10, print_msg); + } + + subscription_list get_subscriptions() const + { + return {sub1_, sub2_, sub3_}; + } + +private: + rclcpp::Subscription::SharedPtr sub1_; + rclcpp::Subscription::SharedPtr sub2_; + rclcpp::Subscription::SharedPtr sub3_; +}; +#endif // WAIT_SET__RANDOM_LISTENER_HPP_ diff --git a/rclcpp/wait_set/include/wait_set/random_talker.hpp b/rclcpp/wait_set/include/wait_set/random_talker.hpp new file mode 100644 index 00000000..8cf1316b --- /dev/null +++ b/rclcpp/wait_set/include/wait_set/random_talker.hpp @@ -0,0 +1,74 @@ +// 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. + +#ifndef WAIT_SET__RANDOM_TALKER_HPP_ +#define WAIT_SET__RANDOM_TALKER_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +class RandomTalker : public rclcpp::Node +{ +public: + RandomTalker() + : Node("random_talker"), + pub1_(this->create_publisher("topicA", 10)), + pub2_(this->create_publisher("topicB", 10)), + pub3_(this->create_publisher("topicC", 10)), + rand_engine_(static_cast( + std::abs(std::chrono::system_clock::now().time_since_epoch().count()) + )) + { + publish_functions_.emplace_back( + ([this]() { + std_msgs::msg::String msg; + msg.data = "A"; + RCLCPP_INFO(this->get_logger(), "Publishing: %s", msg.data.c_str()); + pub1_->publish(msg); + })); + publish_functions_.emplace_back( + ([this]() { + std_msgs::msg::String msg; + msg.data = "B"; + RCLCPP_INFO(this->get_logger(), "Publishing: %s", msg.data.c_str()); + pub2_->publish(msg); + })); + publish_functions_.emplace_back( + ([this]() { + std_msgs::msg::String msg; + msg.data = "C"; + RCLCPP_INFO(this->get_logger(), "Publishing: %s", msg.data.c_str()); + pub3_->publish(msg); + })); + auto timer_callback = + [this]() -> void { + std::shuffle(publish_functions_.begin(), publish_functions_.end(), rand_engine_); + for (const auto & f : publish_functions_) {f();} + }; + timer_ = this->create_wall_timer(std::chrono::seconds(1), timer_callback); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; + rclcpp::Publisher::SharedPtr pub3_; + std::vector> publish_functions_; + std::default_random_engine rand_engine_; +}; +#endif // WAIT_SET__RANDOM_TALKER_HPP_ diff --git a/rclcpp/wait_set/include/wait_set/talker.hpp b/rclcpp/wait_set/include/wait_set/talker.hpp new file mode 100644 index 00000000..09c912a7 --- /dev/null +++ b/rclcpp/wait_set/include/wait_set/talker.hpp @@ -0,0 +1,33 @@ +// 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. + +#ifndef WAIT_SET__TALKER_HPP_ +#define WAIT_SET__TALKER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "wait_set/visibility.h" + +class Talker : public rclcpp::Node +{ +public: + WAIT_SET_PUBLIC explicit Talker(rclcpp::NodeOptions options); + +private: + size_t count_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +#endif // WAIT_SET__TALKER_HPP_ diff --git a/rclcpp/wait_set/include/wait_set/visibility.h b/rclcpp/wait_set/include/wait_set/visibility.h new file mode 100644 index 00000000..a7449f02 --- /dev/null +++ b/rclcpp/wait_set/include/wait_set/visibility.h @@ -0,0 +1,66 @@ +// 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. + +#ifndef WAIT_SET__VISIBILITY_H_ +#define WAIT_SET__VISIBILITY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + + #ifdef __GNUC__ + #define WAIT_SET_EXPORT __attribute__ ((dllexport)) + #define WAIT_SET_IMPORT __attribute__ ((dllimport)) + #else + #define WAIT_SET_EXPORT __declspec(dllexport) + #define WAIT_SET_IMPORT __declspec(dllimport) + #endif + + #ifdef WAIT_SET_DLL + #define WAIT_SET_PUBLIC WAIT_SET_EXPORT + #else + #define WAIT_SET_PUBLIC WAIT_SET_IMPORT + #endif + + #define WAIT_SET_PUBLIC_TYPE WAIT_SET_PUBLIC + + #define WAIT_SET_LOCAL + +#else + + #define WAIT_SET_EXPORT __attribute__ ((visibility("default"))) + #define WAIT_SET_IMPORT + + #if __GNUC__ >= 4 + #define WAIT_SET_PUBLIC __attribute__ ((visibility("default"))) + #define WAIT_SET_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define WAIT_SET_PUBLIC + #define WAIT_SET_LOCAL + #endif + + #define WAIT_SET_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // WAIT_SET__VISIBILITY_H_ diff --git a/rclcpp/wait_set/package.xml b/rclcpp/wait_set/package.xml new file mode 100644 index 00000000..74d0c554 --- /dev/null +++ b/rclcpp/wait_set/package.xml @@ -0,0 +1,27 @@ + + + + examples_rclcpp_wait_set + 0.8.2 + Example of how to use the rclcpp::WaitSet directly. + William Woodall + Apache License 2.0 + + ament_cmake + + example_interfaces + rclcpp + rclcpp_components + std_msgs + + example_interfaces + rclcpp + rclcpp_components + std_msgs + + + ament_cmake + + diff --git a/rclcpp/wait_set/src/executor_random_order.cpp b/rclcpp/wait_set/src/executor_random_order.cpp new file mode 100644 index 00000000..c2dfb907 --- /dev/null +++ b/rclcpp/wait_set/src/executor_random_order.cpp @@ -0,0 +1,45 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include "wait_set/random_listener.hpp" +#include "wait_set/random_talker.hpp" + +/* For this example, we will be creating a talker node with three publishers which will + * publish the topics A, B, C in random order each time. In this case the messages are handled + * using an executor. The order in which the messages are handled will depend on the message + * arrival and the type of messages available at the moment. + */ + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + // Note the order of execution would be deterministic if both nodes are spun in the same + // executor (A, B, C). This is because the publishing happens always before the subscription + // handling and the executor handles the messages in the order in which the subscriptions were + // created. Using different threads the handling order depends on the message arrival and + // type of messages available. + auto thread = std::thread([]() {rclcpp::spin(std::make_shared());}); + rclcpp::spin(std::make_shared()); + + rclcpp::shutdown(); + thread.join(); + + return 0; +} diff --git a/rclcpp/wait_set/src/listener.cpp b/rclcpp/wait_set/src/listener.cpp new file mode 100644 index 00000000..e56bda06 --- /dev/null +++ b/rclcpp/wait_set/src/listener.cpp @@ -0,0 +1,83 @@ +// 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 +#include + +#include "wait_set/listener.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +Listener::Listener(rclcpp::NodeOptions options) +: Node("listener", options) +{ + auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s' (executor)", msg->data.c_str()); + }; + subscription1_ = this->create_subscription( + "topic", + 10, + subscription_callback + ); + + auto wait_set_subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s' (wait-set)", msg->data.c_str()); + }; + 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; + subscription2_ = this->create_subscription( + "topic", + 10, + wait_set_subscription_callback, + subscription_options); + wait_set_.add_subscription(subscription2_); + thread_ = std::thread([this]() -> void {spin_wait_set();}); +} + +Listener::~Listener() +{ + if (thread_.joinable()) { + thread_.join(); + } +} + +void Listener::spin_wait_set() +{ + while (rclcpp::ok()) { + // Waiting up to 1s for a message to arrive + const auto wait_result = wait_set_.wait(std::chrono::seconds(1)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (subscription2_->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscription2_->handle_message(type_erased_msg, msg_info); + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout"); + } + } + } +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(Listener) diff --git a/rclcpp/wait_set/src/static_wait_set.cpp b/rclcpp/wait_set/src/static_wait_set.cpp new file mode 100644 index 00000000..79ba4d1a --- /dev/null +++ b/rclcpp/wait_set/src/static_wait_set.cpp @@ -0,0 +1,95 @@ +// Copyright 2021 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("static_wait_set_example_node"); + + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + auto sub1 = node->create_subscription("~/chatter", 10, do_nothing); + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing); + std::vector sub_vector = {sub1, sub2}; + auto guard_condition1 = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + rclcpp::StaticWaitSet<2, 2, 0, 0, 0, 0> static_wait_set( + std::array::SubscriptionEntry, 2>{{{sub1}, {sub2}}}, + std::array{{{guard_condition1}, {guard_condition2}}}, + std::array{}, + std::array{}, + std::array{}, + std::array::WaitableEntry, 0>{}); + + auto wait_and_print_results = [&]() { + RCLCPP_INFO(node->get_logger(), "Waiting..."); + auto wait_result = static_wait_set.wait(std::chrono::seconds(1)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + size_t guard_conditions_num = static_wait_set.get_rcl_wait_set().size_of_guard_conditions; + size_t subscriptions_num = static_wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (size_t i = 0; i < guard_conditions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[i]) { + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); + } + } + for (size_t i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + RCLCPP_INFO(node->get_logger(), "subscription %zu triggered", i + 1); + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (sub_vector.at(i)->take(msg, msg_info)) { + RCLCPP_INFO( + node->get_logger(), + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); + } + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed with timeout"); + } else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed because wait-set is empty"); + } + }; + + RCLCPP_INFO(node->get_logger(), "Action: Nothing triggered"); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 1"); + guard_condition1->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 2"); + guard_condition2->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Message published"); + auto pub = node->create_publisher("~/chatter", 1); + pub->publish(std_msgs::msg::String().set__data("test")); + wait_and_print_results(); + + // Note the static wait-set does not allow adding or removing entities dynamically. + // It will result in a compilation error. + + return 0; +} diff --git a/rclcpp/wait_set/src/talker.cpp b/rclcpp/wait_set/src/talker.cpp new file mode 100644 index 00000000..86a7aa2b --- /dev/null +++ b/rclcpp/wait_set/src/talker.cpp @@ -0,0 +1,39 @@ +// 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 + +#include "wait_set/talker.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +Talker::Talker(rclcpp::NodeOptions options) +: Node("talker", options), count_(0) +{ + publisher_ = create_publisher("topic", 10); + auto timer_callback = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publisher: '%s'", message.data.c_str()); + publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(Talker) diff --git a/rclcpp/wait_set/src/thread_safe_wait_set.cpp b/rclcpp/wait_set/src/thread_safe_wait_set.cpp new file mode 100644 index 00000000..b0543e11 --- /dev/null +++ b/rclcpp/wait_set/src/thread_safe_wait_set.cpp @@ -0,0 +1,112 @@ +// Copyright 2021 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("wait_set_example_node"); + + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + auto sub1 = node->create_subscription("~/chatter", 10, do_nothing); + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing); + std::vector sub_vector = {sub1, sub2}; + auto guard_condition1 = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + // FIXME: removing sub if it was added in the ctor leads to a failure + // terminate called after throwing an instance of 'std::runtime_error' + // what(): waitable not in wait set + rclcpp::ThreadSafeWaitSet wait_set( + // std::vector{{sub}}, + {}, + std::vector{guard_condition1}); + + wait_set.add_subscription(sub1); // FIXME: add it in the ctor + wait_set.add_subscription(sub2); + wait_set.add_guard_condition(guard_condition2); + + auto wait_and_print_results = [&]() { + RCLCPP_INFO(node->get_logger(), "Waiting..."); + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + size_t guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; + size_t subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (size_t i = 0; i < guard_conditions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[i]) { + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); + } + } + for (size_t i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + RCLCPP_INFO(node->get_logger(), "subscription %zu triggered", i + 1); + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (sub_vector.at(i)->take(msg, msg_info)) { + RCLCPP_INFO( + node->get_logger(), + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); + } + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed with timeout"); + } else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed because wait-set is empty"); + } + }; + + RCLCPP_INFO(node->get_logger(), "Action: Nothing triggered"); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 1"); + guard_condition1->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 2"); + guard_condition2->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Message published"); + auto pub = node->create_publisher("~/chatter", 1); + pub->publish(std_msgs::msg::String().set__data("test")); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Guard condition 1 removed"); + RCLCPP_INFO(node->get_logger(), "Action: Guard condition 2 removed"); + wait_set.remove_guard_condition(guard_condition1); + wait_set.remove_guard_condition(guard_condition2); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Subscription 2 removed"); + wait_set.remove_subscription(sub2); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Subscription 1 removed"); + wait_set.remove_subscription(sub1); + wait_and_print_results(); + + + return 0; +} diff --git a/rclcpp/wait_set/src/wait_set.cpp b/rclcpp/wait_set/src/wait_set.cpp new file mode 100644 index 00000000..b6422925 --- /dev/null +++ b/rclcpp/wait_set/src/wait_set.cpp @@ -0,0 +1,111 @@ +// Copyright 2021 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 +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("wait_set_example_node"); + + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + auto sub1 = node->create_subscription("~/chatter", 10, do_nothing); + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing); + std::vector sub_vector = {sub1, sub2}; + auto guard_condition1 = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + // FIXME: removing sub if it was added in the ctor leads to a failure + // terminate called after throwing an instance of 'std::runtime_error' + // what(): waitable not in wait set + rclcpp::WaitSet wait_set( + // std::vector{{sub}}, + {}, + std::vector{guard_condition1}); + + wait_set.add_subscription(sub1); // FIXME: add it in the ctor + wait_set.add_subscription(sub2); + wait_set.add_guard_condition(guard_condition2); + + auto wait_and_print_results = [&]() { + RCLCPP_INFO(node->get_logger(), "Waiting..."); + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + size_t guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; + size_t subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (size_t i = 0; i < guard_conditions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[i]) { + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); + } + } + for (size_t i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + RCLCPP_INFO(node->get_logger(), "subscription %zu triggered", i + 1); + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (sub_vector.at(i)->take(msg, msg_info)) { + RCLCPP_INFO( + node->get_logger(), + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); + } + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed with timeout"); + } else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) { + RCLCPP_INFO(node->get_logger(), "wait-set waiting failed because wait-set is empty"); + } + }; + + RCLCPP_INFO(node->get_logger(), "Action: Nothing triggered"); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 1"); + guard_condition1->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 2"); + guard_condition2->trigger(); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Message published"); + auto pub = node->create_publisher("~/chatter", 1); + pub->publish(std_msgs::msg::String().set__data("test")); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Guard condition 1 removed"); + RCLCPP_INFO(node->get_logger(), "Action: Guard condition 2 removed"); + wait_set.remove_guard_condition(guard_condition1); + wait_set.remove_guard_condition(guard_condition2); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Subscription 2 removed"); + wait_set.remove_subscription(sub2); + wait_and_print_results(); + + RCLCPP_INFO(node->get_logger(), "Action: Subscription 1 removed"); + wait_set.remove_subscription(sub1); + wait_and_print_results(); + + return 0; +} diff --git a/rclcpp/wait_set/src/wait_set_composed.cpp b/rclcpp/wait_set/src/wait_set_composed.cpp new file mode 100644 index 00000000..a248b960 --- /dev/null +++ b/rclcpp/wait_set/src/wait_set_composed.cpp @@ -0,0 +1,32 @@ +// 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 +#include "wait_set/talker.hpp" +#include "wait_set/listener.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + auto talker = std::make_shared(options); + auto listener = std::make_shared(options); + exec.add_node(talker); + exec.add_node(listener); + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/wait_set/src/wait_set_random_order.cpp b/rclcpp/wait_set/src/wait_set_random_order.cpp new file mode 100644 index 00000000..a71d5a82 --- /dev/null +++ b/rclcpp/wait_set/src/wait_set_random_order.cpp @@ -0,0 +1,78 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include "wait_set/random_listener.hpp" +#include "wait_set/random_talker.hpp" + +using namespace std::chrono_literals; + +/* For this example, we will be creating a talker node with three publishers which will + * publish the topics A, B, C in random order each time. The order in which the messages are + * handled is defined deterministically by the user in the code using a wait-set based loop. + * That is, in this example we always take and process the data in the same order A, B, C + * regardless of the arrival order. */ + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + auto random_listener = std::make_shared(); + auto subscriptions = random_listener->get_subscriptions(); + + // Create a wait-set and add the subscriptions + rclcpp::WaitSet wait_set; + for (const auto & subscription : subscriptions) { + wait_set.add_subscription(subscription); + } + + // Create a random talker and start publishing in another thread + auto thread = std::thread([]() {rclcpp::spin(std::make_shared());}); + + while (rclcpp::ok()) { + const auto wait_result = wait_set.wait(2s); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + bool sub1_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]; + bool sub2_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]; + bool sub3_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]; + + // Handle all the messages when all subscriptions have data + if (sub1_has_data && sub2_has_data && sub3_has_data) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + const size_t subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + for (size_t i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + if (subscriptions.at(i)->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscriptions.at(i)->handle_message(type_erased_msg, msg_info); + } + } + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(random_listener->get_logger(), "Wait-set failed with timeout"); + } + } + } + + rclcpp::shutdown(); + thread.join(); + return 0; +} diff --git a/rclcpp/wait_set/src/wait_set_topics_and_timer.cpp b/rclcpp/wait_set/src/wait_set_topics_and_timer.cpp new file mode 100644 index 00000000..3b61bf1b --- /dev/null +++ b/rclcpp/wait_set/src/wait_set_topics_and_timer.cpp @@ -0,0 +1,108 @@ +// 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 + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* This example creates a node with three publishers, three subscriptions and a one-off timer. The + * node will use a wait-set based loop to trigger the timer, publish the messages and handle the + * received data */ + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("wait_set_listener"); + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + + auto sub1 = node->create_subscription("topicA", 1, do_nothing); + auto sub2 = node->create_subscription("topicB", 1, do_nothing); + auto sub3 = node->create_subscription("topicC", 1, do_nothing); + + std_msgs::msg::String msg1, msg2, msg3; + msg1.data = "Hello, world!"; + msg2.data = "Hello, world!"; + msg3.data = "Hello, world!"; + + const auto pub1 = + node->create_publisher("topicA", 1); + const auto pub2 = + node->create_publisher("topicB", 1); + const auto pub3 = + node->create_publisher("topicC", 1); + + // Use a timer to schedule one-off message publishing. + // Note in this case the callback won't be triggered automatically. It is up to the user to + // trigger it manually inside the wait-set loop. + rclcpp::TimerBase::SharedPtr one_off_timer; + auto timer_callback = [&]() { + RCLCPP_INFO(node->get_logger(), "Publishing msg1: '%s'", msg1.data.c_str()); + RCLCPP_INFO(node->get_logger(), "Publishing msg2: '%s'", msg2.data.c_str()); + RCLCPP_INFO(node->get_logger(), "Publishing msg3: '%s'", msg3.data.c_str()); + pub1->publish(msg1); + pub2->publish(msg2); + pub3->publish(msg3); + // disable the timer after the first call + one_off_timer->cancel(); + }; + + one_off_timer = node->create_wall_timer(1s, timer_callback); + + rclcpp::WaitSet wait_set({{{sub1}, {sub2}, {sub3}}}, {}, {one_off_timer}); + + // Loop waiting on the wait-set until 3 messages are received + auto num_recv = std::size_t(); + while (num_recv < 3U) { + const auto wait_result = wait_set.wait(2s); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { + // The timer callback is executed manually here + one_off_timer->execute_callback(); + } else { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { + if (sub1->take(msg, msg_info)) { + ++num_recv; + RCLCPP_INFO(node->get_logger(), "msg1 data: '%s'", msg.data.c_str()); + } + } + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]) { + if (sub2->take(msg, msg_info)) { + ++num_recv; + RCLCPP_INFO(node->get_logger(), "msg2 data: '%s'", msg.data.c_str()); + } + } + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]) { + if (sub3->take(msg, msg_info)) { + ++num_recv; + RCLCPP_INFO(node->get_logger(), "msg3 data: '%s'", msg.data.c_str()); + } + } + RCLCPP_INFO(node->get_logger(), "Number of messages already got: %zu of 3", num_recv); + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Wait-set failed with timeout"); + } + } + } + RCLCPP_INFO(node->get_logger(), "Got all messages!"); + + return 0; +} diff --git a/rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp b/rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp new file mode 100644 index 00000000..3a4f42a0 --- /dev/null +++ b/rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp @@ -0,0 +1,125 @@ +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace std::chrono_literals; + +/* For this example, we will be creating three talkers publishing the topics A, B, C at different + * rates. The messages are handled by a wait-set loop which handles topics A and B together on a + * using topic B as trigger condition. Topic C is handled independently using the topic C + * itself as a trigger condition. */ + +class Talker : public rclcpp::Node +{ +public: + Talker( + const std::string & node_name, + const std::string & topic_name, + const std::string & message_data, + std::chrono::nanoseconds period) + : Node(node_name) + { + publisher_ = this->create_publisher(topic_name, 10); + auto timer_callback = + [this, message_data]() -> void { + std_msgs::msg::String message; + message.data = message_data; + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(period, timer_callback); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; +}; + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("wait_set_listener"); + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + + auto sub1 = node->create_subscription("topicA", 10, do_nothing); + auto sub2 = node->create_subscription("topicB", 10, do_nothing); + auto sub3 = node->create_subscription("topicC", 10, do_nothing); + + rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); + + // Create three talkers publishing in topics A, B, and C with different publishing rates + auto talkerA = std::make_shared("TalkerA", "topicA", "A", 1500ms); + auto talkerB = std::make_shared("TalkerB", "topicB", "B", 2000ms); + auto talkerC = std::make_shared("TalkerC", "topicC", "C", 3000ms); + + // Create an executor to spin the talkers in a separate thread + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(talkerA); + exec.add_node(talkerB); + exec.add_node(talkerC); + auto publisher_thread = std::thread([&exec]() {exec.spin();}); + + while (rclcpp::ok()) { + const auto wait_result = wait_set.wait(3s); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + bool sub2_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]; + bool sub3_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]; + + // topic A and B handling + // Note only topic B is used as a trigger condition + if (sub2_has_data) { + std_msgs::msg::String msg1; + std_msgs::msg::String msg2; + rclcpp::MessageInfo msg_info; + std::string handled_data; + + if (sub2->take(msg2, msg_info)) { + // since topic A is published at a faster rate we expect to take multiple messages + while (sub1->take(msg1, msg_info)) { + handled_data.append(msg1.data); + } + handled_data.append(msg2.data); + RCLCPP_INFO(node->get_logger(), "I heard: '%s'", handled_data.c_str()); + } else { + RCLCPP_ERROR(node->get_logger(), "An invalid message from topic B was received."); + } + } + + // topic C handling + if (sub3_has_data) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (sub3->take(msg, msg_info)) { + RCLCPP_INFO(node->get_logger(), "I heard: '%s'", msg.data.c_str()); + } else { + RCLCPP_ERROR(node->get_logger(), "An invalid message from topic C was received."); + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Wait-set failed with timeout"); + } + } + } + + rclcpp::shutdown(); + publisher_thread.join(); + return 0; +}