From 709f3444af5062c512b29ecb927d2b3c8c8c03d9 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 1 Apr 2020 06:48:17 -0700 Subject: [PATCH 01/59] wip Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 24 ++++++++++++++ rclcpp/wait_set/package.xml | 23 +++++++++++++ rclcpp/wait_set/wait_set.cpp | 60 ++++++++++++++++++++++++++++++++++ 3 files changed, 107 insertions(+) create mode 100644 rclcpp/wait_set/CMakeLists.txt create mode 100644 rclcpp/wait_set/package.xml create mode 100644 rclcpp/wait_set/wait_set.cpp diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt new file mode 100644 index 00000000..5f68f202 --- /dev/null +++ b/rclcpp/wait_set/CMakeLists.txt @@ -0,0 +1,24 @@ +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(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(wait_set wait_set.cpp) +ament_target_dependencies(wait_set rclcpp std_msgs) + +install(TARGETS + wait_set + DESTINATION lib/${PROJECT_NAME} +) +ament_package() diff --git a/rclcpp/wait_set/package.xml b/rclcpp/wait_set/package.xml new file mode 100644 index 00000000..e354d2ba --- /dev/null +++ b/rclcpp/wait_set/package.xml @@ -0,0 +1,23 @@ + + + + examples_rclcpp_wait_set + 0.8.2 + Example of how to use the rclcpp::WaitSet directly. + William Woodall + Apache License 2.0 + + ament_cmake + + rclcpp + std_msgs + + rclcpp + std_msgs + + + ament_cmake + + diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp new file mode 100644 index 00000000..8fba162e --- /dev/null +++ b/rclcpp/wait_set/wait_set.cpp @@ -0,0 +1,60 @@ +// 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 +// #include +// #include + +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + // auto node = std::make_shared("wait_set_example_node"); + + // Normal WaitSet example + { + auto guard_condition = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + rclcpp::WaitSet wait_set({{guard_condition}}); + wait_set.add_guard_condition(guard_condition2); + wait_set.remove_guard_condition(guard_condition2); + } + + // StaticWaitSet example + { + auto guard_condition = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + rclcpp::StaticWaitSet<1> static_wait_set({{guard_condition}}); + // 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; + } + + // ThreadSafeWaitSet example + // { + // auto guard_condition = std::make_shared(); + // auto guard_condition2 = std::make_shared(); + + // rclcpp::WaitSet wait_set({{guard_condition}}); + // wait_set.add_guard_condition(guard_condition2); + // } + + return 0; +} From fb71560647533b47eb930426b59cfc44e86a18f7 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 3 Apr 2020 05:22:48 -0700 Subject: [PATCH 02/59] more WIP examples Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set.cpp | 30 ++++++++++++++++++++++++++++-- 1 file changed, 28 insertions(+), 2 deletions(-) diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 8fba162e..6bc6d637 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -29,9 +29,29 @@ int main(int argc, char * argv[]) auto guard_condition = std::make_shared(); auto guard_condition2 = std::make_shared(); - rclcpp::WaitSet wait_set({{guard_condition}}); + rclcpp::WaitSet wait_set(std::vector{guard_condition}); wait_set.add_guard_condition(guard_condition2); + + { + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout); + } + 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); + + { + // now fails (fast) with empty + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + assert(wait_result.kind() == rclcpp::WaitResultKind::Empty); + } } // StaticWaitSet example @@ -39,12 +59,18 @@ int main(int argc, char * argv[]) auto guard_condition = std::make_shared(); auto guard_condition2 = std::make_shared(); - rclcpp::StaticWaitSet<1> static_wait_set({{guard_condition}}); + rclcpp::StaticWaitSet<1> static_wait_set( + std::array{{guard_condition}}); // 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); + } } // ThreadSafeWaitSet example From 4fa827fdbe62b5780bb6a47bd6f7d7cb37cb0c9e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Mon, 6 Apr 2020 09:38:32 -0700 Subject: [PATCH 03/59] updates Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set.cpp | 45 ++++++++++++++++++++++++++++++++---- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 6bc6d637..8c6f5270 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -17,19 +17,28 @@ // #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){}; // Normal WaitSet example { + auto node = std::make_shared("wait_set_example_node"); + auto sub = node->create_subscription("~/chatter", 10, do_nothing); + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); auto guard_condition = std::make_shared(); auto guard_condition2 = std::make_shared(); - rclcpp::WaitSet wait_set(std::vector{guard_condition}); + rclcpp::WaitSet wait_set( + std::vector{sub}, + std::vector{guard_condition}); + wait_set.add_subscription(sub2); wait_set.add_guard_condition(guard_condition2); { @@ -37,6 +46,15 @@ int main(int argc, char * argv[]) 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); + } + wait_set.remove_guard_condition(guard_condition2); { @@ -47,6 +65,22 @@ int main(int argc, char * argv[]) 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(sub); + + { + // 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); + { // now fails (fast) with empty auto wait_result = wait_set.wait(std::chrono::seconds(1)); @@ -59,8 +93,11 @@ int main(int argc, char * argv[]) auto guard_condition = std::make_shared(); auto guard_condition2 = std::make_shared(); - rclcpp::StaticWaitSet<1> static_wait_set( - std::array{{guard_condition}}); + rclcpp::StaticWaitSet<0, 1, 0, 0> static_wait_set( + std::array{}, + std::array{{guard_condition}}, + std::array{}, + std::array::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); From 78bdd931040dc435d497598f9f16b2beb6825be7 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 7 Apr 2020 08:29:03 -0700 Subject: [PATCH 04/59] adding examples using Subscription::take() Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set.cpp | 23 +++++++++++++++++++---- 1 file changed, 19 insertions(+), 4 deletions(-) diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 8c6f5270..b21bd427 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -36,7 +36,7 @@ int main(int argc, char * argv[]) auto guard_condition2 = std::make_shared(); rclcpp::WaitSet wait_set( - std::vector{sub}, + std::vector{{sub}}, std::vector{guard_condition}); wait_set.add_subscription(sub2); wait_set.add_guard_condition(guard_condition2); @@ -53,6 +53,8 @@ int main(int argc, char * argv[]) 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); @@ -71,7 +73,7 @@ int main(int argc, char * argv[]) assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout); } - wait_set.remove_subscription(sub); + wait_set.remove_subscription(sub2); { // still fails with timeout @@ -79,7 +81,20 @@ int main(int argc, char * argv[]) assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout); } - wait_set.remove_subscription(sub2); + auto pub = node->create_publisher("~/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; + rmw_message_info_t msg_info; + assert(sub->take(msg, msg_info)); + assert(msg.data == "test"); + } + + wait_set.remove_subscription(sub); { // now fails (fast) with empty @@ -94,7 +109,7 @@ int main(int argc, char * argv[]) auto guard_condition2 = std::make_shared(); rclcpp::StaticWaitSet<0, 1, 0, 0> static_wait_set( - std::array{}, + std::array::SubscriptionEntry, 0>{}, std::array{{guard_condition}}, std::array{}, std::array::WaitableEntry, 0>{}); From f7738506f826a91178eee689bc126bb693112818 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Tue, 7 Apr 2020 09:28:30 -0700 Subject: [PATCH 05/59] use rclcpp::MessageInfo Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index b21bd427..f8bffce8 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -89,7 +89,7 @@ int main(int argc, char * argv[]) 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; - rmw_message_info_t msg_info; + rclcpp::MessageInfo msg_info; assert(sub->take(msg, msg_info)); assert(msg.data == "test"); } From 593e5b9bc99f100ce23ffac761310aa1dfb4644f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Wed, 8 Apr 2020 05:12:53 -0700 Subject: [PATCH 06/59] break examples into separate files Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 3 + rclcpp/wait_set/static_wait_set.cpp | 46 ++++++++ rclcpp/wait_set/wait_set.cpp | 168 +++++++++++----------------- 3 files changed, 116 insertions(+), 101 deletions(-) create mode 100644 rclcpp/wait_set/static_wait_set.cpp diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 5f68f202..6279320c 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -17,6 +17,9 @@ find_package(std_msgs REQUIRED) add_executable(wait_set wait_set.cpp) ament_target_dependencies(wait_set rclcpp std_msgs) +add_executable(static_wait_set static_wait_set.cpp) +ament_target_dependencies(static_wait_set rclcpp std_msgs) + install(TARGETS wait_set DESTINATION lib/${PROJECT_NAME} diff --git a/rclcpp/wait_set/static_wait_set.cpp b/rclcpp/wait_set/static_wait_set.cpp new file mode 100644 index 00000000..52b217db --- /dev/null +++ b/rclcpp/wait_set/static_wait_set.cpp @@ -0,0 +1,46 @@ +// 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 + +#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(); + auto guard_condition2 = std::make_shared(); + + rclcpp::StaticWaitSet<0, 1, 0, 0> static_wait_set( + std::array::SubscriptionEntry, 0>{}, + std::array{{guard_condition}}, + std::array{}, + std::array::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; +} diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index f8bffce8..97a09df0 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -25,114 +25,80 @@ int main(int argc, char * argv[]) auto do_nothing = [](std_msgs::msg::String::UniquePtr){}; - // Normal WaitSet example + auto node = std::make_shared("wait_set_example_node"); + auto sub = node->create_subscription("~/chatter", 10, do_nothing); + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); + auto guard_condition = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + rclcpp::WaitSet wait_set( + std::vector{{sub}}, + std::vector{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); + { - auto node = std::make_shared("wait_set_example_node"); - auto sub = node->create_subscription("~/chatter", 10, do_nothing); - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); - auto guard_condition = std::make_shared(); - auto guard_condition2 = std::make_shared(); - - rclcpp::WaitSet wait_set( - std::vector{{sub}}, - std::vector{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("~/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); - } + // still fails with timeout + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout); } - // StaticWaitSet example + auto pub = node->create_publisher("~/chatter", 1); + pub->publish(std_msgs::msg::String().set__data("test")); + { - auto guard_condition = std::make_shared(); - auto guard_condition2 = std::make_shared(); - - rclcpp::StaticWaitSet<0, 1, 0, 0> static_wait_set( - std::array::SubscriptionEntry, 0>{}, - std::array{{guard_condition}}, - std::array{}, - std::array::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); - } + 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"); } - // ThreadSafeWaitSet example - // { - // auto guard_condition = std::make_shared(); - // auto guard_condition2 = std::make_shared(); + wait_set.remove_subscription(sub); - // rclcpp::WaitSet wait_set({{guard_condition}}); - // wait_set.add_guard_condition(guard_condition2); - // } + { + // now fails (fast) with empty + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + assert(wait_result.kind() == rclcpp::WaitResultKind::Empty); + } return 0; } From b4b41e35095a84a1b559fd2109268e6dc3315d93 Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 9 Apr 2020 06:46:35 -0700 Subject: [PATCH 07/59] update to support thread-safe wait set Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 8 +- rclcpp/wait_set/package.xml | 2 + rclcpp/wait_set/static_wait_set.cpp | 8 +- rclcpp/wait_set/thread_safe_wait_set.cpp | 109 +++++++++++++++++++++++ rclcpp/wait_set/wait_set.cpp | 27 +++++- 5 files changed, 146 insertions(+), 8 deletions(-) create mode 100644 rclcpp/wait_set/thread_safe_wait_set.cpp diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 6279320c..462c7623 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -11,17 +11,23 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 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 rclcpp std_msgs) +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() diff --git a/rclcpp/wait_set/package.xml b/rclcpp/wait_set/package.xml index e354d2ba..8455ea56 100644 --- a/rclcpp/wait_set/package.xml +++ b/rclcpp/wait_set/package.xml @@ -11,9 +11,11 @@ ament_cmake + example_interfaces rclcpp std_msgs + example_interfaces rclcpp std_msgs diff --git a/rclcpp/wait_set/static_wait_set.cpp b/rclcpp/wait_set/static_wait_set.cpp index 52b217db..8c75a4b5 100644 --- a/rclcpp/wait_set/static_wait_set.cpp +++ b/rclcpp/wait_set/static_wait_set.cpp @@ -26,11 +26,13 @@ int main(int argc, char * argv[]) auto guard_condition = std::make_shared(); auto guard_condition2 = std::make_shared(); - rclcpp::StaticWaitSet<0, 1, 0, 0> static_wait_set( - std::array::SubscriptionEntry, 0>{}, + rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> static_wait_set( + std::array::SubscriptionEntry, 0>{}, std::array{{guard_condition}}, std::array{}, - std::array::WaitableEntry, 0>{}); + std::array{}, + std::array{}, + std::array::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); diff --git a/rclcpp/wait_set/thread_safe_wait_set.cpp b/rclcpp/wait_set/thread_safe_wait_set.cpp new file mode 100644 index 00000000..e0cd17bf --- /dev/null +++ b/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 +#include +// #include +// #include + +#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("wait_set_example_node"); + + auto guard_condition = std::make_shared(); + auto guard_condition2 = std::make_shared(); + + auto sub = node->create_subscription("~/chatter", 10, do_nothing); + rclcpp::SubscriptionOptions so; + so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); + + rclcpp::ThreadSafeWaitSet wait_set( + std::vector{{sub}}, + std::vector{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("~/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; +} diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 97a09df0..5dc1f786 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -12,10 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include // #include // #include +#include "example_interfaces/srv/add_two_ints.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -23,15 +25,32 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto do_nothing = [](std_msgs::msg::String::UniquePtr){}; + auto do_nothing = [](std_msgs::msg::String::UniquePtr){ assert(false); }; + auto add_two_ints = + []( + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) + { assert(false); }; auto node = std::make_shared("wait_set_example_node"); + + auto guard_condition = std::make_shared(); + auto guard_condition2 = std::make_shared(); + auto sub = node->create_subscription("~/chatter", 10, do_nothing); rclcpp::SubscriptionOptions so; so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); - auto guard_condition = std::make_shared(); - auto guard_condition2 = std::make_shared(); + + auto pub = node->create_publisher("~/chatter", 10); + rclcpp::PublisherOptions po; + po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + auto pub2 = node->create_publisher("~/chatter", 10, po); + + auto client = node->create_client("~/add_two_ints"); + auto service = + node->create_service("~/add_two_ints", add_two_ints); rclcpp::WaitSet wait_set( std::vector{{sub}}, @@ -79,7 +98,7 @@ int main(int argc, char * argv[]) assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout); } - auto pub = node->create_publisher("~/chatter", 1); + // auto pub = node->create_publisher("~/chatter", 1); pub->publish(std_msgs::msg::String().set__data("test")); { From e915fce6a4b2ae430efc47f53837bb3f940fdf1f Mon Sep 17 00:00:00 2001 From: William Woodall Date: Thu, 9 Apr 2020 14:49:52 -0700 Subject: [PATCH 08/59] style Signed-off-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/thread_safe_wait_set.cpp | 2 +- rclcpp/wait_set/wait_set.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/wait_set/thread_safe_wait_set.cpp b/rclcpp/wait_set/thread_safe_wait_set.cpp index e0cd17bf..0d2b494b 100644 --- a/rclcpp/wait_set/thread_safe_wait_set.cpp +++ b/rclcpp/wait_set/thread_safe_wait_set.cpp @@ -25,7 +25,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto do_nothing = [](std_msgs::msg::String::UniquePtr){ assert(false); }; + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; auto node = std::make_shared("wait_set_example_node"); diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 5dc1f786..68eb0c0e 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -25,13 +25,13 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto do_nothing = [](std_msgs::msg::String::UniquePtr){ assert(false); }; + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; auto add_two_ints = []( - const std::shared_ptr, - const std::shared_ptr, - const std::shared_ptr) - { assert(false); }; + const std::shared_ptr, + const std::shared_ptr, + const std::shared_ptr) + {assert(false);}; auto node = std::make_shared("wait_set_example_node"); From ebcc4c79887ecaa1f5e28044d46cdb61705969ad Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 11 Jun 2021 08:25:54 +0200 Subject: [PATCH 09/59] Add addtional wait-set examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 93 +++++++++++ .../topics/minimal_subscriber/CMakeLists.txt | 4 + rclcpp/wait_set/CMakeLists.txt | 9 + rclcpp/wait_set/waitset_example_main.cpp | 112 +++++++++++++ rclcpp/wait_set/waitset_example_main_loop.cpp | 157 ++++++++++++++++++ 5 files changed, 375 insertions(+) create mode 100644 rclcpp/minimal_subscriber/waitset.cpp create mode 100644 rclcpp/wait_set/waitset_example_main.cpp create mode 100644 rclcpp/wait_set/waitset_example_main_loop.cpp diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp new file mode 100644 index 00000000..7129495b --- /dev/null +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -0,0 +1,93 @@ +// Copyright 2016 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" + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", + 10, + [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }); + } + + rclcpp::Subscription::SharedPtr get_subscription() const + { + return subscription_; + } + + void run() + { + rclcpp::WaitSet wait_set; + wait_set.add_subscription(subscription_); + while (rclcpp::ok()) { + const auto wait_result = wait_set.wait(); + 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; + bool message_received = subscription_->take(msg, msg_info); + if (message_received) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); + } + } + } + } + } + +private: + rclcpp::Subscription::SharedPtr subscription_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + + // Option 1: loop in main + auto sub = node->get_subscription(); + rclcpp::WaitSet wait_set; + wait_set.add_subscription(sub); + while (rclcpp::ok()) { + const auto wait_result = wait_set.wait(); + 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; + bool message_received = sub->take(msg, msg_info); + if (message_received) { + RCLCPP_INFO(node->get_logger(), "I heard: '%s'", msg.data.c_str()); + } + } + } + } + + // Option 2: run node + // node->run(); + + // rclcpp::spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index d9221c48..f692b525 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -32,6 +32,9 @@ 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_executable(waitset waitset.cpp) +ament_target_dependencies(waitset rclcpp std_msgs) + install(TARGETS subscriber_lambda subscriber_member_function @@ -39,6 +42,7 @@ install(TARGETS subscriber_member_function_with_type_adapter subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable + waitset DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 462c7623..41ee5a58 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -24,10 +24,19 @@ 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) +add_executable(waitset_example_main waitset_example_main.cpp) +ament_target_dependencies(waitset_example_main rclcpp std_msgs) + +add_executable(waitset_example_main_loop waitset_example_main_loop.cpp) +ament_target_dependencies(waitset_example_main_loop rclcpp std_msgs) + + install(TARGETS wait_set static_wait_set thread_safe_wait_set + waitset_example_main + waitset_example_main_loop DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/rclcpp/wait_set/waitset_example_main.cpp b/rclcpp/wait_set/waitset_example_main.cpp new file mode 100644 index 00000000..5dfaa6d3 --- /dev/null +++ b/rclcpp/wait_set/waitset_example_main.cpp @@ -0,0 +1,112 @@ +#include +#include + +int32_t main(const int32_t argc, char ** const 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 qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); + + auto sub1 = node->create_subscription("a11", qos, do_nothing); + auto sub2 = node->create_subscription("a22", qos, do_nothing); + auto sub3 = node->create_subscription("a33", qos, do_nothing); + + std_msgs::msg::String msg1; + msg1.data = "Hello, world!"; + + std_msgs::msg::String msg2; + msg2.data = "Hello, world!"; + + std_msgs::msg::String msg3; + msg3.data = "Hello, world!"; + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(sub1); + wait_set.add_subscription(sub2); + wait_set.add_subscription(sub3); + + rclcpp::Node thread_node("thread"); + const auto pub1 = + thread_node.create_publisher("a11", qos); + const auto pub2 = + thread_node.create_publisher("a22", qos); + const auto pub3 = + thread_node.create_publisher("a33", qos); + + // use transient_local ? + /* + pub1->wait_for_matched(1U, std::chrono::milliseconds(500U)); + pub2->wait_for_matched(1U, std::chrono::milliseconds(500U)); + pub3->wait_for_matched(1U, std::chrono::milliseconds(500U)); + + sub1->wait_for_matched(1U, std::chrono::milliseconds(500U)); + sub2->wait_for_matched(1U, std::chrono::milliseconds(500U)); + sub3->wait_for_matched(1U, std::chrono::milliseconds(500U)); +*/ + + auto thread = std::thread( + [msg1, msg2, msg3, pub1, pub2, pub3]() { + pub2->publish(msg2); + std::cout << "Publishing msg2: " << msg2.data << std::endl; + pub1->publish(msg1); + std::cout << "Publishing msg1: " << msg1.data << std::endl; + pub3->publish(msg3); + std::cout << "Publishing msg3: " << msg3.data << std::endl; + }); + + + // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + auto num_recv = std::size_t(); + while (num_recv < 3U) { + // Waiting up to 5s for a sample to arrive. + const auto wait_result = wait_set.wait(std::chrono::seconds(5)); + + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + // Use sub1 as triggering condition for all subs? + + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + bool message_received = sub1->take(msg, msg_info); + if (message_received) { + ++num_recv; + std::cout << "msg1 data: " << msg.data << std::endl; + } + } + + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + bool message_received = sub2->take(msg, msg_info); + if (message_received) { + ++num_recv; + std::cout << "msg2 data: " << msg.data << std::endl; + } + } + + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + bool message_received = sub3->take(msg, msg_info); + if (message_received) { + ++num_recv; + std::cout << "msg3 data: " << msg.data << std::endl; + } + } + + std::cout << "Number of messages already got: " << num_recv << " of 3" << std::endl; + + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + std::cerr << "No message received after 5s." << std::endl; + } else { + std::cerr << "Waitset failed" << std::endl; + } + } + std::cout << "Got all messages!" << std::endl; + + thread.join(); + return 0; +} diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/waitset_example_main_loop.cpp new file mode 100644 index 00000000..3403a3d1 --- /dev/null +++ b/rclcpp/wait_set/waitset_example_main_loop.cpp @@ -0,0 +1,157 @@ +#include +#include + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("waitset_node"); + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); + + auto sub1 = node->create_subscription("a11", qos, do_nothing); + auto sub2 = node->create_subscription("a22", qos, do_nothing); + auto sub3 = node->create_subscription("a33", qos, do_nothing); + + std_msgs::msg::String msg1; + // msg1.data = "Hello"; + msg1.data = "A"; + + std_msgs::msg::String msg2; + // msg2.data = "world"; + msg2.data = "B"; + + std_msgs::msg::String msg3; + // msg3.data = "from waitset"; + msg3.data = "C"; + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(sub1); + wait_set.add_subscription(sub2); + wait_set.add_subscription(sub3); + + rclcpp::Node publisher_node("publisher_node"); + const auto pub1 = + publisher_node.create_publisher("a11", qos); + const auto pub2 = + publisher_node.create_publisher("a22", qos); + const auto pub3 = + publisher_node.create_publisher("a33", qos); + + auto thread = std::thread( + [msg1, pub1, msg2, pub2, msg3, pub3, &publisher_node]() { + auto count{0}; + while (rclcpp::ok()) { + + // optional: make publish order random + // i.e: + // ABC, CBA, BAC, BCA, .... + // if different rates are used: + // AB, CBA, BAC, BCA, .... + // currently is always: ACB, ACB + pub1->publish(msg1); + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg1.data.c_str()); + + if (count % 2 == 0) { + if ((count/2) % 2 == 0) { + pub3->publish(msg3); + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg3.data.c_str()); + } + + pub2->publish(msg2); + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg2.data.c_str()); + } + ++count; + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } + }); +/* + auto thread1 = std::thread( + [msg1, pub1, &publisher_node]() { + while (rclcpp::ok()) { + + pub1->publish(msg1); + // std::cout << "Publishing msg1: " << msg1.data << std::endl; + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg1.data.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } + }); + + auto thread2 = std::thread( + [msg2, pub2, &publisher_node]() { + while (rclcpp::ok()) { + pub2->publish(msg2); + // std::cout << "Publishing msg2: " << msg2.data << std::endl; + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg2.data.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + } + }); + + auto thread3 = std::thread( + [msg3, pub3, &publisher_node]() { + while (rclcpp::ok()) { + pub3->publish(msg3); + // std::cout << "Publishing msg3: " << msg3.data << std::endl; + RCLCPP_INFO(publisher_node.get_logger(), "%s", msg3.data.c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(10000)); + } + }); +*/ + + // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + auto num_recv = std::size_t(); + while (rclcpp::ok()) { + // Waiting up to 5s for a sample to arrive. + const auto wait_result = wait_set.wait(std::chrono::seconds(5)); + + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + // Use sub1 as triggering condition for all subs? + + bool expected_sub_triggered = + wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U] && + wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]; + + if (expected_sub_triggered) { + std_msgs::msg::String msg1; + std_msgs::msg::String msg2; + std_msgs::msg::String msg3; + msg1.data.clear(); + msg2.data.clear(); + msg3.data.clear(); + rclcpp::MessageInfo msg_info; + if (sub1->take(msg1, msg_info)) { + ++num_recv; + } + if (sub2->take(msg2, msg_info)) { + ++num_recv; + } + if (sub3->take(msg3, msg_info)) { + ++num_recv; + RCLCPP_INFO( + node->get_logger(), "%s%s%s", + msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); + } else { + RCLCPP_INFO( + node->get_logger(), "%s%s", + msg1.data.c_str(), msg2.data.c_str()); + } + // std::cout << "I heard: " << msg1.data << msg2.data << msg3.data << std::endl; + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + std::cerr << "No message received after 5s." << std::endl; + } else { + std::cerr << "Waitset failed" << std::endl; + } + } + + rclcpp::shutdown(); + + thread.join(); + /* + thread1.join(); + thread2.join(); + thread3.join(); + */ + return 0; +} From 09749ff63a912669af6041e95180fd9bd4d2405a Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 24 Jun 2021 10:17:26 +0200 Subject: [PATCH 10/59] Shuffle publish calls Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/waitset_example_main_loop.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/waitset_example_main_loop.cpp index 3403a3d1..196757f2 100644 --- a/rclcpp/wait_set/waitset_example_main_loop.cpp +++ b/rclcpp/wait_set/waitset_example_main_loop.cpp @@ -1,5 +1,6 @@ #include #include +#include int32_t main(const int32_t argc, char ** const argv) { @@ -39,8 +40,33 @@ int32_t main(const int32_t argc, char ** const argv) const auto pub3 = publisher_node.create_publisher("a33", qos); + std::vector> random_publisher; + random_publisher.emplace_back(([&](){pub1->publish(msg1);})); + random_publisher.emplace_back(([&](){pub2->publish(msg2);})); + random_publisher.emplace_back(([&](){pub3->publish(msg3);})); + auto thread = std::thread([&random_publisher, &publisher_node]() { + auto count{0}; + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine e(seed); + std::vector indexes{0, 1, 2}; + std::vector msgs{"A", "B", "C"}; + while (rclcpp::ok()) { + std::shuffle (indexes.begin(), indexes.end(), e); + RCLCPP_INFO(publisher_node.get_logger(), "%s%s%s", + msgs[indexes.at(0)].c_str(), + msgs[indexes.at(1)].c_str(), + msgs[indexes.at(2)].c_str()); + random_publisher.at(indexes.at(0))(); + random_publisher.at(indexes.at(1))(); + random_publisher.at(indexes.at(2))(); + ++count; + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } + }); + /* auto thread = std::thread( [msg1, pub1, msg2, pub2, msg3, pub3, &publisher_node]() { + auto count{0}; while (rclcpp::ok()) { @@ -66,6 +92,7 @@ int32_t main(const int32_t argc, char ** const argv) std::this_thread::sleep_for(std::chrono::milliseconds(2500)); } }); + */ /* auto thread1 = std::thread( [msg1, pub1, &publisher_node]() { From 4c7cf9ac1e9ad45d01580224ade62ff66388d9a1 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 24 Jun 2021 16:42:23 +0200 Subject: [PATCH 11/59] Refactor publisher in a node class Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/waitset_example_main_loop.cpp | 191 ++++++------------ 1 file changed, 63 insertions(+), 128 deletions(-) diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/waitset_example_main_loop.cpp index 196757f2..17996b0b 100644 --- a/rclcpp/wait_set/waitset_example_main_loop.cpp +++ b/rclcpp/wait_set/waitset_example_main_loop.cpp @@ -2,6 +2,59 @@ #include #include +class RandomPublisher : public rclcpp::Node +{ +public: + RandomPublisher() + : Node("random_publisher"), count_(0) + { + auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); + pub1_ = this->create_publisher("topic_A", qos); + pub2_ = this->create_publisher("topic_B", qos); + pub3_ = this->create_publisher("topic_C", qos); + + auto run_random_publish = [this]() { + std_msgs::msg::String msg1, msg2, msg3; + std::vector> publishers; + msg1.data = "A"; + msg2.data = "B"; + msg3.data = "C"; + + publishers.emplace_back(([&](){pub1_->publish(msg1);})); + publishers.emplace_back(([&](){pub2_->publish(msg2);})); + publishers.emplace_back(([&](){pub3_->publish(msg3);})); + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine e(seed); + std::vector msgs_indexes{0, 1, 2}; + std::vector msgs_data{"A", "B", "C"}; + + while (rclcpp::ok()) { + std::shuffle (msgs_indexes.begin(), msgs_indexes.end(), e); + RCLCPP_INFO(this->get_logger(), "%s%s%s", + msgs_data[msgs_indexes.at(0)].c_str(), + msgs_data[msgs_indexes.at(1)].c_str(), + msgs_data[msgs_indexes.at(2)].c_str()); + publishers.at(msgs_indexes.at(0))(); + publishers.at(msgs_indexes.at(1))(); + publishers.at(msgs_indexes.at(2))(); + + ++count_; + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } + }; + + thread_ = std::thread(run_random_publish); + } + +private: + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; + rclcpp::Publisher::SharedPtr pub3_; + std::thread thread_; + size_t count_; +}; + + int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); @@ -11,133 +64,26 @@ int32_t main(const int32_t argc, char ** const argv) auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); - auto sub1 = node->create_subscription("a11", qos, do_nothing); - auto sub2 = node->create_subscription("a22", qos, do_nothing); - auto sub3 = node->create_subscription("a33", qos, do_nothing); - - std_msgs::msg::String msg1; - // msg1.data = "Hello"; - msg1.data = "A"; - - std_msgs::msg::String msg2; - // msg2.data = "world"; - msg2.data = "B"; - - std_msgs::msg::String msg3; - // msg3.data = "from waitset"; - msg3.data = "C"; + auto sub1 = node->create_subscription("topic_A", qos, do_nothing); + auto sub2 = node->create_subscription("topic_B", qos, do_nothing); + auto sub3 = node->create_subscription("topic_C", qos, do_nothing); rclcpp::WaitSet wait_set; wait_set.add_subscription(sub1); wait_set.add_subscription(sub2); wait_set.add_subscription(sub3); - rclcpp::Node publisher_node("publisher_node"); - const auto pub1 = - publisher_node.create_publisher("a11", qos); - const auto pub2 = - publisher_node.create_publisher("a22", qos); - const auto pub3 = - publisher_node.create_publisher("a33", qos); - - std::vector> random_publisher; - random_publisher.emplace_back(([&](){pub1->publish(msg1);})); - random_publisher.emplace_back(([&](){pub2->publish(msg2);})); - random_publisher.emplace_back(([&](){pub3->publish(msg3);})); - auto thread = std::thread([&random_publisher, &publisher_node]() { - auto count{0}; - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::default_random_engine e(seed); - std::vector indexes{0, 1, 2}; - std::vector msgs{"A", "B", "C"}; - while (rclcpp::ok()) { - std::shuffle (indexes.begin(), indexes.end(), e); - RCLCPP_INFO(publisher_node.get_logger(), "%s%s%s", - msgs[indexes.at(0)].c_str(), - msgs[indexes.at(1)].c_str(), - msgs[indexes.at(2)].c_str()); - random_publisher.at(indexes.at(0))(); - random_publisher.at(indexes.at(1))(); - random_publisher.at(indexes.at(2))(); - ++count; - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } - }); - /* - auto thread = std::thread( - [msg1, pub1, msg2, pub2, msg3, pub3, &publisher_node]() { - - auto count{0}; - while (rclcpp::ok()) { - - // optional: make publish order random - // i.e: - // ABC, CBA, BAC, BCA, .... - // if different rates are used: - // AB, CBA, BAC, BCA, .... - // currently is always: ACB, ACB - pub1->publish(msg1); - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg1.data.c_str()); - - if (count % 2 == 0) { - if ((count/2) % 2 == 0) { - pub3->publish(msg3); - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg3.data.c_str()); - } - - pub2->publish(msg2); - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg2.data.c_str()); - } - ++count; - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } - }); - */ -/* - auto thread1 = std::thread( - [msg1, pub1, &publisher_node]() { - while (rclcpp::ok()) { + RandomPublisher publisher_node; - pub1->publish(msg1); - // std::cout << "Publishing msg1: " << msg1.data << std::endl; - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg1.data.c_str()); - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } - }); - - auto thread2 = std::thread( - [msg2, pub2, &publisher_node]() { - while (rclcpp::ok()) { - pub2->publish(msg2); - // std::cout << "Publishing msg2: " << msg2.data << std::endl; - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg2.data.c_str()); - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); - } - }); - - auto thread3 = std::thread( - [msg3, pub3, &publisher_node]() { - while (rclcpp::ok()) { - pub3->publish(msg3); - // std::cout << "Publishing msg3: " << msg3.data << std::endl; - RCLCPP_INFO(publisher_node.get_logger(), "%s", msg3.data.c_str()); - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - } - }); -*/ - - // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); auto num_recv = std::size_t(); while (rclcpp::ok()) { - // Waiting up to 5s for a sample to arrive. const auto wait_result = wait_set.wait(std::chrono::seconds(5)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - // Use sub1 as triggering condition for all subs? - bool expected_sub_triggered = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U] && - wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]; + wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U] && + wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]; if (expected_sub_triggered) { std_msgs::msg::String msg1; @@ -155,15 +101,11 @@ int32_t main(const int32_t argc, char ** const argv) } if (sub3->take(msg3, msg_info)) { ++num_recv; - RCLCPP_INFO( - node->get_logger(), "%s%s%s", - msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); - } else { - RCLCPP_INFO( - node->get_logger(), "%s%s", - msg1.data.c_str(), msg2.data.c_str()); } - // std::cout << "I heard: " << msg1.data << msg2.data << msg3.data << std::endl; + // handle topics A B C all together + RCLCPP_INFO( + node->get_logger(), "%s%s%s", + msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { std::cerr << "No message received after 5s." << std::endl; @@ -173,12 +115,5 @@ int32_t main(const int32_t argc, char ** const argv) } rclcpp::shutdown(); - - thread.join(); - /* - thread1.join(); - thread2.join(); - thread3.join(); - */ return 0; } From 35d4e7a4f7284c29a3f4d27f3481f6f8c875e204 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 25 Jun 2021 12:02:36 +0200 Subject: [PATCH 12/59] Update examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 3 +- rclcpp/wait_set/waitset_composition.cpp | 118 ++++++++++++++ rclcpp/wait_set/waitset_example_main.cpp | 146 +++++++++-------- rclcpp/wait_set/waitset_example_main_loop.cpp | 150 ++++++++++-------- 4 files changed, 279 insertions(+), 138 deletions(-) create mode 100644 rclcpp/wait_set/waitset_composition.cpp diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index 7129495b..8b5f68db 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -1,4 +1,4 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. +// 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. @@ -65,7 +65,6 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared(); - // Option 1: loop in main auto sub = node->get_subscription(); rclcpp::WaitSet wait_set; diff --git a/rclcpp/wait_set/waitset_composition.cpp b/rclcpp/wait_set/waitset_composition.cpp new file mode 100644 index 00000000..0b46795d --- /dev/null +++ b/rclcpp/wait_set/waitset_composition.cpp @@ -0,0 +1,118 @@ +// 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 + +using namespace std::chrono_literals; + +/* This example creates a node with three publishers, three subscriptions and one-off timer. The + * node will use a wait-set 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_example_node"); + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + + auto sub1 = node->create_subscription("a11", 1, do_nothing); + auto sub2 = node->create_subscription("a22", 1, do_nothing); + auto sub3 = node->create_subscription("a33", 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("a11", 1); + const auto pub2 = + node->create_publisher("a22", 1); + const auto pub3 = + node->create_publisher("a33", 1); + + rclcpp::TimerBase::SharedPtr one_off_timer; + auto timer_callback = [msg1, msg2, msg3, pub1, pub2, pub3, one_off_timer, node]() { + 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(); + }; + + // 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 + // the user to trigger it manually inside the wait-set loop. + one_off_timer = node->create_wall_timer(1s, timer_callback); + + rclcpp::WaitSet wait_set; + wait_set.add_subscription(sub1); + wait_set.add_subscription(sub2); + wait_set.add_subscription(sub3); + wait_set.add_timer(one_off_timer); + + auto num_recv = std::size_t(); + while (num_recv < 3U) { + // Waiting up to 5s for a sample to arrive. + const auto wait_result = wait_set.wait(std::chrono::seconds(5)); + + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + + if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { + // we execute manually the timer callback + one_off_timer->execute_callback(); + } else { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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) { + RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + } else { + RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + } + } + RCLCPP_INFO(node->get_logger(), "Got all messages!"); + + return 0; +} diff --git a/rclcpp/wait_set/waitset_example_main.cpp b/rclcpp/wait_set/waitset_example_main.cpp index 5dfaa6d3..5287e0d2 100644 --- a/rclcpp/wait_set/waitset_example_main.cpp +++ b/rclcpp/wait_set/waitset_example_main.cpp @@ -1,6 +1,26 @@ +// 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 +using namespace std::chrono_literals; + +/* This example creates a node with three publishers, three subscriptions and one-off timer. The + * node will use a wait-set 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); @@ -8,105 +28,91 @@ int32_t main(const int32_t argc, char ** const argv) auto node = std::make_shared("wait_set_example_node"); auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); - - auto sub1 = node->create_subscription("a11", qos, do_nothing); - auto sub2 = node->create_subscription("a22", qos, do_nothing); - auto sub3 = node->create_subscription("a33", qos, do_nothing); + auto sub1 = node->create_subscription("a11", 1, do_nothing); + auto sub2 = node->create_subscription("a22", 1, do_nothing); + auto sub3 = node->create_subscription("a33", 1, do_nothing); - std_msgs::msg::String msg1; + std_msgs::msg::String msg1, msg2, msg3; msg1.data = "Hello, world!"; - - std_msgs::msg::String msg2; msg2.data = "Hello, world!"; - - std_msgs::msg::String msg3; msg3.data = "Hello, world!"; - rclcpp::WaitSet wait_set; - wait_set.add_subscription(sub1); - wait_set.add_subscription(sub2); - wait_set.add_subscription(sub3); - - rclcpp::Node thread_node("thread"); const auto pub1 = - thread_node.create_publisher("a11", qos); + node->create_publisher("a11", 1); const auto pub2 = - thread_node.create_publisher("a22", qos); + node->create_publisher("a22", 1); const auto pub3 = - thread_node.create_publisher("a33", qos); - - // use transient_local ? - /* - pub1->wait_for_matched(1U, std::chrono::milliseconds(500U)); - pub2->wait_for_matched(1U, std::chrono::milliseconds(500U)); - pub3->wait_for_matched(1U, std::chrono::milliseconds(500U)); - - sub1->wait_for_matched(1U, std::chrono::milliseconds(500U)); - sub2->wait_for_matched(1U, std::chrono::milliseconds(500U)); - sub3->wait_for_matched(1U, std::chrono::milliseconds(500U)); -*/ + node->create_publisher("a33", 1); - auto thread = std::thread( - [msg1, msg2, msg3, pub1, pub2, pub3]() { - pub2->publish(msg2); - std::cout << "Publishing msg2: " << msg2.data << std::endl; + rclcpp::TimerBase::SharedPtr one_off_timer; + auto timer_callback = [msg1, msg2, msg3, pub1, pub2, pub3, one_off_timer, node]() { + 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); - std::cout << "Publishing msg1: " << msg1.data << std::endl; + pub2->publish(msg2); pub3->publish(msg3); - std::cout << "Publishing msg3: " << msg3.data << std::endl; - }); + // disable the timer after the first call + one_off_timer->cancel(); + }; + + // 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 + // the user to trigger it manually inside the wait-set loop. + one_off_timer = node->create_wall_timer(1s, timer_callback); + rclcpp::WaitSet wait_set; + wait_set.add_subscription(sub1); + wait_set.add_subscription(sub2); + wait_set.add_subscription(sub3); + wait_set.add_timer(one_off_timer); - // std::this_thread::sleep_for(std::chrono::milliseconds(1000)); auto num_recv = std::size_t(); while (num_recv < 3U) { // Waiting up to 5s for a sample to arrive. const auto wait_result = wait_set.wait(std::chrono::seconds(5)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - // Use sub1 as triggering condition for all subs? - - if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - bool message_received = sub1->take(msg, msg_info); - if (message_received) { - ++num_recv; - std::cout << "msg1 data: " << msg.data << std::endl; + + if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { + // we execute manually the timer callback + one_off_timer->execute_callback(); + } else { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - bool message_received = sub2->take(msg, msg_info); - if (message_received) { - ++num_recv; - std::cout << "msg2 data: " << msg.data << std::endl; + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - bool message_received = sub3->take(msg, msg_info); - if (message_received) { - ++num_recv; - std::cout << "msg3 data: " << msg.data << std::endl; + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + 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); } - - std::cout << "Number of messages already got: " << num_recv << " of 3" << std::endl; - } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - std::cerr << "No message received after 5s." << std::endl; + RCLCPP_INFO(node->get_logger(), "No message received after 5s."); } else { - std::cerr << "Waitset failed" << std::endl; + RCLCPP_INFO(node->get_logger(), "Wait-set failed."); } } - std::cout << "Got all messages!" << std::endl; + RCLCPP_INFO(node->get_logger(), "Got all messages!"); - thread.join(); return 0; } diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/waitset_example_main_loop.cpp index 17996b0b..c1ae65b0 100644 --- a/rclcpp/wait_set/waitset_example_main_loop.cpp +++ b/rclcpp/wait_set/waitset_example_main_loop.cpp @@ -1,119 +1,137 @@ +// 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 +/* For this example, we will be creating a publishing 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 directly by the user in the code. That is, in this example + * we always take and process the data in the same order A, B, C regardless of the arrival order. + */ class RandomPublisher : public rclcpp::Node { public: RandomPublisher() - : Node("random_publisher"), count_(0) + : Node("random_publisher"), + pub1_(this->create_publisher("topicA", 10)), + pub2_(this->create_publisher("topicB", 10)), + pub3_(this->create_publisher("topicC", 10)), + count_(0U) { - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); - pub1_ = this->create_publisher("topic_A", qos); - pub2_ = this->create_publisher("topic_B", qos); - pub3_ = this->create_publisher("topic_C", qos); - - auto run_random_publish = [this]() { - std_msgs::msg::String msg1, msg2, msg3; - std::vector> publishers; - msg1.data = "A"; - msg2.data = "B"; - msg3.data = "C"; - - publishers.emplace_back(([&](){pub1_->publish(msg1);})); - publishers.emplace_back(([&](){pub2_->publish(msg2);})); - publishers.emplace_back(([&](){pub3_->publish(msg3);})); - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::default_random_engine e(seed); - std::vector msgs_indexes{0, 1, 2}; - std::vector msgs_data{"A", "B", "C"}; - - while (rclcpp::ok()) { - std::shuffle (msgs_indexes.begin(), msgs_indexes.end(), e); - RCLCPP_INFO(this->get_logger(), "%s%s%s", - msgs_data[msgs_indexes.at(0)].c_str(), - msgs_data[msgs_indexes.at(1)].c_str(), - msgs_data[msgs_indexes.at(2)].c_str()); - publishers.at(msgs_indexes.at(0))(); - publishers.at(msgs_indexes.at(1))(); - publishers.at(msgs_indexes.at(2))(); - - ++count_; - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } - }; + } - thread_ = std::thread(run_random_publish); + void run() + { + std_msgs::msg::String msg1, msg2, msg3; + std::vector> publisher_functions; + std::vector msgs_data{"A", "B", "C"}; + std::vector publish_order{0, 1, 2}; + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::default_random_engine e(seed); + + msg1.data = msgs_data.at(0U); + msg2.data = msgs_data.at(1U); + msg3.data = msgs_data.at(2U); + publisher_functions.emplace_back(([this, msg1]() {pub1_->publish(msg1);})); + publisher_functions.emplace_back(([this, msg2]() {pub2_->publish(msg2);})); + publisher_functions.emplace_back(([this, msg3]() {pub3_->publish(msg3);})); + + while (rclcpp::ok()) { + std::shuffle(publish_order.begin(), publish_order.end(), e); + RCLCPP_INFO( + this->get_logger(), "%zu %s%s%s", + count_, + msgs_data[publish_order.at(0)].c_str(), + msgs_data[publish_order.at(1)].c_str(), + msgs_data[publish_order.at(2)].c_str()); + publisher_functions.at(publish_order.at(0))(); + publisher_functions.at(publish_order.at(1))(); + publisher_functions.at(publish_order.at(2))(); + ++count_; + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } } private: rclcpp::Publisher::SharedPtr pub1_; rclcpp::Publisher::SharedPtr pub2_; rclcpp::Publisher::SharedPtr pub3_; - std::thread thread_; size_t count_; }; - int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); auto node = std::make_shared("waitset_node"); auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + size_t count{0U}; - auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).transient_local(); - - auto sub1 = node->create_subscription("topic_A", qos, do_nothing); - auto sub2 = node->create_subscription("topic_B", qos, do_nothing); - auto sub3 = node->create_subscription("topic_C", qos, do_nothing); + 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; wait_set.add_subscription(sub1); wait_set.add_subscription(sub2); wait_set.add_subscription(sub3); + // Creates a random publisher and starts publishing in another thread RandomPublisher publisher_node; + auto publisher_thread = std::thread([&publisher_node]() {publisher_node.run();}); - auto num_recv = std::size_t(); while (rclcpp::ok()) { + // Waiting up to 5s for a message to arrive const auto wait_result = wait_set.wait(std::chrono::seconds(5)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - bool expected_sub_triggered = - wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U] && - wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U] && - wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]; + // We will handle all the messages when all subscriptions have received data + 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]; + bool handle_condition = sub1_has_data && sub2_has_data && sub3_has_data; - if (expected_sub_triggered) { + if (handle_condition) { std_msgs::msg::String msg1; std_msgs::msg::String msg2; std_msgs::msg::String msg3; - msg1.data.clear(); - msg2.data.clear(); - msg3.data.clear(); rclcpp::MessageInfo msg_info; - if (sub1->take(msg1, msg_info)) { - ++num_recv; - } - if (sub2->take(msg2, msg_info)) { - ++num_recv; - } - if (sub3->take(msg3, msg_info)) { - ++num_recv; + + // the receiving order is not relevant, the messages are taken in a user-defined order + bool msg1_is_valid = sub1->take(msg1, msg_info); + bool msg2_is_valid = sub2->take(msg2, msg_info); + bool msg3_is_valid = sub3->take(msg3, msg_info); + + if (msg1_is_valid && msg2_is_valid && msg3_is_valid) { + RCLCPP_INFO( + node->get_logger(), "%zu %s%s%s", count, msg1.data.c_str(), + msg2.data.c_str(), msg3.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "Invalid message received."); } - // handle topics A B C all together - RCLCPP_INFO( - node->get_logger(), "%s%s%s", - msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); + ++count; } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - std::cerr << "No message received after 5s." << std::endl; + RCLCPP_INFO(node->get_logger(), "No message received after 5s."); } else { - std::cerr << "Waitset failed" << std::endl; + RCLCPP_INFO(node->get_logger(), "Wait-set failed."); } } rclcpp::shutdown(); + publisher_thread.join(); return 0; } From d197ed4ebebbb1187490bd88e66ee58ae2d2ee57 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 25 Jun 2021 12:02:52 +0200 Subject: [PATCH 13/59] Add waiset example using composition Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 3 + rclcpp/wait_set/waitset_composition.cpp | 179 +++++++++++++----------- 2 files changed, 98 insertions(+), 84 deletions(-) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 41ee5a58..04b06311 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -30,6 +30,8 @@ ament_target_dependencies(waitset_example_main rclcpp std_msgs) add_executable(waitset_example_main_loop waitset_example_main_loop.cpp) ament_target_dependencies(waitset_example_main_loop rclcpp std_msgs) +add_executable(waitset_composition waitset_composition.cpp) +ament_target_dependencies(waitset_composition rclcpp std_msgs) install(TARGETS wait_set @@ -37,6 +39,7 @@ install(TARGETS thread_safe_wait_set waitset_example_main waitset_example_main_loop + waitset_composition DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/rclcpp/wait_set/waitset_composition.cpp b/rclcpp/wait_set/waitset_composition.cpp index 0b46795d..2bec6c7a 100644 --- a/rclcpp/wait_set/waitset_composition.cpp +++ b/rclcpp/wait_set/waitset_composition.cpp @@ -12,107 +12,118 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include +#include +#include +#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 one-off timer. The - * node will use a wait-set to trigger the timer, publish the messages and handle the received - * data */ +class Talker : public rclcpp::Node +{ +public: + Talker() + : Node("talker"), count_(0) + { + publisher_ = this->create_publisher("topic", 10); + auto timer_callback = + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->publisher_->publish(message); + }; + timer_ = this->create_wall_timer(500ms, timer_callback); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; +}; + +class Listener : public rclcpp::Node +{ +public: + Listener() + : Node("listener") + { + subscription_executor_ = this->create_subscription( + "topic", + 10, + [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s' (executor)", msg->data.c_str()); + }); + + // creates a subscription which is not automatically added to an executor + rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + auto options = rclcpp::SubscriptionOptions(); + options.callback_group = cb_group_waitset; + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + subscription_waitset_ = this->create_subscription( + "topic", + 10, + do_nothing, + options); + } + + rclcpp::Subscription::SharedPtr get_subscription() const { + return subscription_waitset_; + } + +private: + rclcpp::Subscription::SharedPtr subscription_executor_; + rclcpp::Subscription::SharedPtr subscription_waitset_; +}; -int32_t main(const int32_t argc, char ** const argv) +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("a11", 1, do_nothing); - auto sub2 = node->create_subscription("a22", 1, do_nothing); - auto sub3 = node->create_subscription("a33", 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("a11", 1); - const auto pub2 = - node->create_publisher("a22", 1); - const auto pub3 = - node->create_publisher("a33", 1); - - rclcpp::TimerBase::SharedPtr one_off_timer; - auto timer_callback = [msg1, msg2, msg3, pub1, pub2, pub3, one_off_timer, node]() { - 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(); - }; - - // 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 - // the user to trigger it manually inside the wait-set loop. - one_off_timer = node->create_wall_timer(1s, timer_callback); + // Create an executor that will be responsible for execution of callbacks for a set of nodes. + // All callbacks will be called except the subscription which as explicitly excluded from it + rclcpp::executors::SingleThreadedExecutor exec; + + auto talker = std::make_shared(); + exec.add_node(talker); + auto listener = std::make_shared(); + exec.add_node(listener); rclcpp::WaitSet wait_set; - wait_set.add_subscription(sub1); - wait_set.add_subscription(sub2); - wait_set.add_subscription(sub3); - wait_set.add_timer(one_off_timer); - - auto num_recv = std::size_t(); - while (num_recv < 3U) { - // Waiting up to 5s for a sample to arrive. - const auto wait_result = wait_set.wait(std::chrono::seconds(5)); + wait_set.add_subscription(listener->get_subscription()); - if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + auto thread = std::thread([&exec]() { + // spin will block until work comes in, execute work as it becomes available, and keep blocking. + // It will only be interrupted by Ctrl-C. + exec.spin(); + }); - if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { - // we execute manually the timer callback - one_off_timer->execute_callback(); - } else { - if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - if (sub2->take(msg, msg_info)) { - ++num_recv; - RCLCPP_INFO(node->get_logger(), "msg2 data: %s", msg.data.c_str()); - } - } + while (rclcpp::ok()) { + // Waiting up to 5s for a message to arrive + const auto wait_result = wait_set.wait(std::chrono::seconds(5)); - if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[2U]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; - if (sub3->take(msg, msg_info)) { - ++num_recv; - RCLCPP_INFO(node->get_logger(), "msg3 data: %s", msg.data.c_str()); - } + 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 (listener->get_subscription()->take(msg, msg_info)) { + RCLCPP_INFO(listener->get_logger(), "I heard: '%s' (waitset)", 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) { - RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + RCLCPP_INFO(listener->get_logger(), "No message received after 5s."); } else { - RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + RCLCPP_INFO(listener->get_logger(), "Wait-set failed."); } } - RCLCPP_INFO(node->get_logger(), "Got all messages!"); + rclcpp::shutdown(); + thread.join(); return 0; -} +} \ No newline at end of file From 4274987b7e68f693d72ce8a94d577f83e0b52e3c Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 25 Jun 2021 12:51:19 +0200 Subject: [PATCH 14/59] Rename minimal_subscriber waitset example Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 6 +++--- rclcpp/topics/minimal_subscriber/CMakeLists.txt | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index 8b5f68db..7b175656 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -42,7 +42,7 @@ class MinimalSubscriber : public rclcpp::Node rclcpp::WaitSet wait_set; wait_set.add_subscription(subscription_); while (rclcpp::ok()) { - const auto wait_result = wait_set.wait(); + const auto wait_result = wait_set.wait(std::chrono::milliseconds(500)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; @@ -65,12 +65,13 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); auto node = std::make_shared(); + // Option 1: loop in main auto sub = node->get_subscription(); rclcpp::WaitSet wait_set; wait_set.add_subscription(sub); while (rclcpp::ok()) { - const auto wait_result = wait_set.wait(); + const auto wait_result = wait_set.wait(std::chrono::milliseconds(500)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; @@ -86,7 +87,6 @@ int main(int argc, char * argv[]) // Option 2: run node // node->run(); - // rclcpp::spin(); rclcpp::shutdown(); return 0; } diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index f692b525..2c9ae2cf 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -32,8 +32,8 @@ 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_executable(waitset waitset.cpp) -ament_target_dependencies(waitset rclcpp std_msgs) +add_executable(subscriber_waitset waitset.cpp) +ament_target_dependencies(subscriber_waitset rclcpp std_msgs) install(TARGETS subscriber_lambda @@ -42,7 +42,7 @@ install(TARGETS subscriber_member_function_with_type_adapter subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable - waitset + subscriber_waitset DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) From 3b3e1f431434e3756086f04ac4582aef63f6283a Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 25 Jun 2021 17:27:26 +0200 Subject: [PATCH 15/59] Update README Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index e11b22c4..5fb9cfd2 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -4,11 +4,12 @@ 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 - + * `waitset.cpp` uses a `rclcpp::waitset` to wait and poll for data + 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. From 04eba55081f5f2aa141f3255431a0ac664dac9a9 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 25 Jun 2021 17:27:57 +0200 Subject: [PATCH 16/59] Update waitset_example_main Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/waitset_example_main.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rclcpp/wait_set/waitset_example_main.cpp b/rclcpp/wait_set/waitset_example_main.cpp index 5287e0d2..afad9c2c 100644 --- a/rclcpp/wait_set/waitset_example_main.cpp +++ b/rclcpp/wait_set/waitset_example_main.cpp @@ -18,8 +18,8 @@ using namespace std::chrono_literals; /* This example creates a node with three publishers, three subscriptions and one-off timer. The - * node will use a wait-set to trigger the timer, publish the messages and handle the received - * data */ + * 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) { @@ -44,8 +44,11 @@ int32_t main(const int32_t argc, char ** const argv) const auto pub3 = node->create_publisher("a33", 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 + // the user to trigger it manually inside the wait-set loop. rclcpp::TimerBase::SharedPtr one_off_timer; - auto timer_callback = [msg1, msg2, msg3, pub1, pub2, pub3, one_off_timer, node]() { + 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()); @@ -56,9 +59,6 @@ int32_t main(const int32_t argc, char ** const argv) one_off_timer->cancel(); }; - // 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 - // the user to trigger it manually inside the wait-set loop. one_off_timer = node->create_wall_timer(1s, timer_callback); rclcpp::WaitSet wait_set; @@ -73,7 +73,6 @@ int32_t main(const int32_t argc, char ** const argv) const auto wait_result = wait_set.wait(std::chrono::seconds(5)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { // we execute manually the timer callback one_off_timer->execute_callback(); From f4f556eeca8d424148a2d0cdc8298c8e116f3d27 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 29 Jun 2021 15:54:31 +0200 Subject: [PATCH 17/59] Refactoring Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/waitset_composition.cpp | 59 ++++---- .../waitset_different_rates_topics.cpp | 126 ++++++++++++++++++ rclcpp/wait_set/waitset_example_main.cpp | 17 ++- rclcpp/wait_set/waitset_example_main_loop.cpp | 13 +- 4 files changed, 175 insertions(+), 40 deletions(-) create mode 100644 rclcpp/wait_set/waitset_different_rates_topics.cpp diff --git a/rclcpp/wait_set/waitset_composition.cpp b/rclcpp/wait_set/waitset_composition.cpp index 2bec6c7a..6d9d4b77 100644 --- a/rclcpp/wait_set/waitset_composition.cpp +++ b/rclcpp/wait_set/waitset_composition.cpp @@ -27,16 +27,16 @@ class Talker : public rclcpp::Node { public: Talker() - : Node("talker"), count_(0) + : Node("talker"), count_(0) { publisher_ = this->create_publisher("topic", 10); auto timer_callback = - [this]() -> void { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(this->count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - this->publisher_->publish(message); - }; + [this]() -> void { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(this->count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + this->publisher_->publish(message); + }; timer_ = this->create_wall_timer(500ms, timer_callback); } @@ -50,30 +50,31 @@ class Listener : public rclcpp::Node { public: Listener() - : Node("listener") + : Node("listener") { subscription_executor_ = this->create_subscription( - "topic", - 10, - [this](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s' (executor)", msg->data.c_str()); - }); + "topic", + 10, + [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s' (executor)", msg->data.c_str()); + }); // creates a subscription which is not automatically added to an executor rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::CallbackGroupType::MutuallyExclusive, false); auto options = rclcpp::SubscriptionOptions(); options.callback_group = cb_group_waitset; auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; subscription_waitset_ = this->create_subscription( - "topic", - 10, - do_nothing, - options); + "topic", + 10, + do_nothing, + options); } - rclcpp::Subscription::SharedPtr get_subscription() const { + rclcpp::Subscription::SharedPtr get_subscription() const + { return subscription_waitset_; } @@ -87,7 +88,7 @@ int main(int argc, char * argv[]) rclcpp::init(argc, argv); // Create an executor that will be responsible for execution of callbacks for a set of nodes. - // All callbacks will be called except the subscription which as explicitly excluded from it + // All callbacks will be called except the subscription which was explicitly excluded from it rclcpp::executors::SingleThreadedExecutor exec; auto talker = std::make_shared(); @@ -95,14 +96,16 @@ int main(int argc, char * argv[]) auto listener = std::make_shared(); exec.add_node(listener); - rclcpp::WaitSet wait_set; - wait_set.add_subscription(listener->get_subscription()); + // Create a static wait-set that will wait on the subscription that will excluded from the + // default executor callback group + rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{listener->get_subscription()}}}); - auto thread = std::thread([&exec]() { - // spin will block until work comes in, execute work as it becomes available, and keep blocking. - // It will only be interrupted by Ctrl-C. - exec.spin(); - }); + auto thread = std::thread( + [&exec]() { + // spin will block until work comes in, execute work as it becomes available, and keep blocking. + // It will only be interrupted by Ctrl-C. + exec.spin(); + }); while (rclcpp::ok()) { // Waiting up to 5s for a message to arrive @@ -126,4 +129,4 @@ int main(int argc, char * argv[]) rclcpp::shutdown(); thread.join(); return 0; -} \ No newline at end of file +} diff --git a/rclcpp/wait_set/waitset_different_rates_topics.cpp b/rclcpp/wait_set/waitset_different_rates_topics.cpp new file mode 100644 index 00000000..47f179d5 --- /dev/null +++ b/rclcpp/wait_set/waitset_different_rates_topics.cpp @@ -0,0 +1,126 @@ +// 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 + +/* For this example, we will be creating a publishing 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 directly by the user in the code. That is, in this example + * we always take and process the data in the same order A, B, C regardless of the arrival order. + */ +class Talker : public rclcpp::Node +{ +public: + Talker() + : Node("talker"), + pub1_(this->create_publisher("topicA", 10)), + pub2_(this->create_publisher("topicB", 10)), + pub3_(this->create_publisher("topicC", 10)), + count_(0U) + { + } + + void run() + { + std_msgs::msg::String msg1, msg2, msg3; + msg1.data = "A"; + msg2.data = "B"; + msg3.data = "C"; + + while (rclcpp::ok()) { + RCLCPP_INFO(this->get_logger(), "Publishing msg1: %s", msg1.data.c_str()); + pub1_->publish(msg1); + if ( (count_ % 4) == 0) { + RCLCPP_INFO(this->get_logger(), "Publishing msg3: %s", msg3.data.c_str()); + pub3_->publish(msg3); + } + + if ( (count_ % 2) == 0) { + RCLCPP_INFO(this->get_logger(), "Publishing msg2: %s", msg2.data.c_str()); + pub2_->publish(msg2); + } + ++count_; + std::this_thread::sleep_for(std::chrono::milliseconds(2500)); + } + } + +private: + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; + rclcpp::Publisher::SharedPtr pub3_; + size_t count_; +}; + +int32_t main(const int32_t argc, char ** const argv) +{ + rclcpp::init(argc, argv); + + auto node = std::make_shared("waitset_node"); + auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; + size_t count{0U}; + + 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}}); + + // Creates a random publisher and starts publishing in another thread + Talker publisher_node; + auto publisher_thread = std::thread([&publisher_node]() {publisher_node.run();}); + + while (rclcpp::ok()) { + // Waiting up to 5s for a message to arrive + const auto wait_result = wait_set.wait(std::chrono::seconds(5)); + + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + bool sub2_has_data = wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1U]; + if (sub2_has_data) { + std_msgs::msg::String msg1; + std_msgs::msg::String msg2; + std_msgs::msg::String msg3; + rclcpp::MessageInfo msg_info; + + // the receiving order is not relevant, the messages are taken in a user-defined order + bool msg1_is_valid = sub1->take(msg1, msg_info); + bool msg2_is_valid = sub2->take(msg2, msg_info); + bool msg3_is_valid = sub3->take(msg3, msg_info); + + // we process all the messages only if all are valid + if (msg1_is_valid && msg2_is_valid) { + RCLCPP_INFO( + node->get_logger(), "Handle: %s%s", msg1.data.c_str(), + msg2.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "An invalid message was received."); + } + if (msg3_is_valid) { + RCLCPP_INFO(node->get_logger(), "Handle: %s", msg3.data.c_str()); + } + + ++count; + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + } else { + RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + } + } + + rclcpp::shutdown(); + publisher_thread.join(); + return 0; +} diff --git a/rclcpp/wait_set/waitset_example_main.cpp b/rclcpp/wait_set/waitset_example_main.cpp index afad9c2c..7cea2b66 100644 --- a/rclcpp/wait_set/waitset_example_main.cpp +++ b/rclcpp/wait_set/waitset_example_main.cpp @@ -28,9 +28,9 @@ int32_t main(const int32_t argc, char ** const 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("a11", 1, do_nothing); - auto sub2 = node->create_subscription("a22", 1, do_nothing); - auto sub3 = node->create_subscription("a33", 1, do_nothing); + 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!"; @@ -38,11 +38,11 @@ int32_t main(const int32_t argc, char ** const argv) msg3.data = "Hello, world!"; const auto pub1 = - node->create_publisher("a11", 1); + node->create_publisher("topicA", 1); const auto pub2 = - node->create_publisher("a22", 1); + node->create_publisher("topicB", 1); const auto pub3 = - node->create_publisher("a33", 1); + 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 @@ -61,11 +61,16 @@ int32_t main(const int32_t argc, char ** const argv) one_off_timer = node->create_wall_timer(1s, timer_callback); + /* + // Option 1: add entities after construction rclcpp::WaitSet wait_set; wait_set.add_subscription(sub1); wait_set.add_subscription(sub2); wait_set.add_subscription(sub3); wait_set.add_timer(one_off_timer); + */ + // Option 2: add entities in the constructor + rclcpp::WaitSet wait_set({{{sub1}, {sub2}, {sub3}}}, {}, {one_off_timer}); auto num_recv = std::size_t(); while (num_recv < 3U) { diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/waitset_example_main_loop.cpp index c1ae65b0..d3ea37d5 100644 --- a/rclcpp/wait_set/waitset_example_main_loop.cpp +++ b/rclcpp/wait_set/waitset_example_main_loop.cpp @@ -84,10 +84,7 @@ int32_t main(const int32_t argc, char ** const argv) auto sub2 = node->create_subscription("topicB", 10, do_nothing); auto sub3 = node->create_subscription("topicC", 10, do_nothing); - rclcpp::WaitSet wait_set; - wait_set.add_subscription(sub1); - wait_set.add_subscription(sub2); - wait_set.add_subscription(sub3); + rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); // Creates a random publisher and starts publishing in another thread RandomPublisher publisher_node; @@ -98,10 +95,13 @@ int32_t main(const int32_t argc, char ** const argv) const auto wait_result = wait_set.wait(std::chrono::seconds(5)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - // We will handle all the messages when all subscriptions have received data 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]; + + // We will handle all the messages when all subscriptions have received data. + // Note we could use just one subscription or a combination of conditions to trigger data + // handling. bool handle_condition = sub1_has_data && sub2_has_data && sub3_has_data; if (handle_condition) { @@ -115,12 +115,13 @@ int32_t main(const int32_t argc, char ** const argv) bool msg2_is_valid = sub2->take(msg2, msg_info); bool msg3_is_valid = sub3->take(msg3, msg_info); + // we process all the messages only if all are valid if (msg1_is_valid && msg2_is_valid && msg3_is_valid) { RCLCPP_INFO( node->get_logger(), "%zu %s%s%s", count, msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "Invalid message received."); + RCLCPP_INFO(node->get_logger(), "An invalid message was received."); } ++count; } From db804b70d11b27fe6d5540b675090086cf93d703 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 29 Jun 2021 15:54:51 +0200 Subject: [PATCH 18/59] Add example using different topic rates Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 4 + .../waitset_different_rates_topics.cpp | 119 +++++++++--------- 2 files changed, 64 insertions(+), 59 deletions(-) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 04b06311..da70c08f 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -30,6 +30,9 @@ ament_target_dependencies(waitset_example_main rclcpp std_msgs) add_executable(waitset_example_main_loop waitset_example_main_loop.cpp) ament_target_dependencies(waitset_example_main_loop rclcpp std_msgs) +add_executable(waitset_different_rates_topics waitset_different_rates_topics.cpp) +ament_target_dependencies(waitset_different_rates_topics rclcpp std_msgs) + add_executable(waitset_composition waitset_composition.cpp) ament_target_dependencies(waitset_composition rclcpp std_msgs) @@ -39,6 +42,7 @@ install(TARGETS thread_safe_wait_set waitset_example_main waitset_example_main_loop + waitset_different_rates_topics waitset_composition DESTINATION lib/${PROJECT_NAME} ) diff --git a/rclcpp/wait_set/waitset_different_rates_topics.cpp b/rclcpp/wait_set/waitset_different_rates_topics.cpp index 47f179d5..2be44d3e 100644 --- a/rclcpp/wait_set/waitset_different_rates_topics.cpp +++ b/rclcpp/wait_set/waitset_different_rates_topics.cpp @@ -16,52 +16,37 @@ #include #include -/* For this example, we will be creating a publishing 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 directly by the user in the code. That is, in this example - * we always take and process the data in the same order A, B, C regardless of the arrival order. +using namespace std::chrono_literals; + +/* For this example, we will be creating three talkers publishing the topics A, B, C at a different + * rate. The messages are handled by a wait-set loop which handles topics A and B on a topic B + * message arrival and topic C separately. */ + class Talker : public rclcpp::Node { public: - Talker() - : Node("talker"), - pub1_(this->create_publisher("topicA", 10)), - pub2_(this->create_publisher("topicB", 10)), - pub3_(this->create_publisher("topicC", 10)), - count_(0U) - { - } - - void run() + Talker( + const std::string & node_name, + const std::string & topic_name, + const std::string & message_data, + std::chrono::nanoseconds period) + : Node(node_name) { - std_msgs::msg::String msg1, msg2, msg3; - msg1.data = "A"; - msg2.data = "B"; - msg3.data = "C"; - - while (rclcpp::ok()) { - RCLCPP_INFO(this->get_logger(), "Publishing msg1: %s", msg1.data.c_str()); - pub1_->publish(msg1); - if ( (count_ % 4) == 0) { - RCLCPP_INFO(this->get_logger(), "Publishing msg3: %s", msg3.data.c_str()); - pub3_->publish(msg3); - } - - if ( (count_ % 2) == 0) { - RCLCPP_INFO(this->get_logger(), "Publishing msg2: %s", msg2.data.c_str()); - pub2_->publish(msg2); - } - ++count_; - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } + publisher_ = this->create_publisher(topic_name, 10); + auto timer_callback = + [this, message_data]() -> void { + auto message = std_msgs::msg::String(); + 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::Publisher::SharedPtr pub1_; - rclcpp::Publisher::SharedPtr pub2_; - rclcpp::Publisher::SharedPtr pub3_; - size_t count_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; }; int32_t main(const int32_t argc, char ** const argv) @@ -70,7 +55,6 @@ int32_t main(const int32_t argc, char ** const argv) auto node = std::make_shared("waitset_node"); auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; - size_t count{0U}; auto sub1 = node->create_subscription("topicA", 10, do_nothing); auto sub2 = node->create_subscription("topicB", 10, do_nothing); @@ -78,9 +62,17 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); - // Creates a random publisher and starts publishing in another thread - Talker publisher_node; - auto publisher_thread = std::thread([&publisher_node]() {publisher_node.run();}); + // Creates 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()) { // Waiting up to 5s for a message to arrive @@ -88,30 +80,39 @@ int32_t main(const int32_t argc, char ** const argv) 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]; + + // topics A and B and handled together, but only on a topic B message arrival if (sub2_has_data) { std_msgs::msg::String msg1; std_msgs::msg::String msg2; - std_msgs::msg::String msg3; rclcpp::MessageInfo msg_info; - - // the receiving order is not relevant, the messages are taken in a user-defined order - bool msg1_is_valid = sub1->take(msg1, msg_info); - bool msg2_is_valid = sub2->take(msg2, msg_info); - bool msg3_is_valid = sub3->take(msg3, msg_info); - - // we process all the messages only if all are valid - if (msg1_is_valid && msg2_is_valid) { - RCLCPP_INFO( - node->get_logger(), "Handle: %s%s", msg1.data.c_str(), - msg2.data.c_str()); + std::string handled_data; + + // this simulates a case where topics A and B must be handled together + // since topic A is published at a faster rate we expect to have multiple messages in queue + // depending on the application we may want to take the latest one, take all, etc + if (sub2->take(msg2, msg_info)) { + // take all the messages received from topic A + while (sub1->take(msg1, msg_info)) { + handled_data.append(msg1.data); + } + handled_data.append(msg2.data); + RCLCPP_INFO(node->get_logger(), "Handle topic A and B: %s", handled_data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "An invalid message was received."); - } - if (msg3_is_valid) { - RCLCPP_INFO(node->get_logger(), "Handle: %s", msg3.data.c_str()); + RCLCPP_INFO(node->get_logger(), "An invalid message from topic B was received."); } + } - ++count; + // topics C is handled independently + if (sub3_has_data) { + std_msgs::msg::String msg; + rclcpp::MessageInfo msg_info; + if (sub3->take(msg, msg_info)) { + RCLCPP_INFO(node->get_logger(), "Handle topic C: %s", msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "An invalid message from topic C was received."); + } } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { RCLCPP_INFO(node->get_logger(), "No message received after 5s."); From 54de281fdae346e4d635b24461700a0841af457e Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 29 Jun 2021 16:36:07 +0200 Subject: [PATCH 19/59] Rename examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 24 +++++++++---------- ...mposition.cpp => wait_set_composition.cpp} | 4 ++-- ...pp => wait_set_different_rates_topics.cpp} | 8 +++---- ..._example_main.cpp => wait_set_example.cpp} | 4 ++-- ...ain_loop.cpp => wait_set_random_order.cpp} | 6 ++--- 5 files changed, 23 insertions(+), 23 deletions(-) rename rclcpp/wait_set/{waitset_composition.cpp => wait_set_composition.cpp} (96%) rename rclcpp/wait_set/{waitset_different_rates_topics.cpp => wait_set_different_rates_topics.cpp} (93%) rename rclcpp/wait_set/{waitset_example_main.cpp => wait_set_example.cpp} (97%) rename rclcpp/wait_set/{waitset_example_main_loop.cpp => wait_set_random_order.cpp} (96%) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index da70c08f..b62352c9 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -24,26 +24,26 @@ 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) -add_executable(waitset_example_main waitset_example_main.cpp) -ament_target_dependencies(waitset_example_main rclcpp std_msgs) +add_executable(wait_set_example wait_set_example.cpp) +ament_target_dependencies(wait_set_example rclcpp std_msgs) -add_executable(waitset_example_main_loop waitset_example_main_loop.cpp) -ament_target_dependencies(waitset_example_main_loop rclcpp std_msgs) +add_executable(wait_set_random_order wait_set_random_order.cpp) +ament_target_dependencies(wait_set_random_order rclcpp std_msgs) -add_executable(waitset_different_rates_topics waitset_different_rates_topics.cpp) -ament_target_dependencies(waitset_different_rates_topics rclcpp std_msgs) +add_executable(wait_set_different_rates_topics wait_set_different_rates_topics.cpp) +ament_target_dependencies(wait_set_different_rates_topics rclcpp std_msgs) -add_executable(waitset_composition waitset_composition.cpp) -ament_target_dependencies(waitset_composition rclcpp std_msgs) +add_executable(wait_set_composition wait_set_composition.cpp) +ament_target_dependencies(wait_set_composition rclcpp std_msgs) install(TARGETS wait_set static_wait_set thread_safe_wait_set - waitset_example_main - waitset_example_main_loop - waitset_different_rates_topics - waitset_composition + wait_set_example + wait_set_random_order + wait_set_different_rates_topics + wait_set_composition DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/rclcpp/wait_set/waitset_composition.cpp b/rclcpp/wait_set/wait_set_composition.cpp similarity index 96% rename from rclcpp/wait_set/waitset_composition.cpp rename to rclcpp/wait_set/wait_set_composition.cpp index 6d9d4b77..54a01dc2 100644 --- a/rclcpp/wait_set/waitset_composition.cpp +++ b/rclcpp/wait_set/wait_set_composition.cpp @@ -120,9 +120,9 @@ int main(int argc, char * argv[]) } } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_INFO(listener->get_logger(), "No message received after 5s."); + RCLCPP_ERROR(listener->get_logger(), "No message received after 5s."); } else { - RCLCPP_INFO(listener->get_logger(), "Wait-set failed."); + RCLCPP_ERROR(listener->get_logger(), "Wait-set failed."); } } diff --git a/rclcpp/wait_set/waitset_different_rates_topics.cpp b/rclcpp/wait_set/wait_set_different_rates_topics.cpp similarity index 93% rename from rclcpp/wait_set/waitset_different_rates_topics.cpp rename to rclcpp/wait_set/wait_set_different_rates_topics.cpp index 2be44d3e..066cac81 100644 --- a/rclcpp/wait_set/waitset_different_rates_topics.cpp +++ b/rclcpp/wait_set/wait_set_different_rates_topics.cpp @@ -100,7 +100,7 @@ int32_t main(const int32_t argc, char ** const argv) handled_data.append(msg2.data); RCLCPP_INFO(node->get_logger(), "Handle topic A and B: %s", handled_data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "An invalid message from topic B was received."); + RCLCPP_ERROR(node->get_logger(), "An invalid message from topic B was received."); } } @@ -111,13 +111,13 @@ int32_t main(const int32_t argc, char ** const argv) if (sub3->take(msg, msg_info)) { RCLCPP_INFO(node->get_logger(), "Handle topic C: %s", msg.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "An invalid message from topic C was received."); + RCLCPP_ERROR(node->get_logger(), "An invalid message from topic C was received."); } } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); } else { - RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); } } diff --git a/rclcpp/wait_set/waitset_example_main.cpp b/rclcpp/wait_set/wait_set_example.cpp similarity index 97% rename from rclcpp/wait_set/waitset_example_main.cpp rename to rclcpp/wait_set/wait_set_example.cpp index 7cea2b66..2091effb 100644 --- a/rclcpp/wait_set/waitset_example_main.cpp +++ b/rclcpp/wait_set/wait_set_example.cpp @@ -111,9 +111,9 @@ int32_t main(const int32_t argc, char ** const argv) RCLCPP_INFO(node->get_logger(), "Number of messages already got: %zu of 3", num_recv); } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); } else { - RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); } } RCLCPP_INFO(node->get_logger(), "Got all messages!"); diff --git a/rclcpp/wait_set/waitset_example_main_loop.cpp b/rclcpp/wait_set/wait_set_random_order.cpp similarity index 96% rename from rclcpp/wait_set/waitset_example_main_loop.cpp rename to rclcpp/wait_set/wait_set_random_order.cpp index d3ea37d5..92c2ef7a 100644 --- a/rclcpp/wait_set/waitset_example_main_loop.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -121,14 +121,14 @@ int32_t main(const int32_t argc, char ** const argv) node->get_logger(), "%zu %s%s%s", count, msg1.data.c_str(), msg2.data.c_str(), msg3.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "An invalid message was received."); + RCLCPP_ERROR(node->get_logger(), "An invalid message was received."); } ++count; } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_INFO(node->get_logger(), "No message received after 5s."); + RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); } else { - RCLCPP_INFO(node->get_logger(), "Wait-set failed."); + RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); } } From 27dc9de1a5bda75c292d71de94aa8964a7030325 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 29 Jun 2021 19:12:16 +0200 Subject: [PATCH 20/59] Use handle_message to handle callbacks Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set_random_order.cpp | 43 +++++++++++------------ 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/rclcpp/wait_set/wait_set_random_order.cpp b/rclcpp/wait_set/wait_set_random_order.cpp index 92c2ef7a..30fd8c61 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -52,8 +52,7 @@ class RandomPublisher : public rclcpp::Node while (rclcpp::ok()) { std::shuffle(publish_order.begin(), publish_order.end(), e); RCLCPP_INFO( - this->get_logger(), "%zu %s%s%s", - count_, + this->get_logger(), "Publishing: %s%s%s", msgs_data[publish_order.at(0)].c_str(), msgs_data[publish_order.at(1)].c_str(), msgs_data[publish_order.at(2)].c_str()); @@ -77,12 +76,13 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::init(argc, argv); auto node = std::make_shared("waitset_node"); - auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; - size_t count{0U}; + auto print_msg = [&node](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(node->get_logger(), "I heard: %s", msg->data.c_str()); + }; - 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); + auto sub1 = node->create_subscription("topicA", 10, print_msg); + auto sub2 = node->create_subscription("topicB", 10, print_msg); + auto sub3 = node->create_subscription("topicC", 10, print_msg); rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); @@ -105,25 +105,22 @@ int32_t main(const int32_t argc, char ** const argv) bool handle_condition = sub1_has_data && sub2_has_data && sub3_has_data; if (handle_condition) { - std_msgs::msg::String msg1; - std_msgs::msg::String msg2; - std_msgs::msg::String msg3; + std_msgs::msg::String msg1, msg2, msg3; rclcpp::MessageInfo msg_info; - // the receiving order is not relevant, the messages are taken in a user-defined order - bool msg1_is_valid = sub1->take(msg1, msg_info); - bool msg2_is_valid = sub2->take(msg2, msg_info); - bool msg3_is_valid = sub3->take(msg3, msg_info); - - // we process all the messages only if all are valid - if (msg1_is_valid && msg2_is_valid && msg3_is_valid) { - RCLCPP_INFO( - node->get_logger(), "%zu %s%s%s", count, msg1.data.c_str(), - msg2.data.c_str(), msg3.data.c_str()); - } else { - RCLCPP_ERROR(node->get_logger(), "An invalid message was received."); + // the messages are taken and handled in a user-defined order + if (sub1->take(msg1, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg1); + sub1->handle_message(type_erased_msg, msg_info); + } + if (sub2->take(msg2, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg2); + sub2->handle_message(type_erased_msg, msg_info); + } + if (sub3->take(msg3, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg3); + sub3->handle_message(type_erased_msg, msg_info); } - ++count; } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); From 65cf4fc16bba02fa46d358a78212def84be87a19 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 29 Jun 2021 23:09:00 +0200 Subject: [PATCH 21/59] Refactor random talker example Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 4 + rclcpp/wait_set/executor_random_order.cpp | 44 +++++++ rclcpp/wait_set/random_listener.hpp | 44 +++++++ rclcpp/wait_set/random_talker.hpp | 67 ++++++++++ rclcpp/wait_set/wait_set_composition.cpp | 8 +- .../wait_set_different_rates_topics.cpp | 16 +-- rclcpp/wait_set/wait_set_example.cpp | 12 +- rclcpp/wait_set/wait_set_random_order.cpp | 122 +++++------------- 8 files changed, 209 insertions(+), 108 deletions(-) create mode 100644 rclcpp/wait_set/executor_random_order.cpp create mode 100644 rclcpp/wait_set/random_listener.hpp create mode 100644 rclcpp/wait_set/random_talker.hpp diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index b62352c9..9dab43ca 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -30,6 +30,9 @@ ament_target_dependencies(wait_set_example rclcpp std_msgs) add_executable(wait_set_random_order wait_set_random_order.cpp) ament_target_dependencies(wait_set_random_order rclcpp std_msgs) +add_executable(executor_random_order executor_random_order.cpp) +ament_target_dependencies(executor_random_order rclcpp std_msgs) + add_executable(wait_set_different_rates_topics wait_set_different_rates_topics.cpp) ament_target_dependencies(wait_set_different_rates_topics rclcpp std_msgs) @@ -42,6 +45,7 @@ install(TARGETS thread_safe_wait_set wait_set_example wait_set_random_order + executor_random_order wait_set_different_rates_topics wait_set_composition DESTINATION lib/${PROJECT_NAME} diff --git a/rclcpp/wait_set/executor_random_order.cpp b/rclcpp/wait_set/executor_random_order.cpp new file mode 100644 index 00000000..9785d0bb --- /dev/null +++ b/rclcpp/wait_set/executor_random_order.cpp @@ -0,0 +1,44 @@ +// 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 "random_listener.hpp" +#include "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); + + // We run the talker and the listener in different threads to simulate inter-process + // communications. + // 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 difference 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/random_listener.hpp b/rclcpp/wait_set/random_listener.hpp new file mode 100644 index 00000000..c8db29e9 --- /dev/null +++ b/rclcpp/wait_set/random_listener.hpp @@ -0,0 +1,44 @@ +// 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 + +class Listener : public rclcpp::Node +{ + using subscription_list = std::vector::SharedPtr>; + +public: + Listener() + : 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_; +}; diff --git a/rclcpp/wait_set/random_talker.hpp b/rclcpp/wait_set/random_talker.hpp new file mode 100644 index 00000000..8e93b987 --- /dev/null +++ b/rclcpp/wait_set/random_talker.hpp @@ -0,0 +1,67 @@ +// 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 + +using namespace std::chrono_literals; + +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_(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(1s, 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_; +}; diff --git a/rclcpp/wait_set/wait_set_composition.cpp b/rclcpp/wait_set/wait_set_composition.cpp index 54a01dc2..1310113e 100644 --- a/rclcpp/wait_set/wait_set_composition.cpp +++ b/rclcpp/wait_set/wait_set_composition.cpp @@ -116,13 +116,13 @@ int main(int argc, char * argv[]) std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; if (listener->get_subscription()->take(msg, msg_info)) { - RCLCPP_INFO(listener->get_logger(), "I heard: '%s' (waitset)", msg.data.c_str()); + RCLCPP_INFO(listener->get_logger(), "I heard: '%s' (wait-set)", msg.data.c_str()); } } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_ERROR(listener->get_logger(), "No message received after 5s."); - } else { - RCLCPP_ERROR(listener->get_logger(), "Wait-set failed."); + if (rclcpp::ok()) { + RCLCPP_ERROR(listener->get_logger(), "Wait-set failed with timeout"); + } } } diff --git a/rclcpp/wait_set/wait_set_different_rates_topics.cpp b/rclcpp/wait_set/wait_set_different_rates_topics.cpp index 066cac81..513bcc5e 100644 --- a/rclcpp/wait_set/wait_set_different_rates_topics.cpp +++ b/rclcpp/wait_set/wait_set_different_rates_topics.cpp @@ -53,7 +53,7 @@ int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("waitset_node"); + 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); @@ -75,9 +75,7 @@ int32_t main(const int32_t argc, char ** const argv) auto publisher_thread = std::thread([&exec]() {exec.spin();}); while (rclcpp::ok()) { - // Waiting up to 5s for a message to arrive - const auto wait_result = wait_set.wait(std::chrono::seconds(5)); - + 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]; @@ -98,7 +96,7 @@ int32_t main(const int32_t argc, char ** const argv) handled_data.append(msg1.data); } handled_data.append(msg2.data); - RCLCPP_INFO(node->get_logger(), "Handle topic A and B: %s", handled_data.c_str()); + 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."); } @@ -109,15 +107,15 @@ int32_t main(const int32_t argc, char ** const argv) std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; if (sub3->take(msg, msg_info)) { - RCLCPP_INFO(node->get_logger(), "Handle topic C: %s", msg.data.c_str()); + 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) { - RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); - } else { - RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); + if (rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Wait-set failed with timeout"); + } } } diff --git a/rclcpp/wait_set/wait_set_example.cpp b/rclcpp/wait_set/wait_set_example.cpp index 2091effb..ba10d094 100644 --- a/rclcpp/wait_set/wait_set_example.cpp +++ b/rclcpp/wait_set/wait_set_example.cpp @@ -25,7 +25,7 @@ int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("wait_set_example_node"); + 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); @@ -74,9 +74,7 @@ int32_t main(const int32_t argc, char ** const argv) auto num_recv = std::size_t(); while (num_recv < 3U) { - // Waiting up to 5s for a sample to arrive. - const auto wait_result = wait_set.wait(std::chrono::seconds(5)); - + 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]) { // we execute manually the timer callback @@ -111,9 +109,9 @@ int32_t main(const int32_t argc, char ** const argv) RCLCPP_INFO(node->get_logger(), "Number of messages already got: %zu of 3", num_recv); } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); - } else { - RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); + if (rclcpp::ok()) { + RCLCPP_ERROR(node->get_logger(), "Wait-set failed with timeout"); + } } } RCLCPP_INFO(node->get_logger(), "Got all messages!"); diff --git a/rclcpp/wait_set/wait_set_random_order.cpp b/rclcpp/wait_set/wait_set_random_order.cpp index 30fd8c61..f9772ff6 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -14,122 +14,68 @@ #include #include -#include +#include "random_listener.hpp" +#include "random_talker.hpp" -/* For this example, we will be creating a publishing node with three publishers which will +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 directly by the user in the code. That is, in this example - * we always take and process the data in the same order A, B, C regardless of the arrival order. + * handled is defined deterministically directly 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. */ -class RandomPublisher : public rclcpp::Node -{ -public: - RandomPublisher() - : Node("random_publisher"), - pub1_(this->create_publisher("topicA", 10)), - pub2_(this->create_publisher("topicB", 10)), - pub3_(this->create_publisher("topicC", 10)), - count_(0U) - { - } - - void run() - { - std_msgs::msg::String msg1, msg2, msg3; - std::vector> publisher_functions; - std::vector msgs_data{"A", "B", "C"}; - std::vector publish_order{0, 1, 2}; - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::default_random_engine e(seed); - - msg1.data = msgs_data.at(0U); - msg2.data = msgs_data.at(1U); - msg3.data = msgs_data.at(2U); - publisher_functions.emplace_back(([this, msg1]() {pub1_->publish(msg1);})); - publisher_functions.emplace_back(([this, msg2]() {pub2_->publish(msg2);})); - publisher_functions.emplace_back(([this, msg3]() {pub3_->publish(msg3);})); - - while (rclcpp::ok()) { - std::shuffle(publish_order.begin(), publish_order.end(), e); - RCLCPP_INFO( - this->get_logger(), "Publishing: %s%s%s", - msgs_data[publish_order.at(0)].c_str(), - msgs_data[publish_order.at(1)].c_str(), - msgs_data[publish_order.at(2)].c_str()); - publisher_functions.at(publish_order.at(0))(); - publisher_functions.at(publish_order.at(1))(); - publisher_functions.at(publish_order.at(2))(); - ++count_; - std::this_thread::sleep_for(std::chrono::milliseconds(2500)); - } - } - -private: - rclcpp::Publisher::SharedPtr pub1_; - rclcpp::Publisher::SharedPtr pub2_; - rclcpp::Publisher::SharedPtr pub3_; - size_t count_; -}; int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("waitset_node"); - auto print_msg = [&node](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(node->get_logger(), "I heard: %s", msg->data.c_str()); - }; + auto listener = std::make_shared(); + auto subscriptions = listener->get_subscriptions(); - auto sub1 = node->create_subscription("topicA", 10, print_msg); - auto sub2 = node->create_subscription("topicB", 10, print_msg); - auto sub3 = node->create_subscription("topicC", 10, print_msg); - - rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); + // Create a wait-set and add the subscriptions + rclcpp::WaitSet wait_set; + for (const auto & subscription : subscriptions) { + wait_set.add_subscription(subscription); + } - // Creates a random publisher and starts publishing in another thread - RandomPublisher publisher_node; - auto publisher_thread = std::thread([&publisher_node]() {publisher_node.run();}); + // Create a random talker and start publishing in another thread + auto thread = std::thread([]() {rclcpp::spin(std::make_shared());}); while (rclcpp::ok()) { - // Waiting up to 5s for a message to arrive - const auto wait_result = wait_set.wait(std::chrono::seconds(5)); - + 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]; - // We will handle all the messages when all subscriptions have received data. - // Note we could use just one subscription or a combination of conditions to trigger data - // handling. - bool handle_condition = sub1_has_data && sub2_has_data && sub3_has_data; - - if (handle_condition) { - std_msgs::msg::String msg1, msg2, msg3; + // 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; // the messages are taken and handled in a user-defined order - if (sub1->take(msg1, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg1); - sub1->handle_message(type_erased_msg, msg_info); + if (subscriptions.at(0)->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscriptions.at(0)->handle_message(type_erased_msg, msg_info); } - if (sub2->take(msg2, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg2); - sub2->handle_message(type_erased_msg, msg_info); + if (subscriptions.at(1)->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscriptions.at(1)->handle_message(type_erased_msg, msg_info); } - if (sub3->take(msg3, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg3); - sub3->handle_message(type_erased_msg, msg_info); + if (subscriptions.at(2)->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscriptions.at(2)->handle_message(type_erased_msg, msg_info); } } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - RCLCPP_ERROR(node->get_logger(), "No message received after 5s."); - } else { - RCLCPP_ERROR(node->get_logger(), "Wait-set failed."); + if (rclcpp::ok()) { + RCLCPP_ERROR(listener->get_logger(), "Wait-set failed with timeout"); + } } } rclcpp::shutdown(); - publisher_thread.join(); + thread.join(); return 0; } From ce06accde23b0cf25b37e4cb3e0a32a3a6edcb6c Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 30 Jun 2021 17:07:27 +0200 Subject: [PATCH 22/59] Fix cpplint errors Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/executor_random_order.cpp | 7 +++++-- rclcpp/wait_set/random_listener.hpp | 10 ++++++++-- rclcpp/wait_set/random_talker.hpp | 12 ++++++++---- rclcpp/wait_set/thread_safe_wait_set.cpp | 1 + rclcpp/wait_set/wait_set.cpp | 1 + rclcpp/wait_set/wait_set_composition.cpp | 7 +------ rclcpp/wait_set/wait_set_different_rates_topics.cpp | 11 ++++++++--- rclcpp/wait_set/wait_set_example.cpp | 6 ++++-- rclcpp/wait_set/wait_set_random_order.cpp | 7 +++++-- 9 files changed, 41 insertions(+), 21 deletions(-) diff --git a/rclcpp/wait_set/executor_random_order.cpp b/rclcpp/wait_set/executor_random_order.cpp index 9785d0bb..606db758 100644 --- a/rclcpp/wait_set/executor_random_order.cpp +++ b/rclcpp/wait_set/executor_random_order.cpp @@ -12,8 +12,11 @@ // 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" + #include "random_listener.hpp" #include "random_talker.hpp" diff --git a/rclcpp/wait_set/random_listener.hpp b/rclcpp/wait_set/random_listener.hpp index c8db29e9..e704627a 100644 --- a/rclcpp/wait_set/random_listener.hpp +++ b/rclcpp/wait_set/random_listener.hpp @@ -12,10 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#ifndef RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ +#define RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ + +#include #include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + class Listener : public rclcpp::Node { using subscription_list = std::vector::SharedPtr>; @@ -42,3 +47,4 @@ class Listener : public rclcpp::Node rclcpp::Subscription::SharedPtr sub2_; rclcpp::Subscription::SharedPtr sub3_; }; +#endif // RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ diff --git a/rclcpp/wait_set/random_talker.hpp b/rclcpp/wait_set/random_talker.hpp index 8e93b987..08dbb79f 100644 --- a/rclcpp/wait_set/random_talker.hpp +++ b/rclcpp/wait_set/random_talker.hpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#ifndef RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ +#define RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ + +#include #include -using namespace std::chrono_literals; +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" class RandomTalker : public rclcpp::Node { @@ -54,7 +57,7 @@ class RandomTalker : public rclcpp::Node std::shuffle(publish_functions_.begin(), publish_functions_.end(), rand_engine_); for (const auto & f : publish_functions_) {f();} }; - timer_ = this->create_wall_timer(1s, timer_callback); + timer_ = this->create_wall_timer(std::chrono::seconds(1), timer_callback); } private: @@ -65,3 +68,4 @@ class RandomTalker : public rclcpp::Node std::vector> publish_functions_; std::default_random_engine rand_engine_; }; +#endif // RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ diff --git a/rclcpp/wait_set/thread_safe_wait_set.cpp b/rclcpp/wait_set/thread_safe_wait_set.cpp index 0d2b494b..963e536e 100644 --- a/rclcpp/wait_set/thread_safe_wait_set.cpp +++ b/rclcpp/wait_set/thread_safe_wait_set.cpp @@ -14,6 +14,7 @@ #include #include +#include // #include // #include diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 68eb0c0e..b7fe8b5d 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -14,6 +14,7 @@ #include #include +#include // #include // #include diff --git a/rclcpp/wait_set/wait_set_composition.cpp b/rclcpp/wait_set/wait_set_composition.cpp index 1310113e..d2ab6e04 100644 --- a/rclcpp/wait_set/wait_set_composition.cpp +++ b/rclcpp/wait_set/wait_set_composition.cpp @@ -100,12 +100,7 @@ int main(int argc, char * argv[]) // default executor callback group rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{listener->get_subscription()}}}); - auto thread = std::thread( - [&exec]() { - // spin will block until work comes in, execute work as it becomes available, and keep blocking. - // It will only be interrupted by Ctrl-C. - exec.spin(); - }); + auto thread = std::thread([&exec]() {exec.spin();}); while (rclcpp::ok()) { // Waiting up to 5s for a message to arrive diff --git a/rclcpp/wait_set/wait_set_different_rates_topics.cpp b/rclcpp/wait_set/wait_set_different_rates_topics.cpp index 513bcc5e..cfe1ddf7 100644 --- a/rclcpp/wait_set/wait_set_different_rates_topics.cpp +++ b/rclcpp/wait_set/wait_set_different_rates_topics.cpp @@ -12,9 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" + +#include "random_listener.hpp" +#include "random_talker.hpp" using namespace std::chrono_literals; diff --git a/rclcpp/wait_set/wait_set_example.cpp b/rclcpp/wait_set/wait_set_example.cpp index ba10d094..88cf77d2 100644 --- a/rclcpp/wait_set/wait_set_example.cpp +++ b/rclcpp/wait_set/wait_set_example.cpp @@ -12,8 +12,10 @@ // 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" using namespace std::chrono_literals; diff --git a/rclcpp/wait_set/wait_set_random_order.cpp b/rclcpp/wait_set/wait_set_random_order.cpp index f9772ff6..70a1e46c 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -12,8 +12,11 @@ // 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" + #include "random_listener.hpp" #include "random_talker.hpp" From 3197c11a8366ec4c776a836fddecae0bf2b5b333 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 30 Jun 2021 17:11:23 +0200 Subject: [PATCH 23/59] Rename executables Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 18 +++++++++--------- ...p => wait_set_and_executor_composition.cpp} | 0 ...ample.cpp => wait_set_topics_and_timer.cpp} | 0 ....cpp => wait_set_topics_with_different.cpp} | 0 4 files changed, 9 insertions(+), 9 deletions(-) rename rclcpp/wait_set/{wait_set_composition.cpp => wait_set_and_executor_composition.cpp} (100%) rename rclcpp/wait_set/{wait_set_example.cpp => wait_set_topics_and_timer.cpp} (100%) rename rclcpp/wait_set/{wait_set_different_rates_topics.cpp => wait_set_topics_with_different.cpp} (100%) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 9dab43ca..c1bd5643 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -24,8 +24,8 @@ 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) -add_executable(wait_set_example wait_set_example.cpp) -ament_target_dependencies(wait_set_example rclcpp std_msgs) +add_executable(wait_set_topics_and_timer wait_set_topics_and_timer.cpp) +ament_target_dependencies(wait_set_topics_and_timer rclcpp std_msgs) add_executable(wait_set_random_order wait_set_random_order.cpp) ament_target_dependencies(wait_set_random_order rclcpp std_msgs) @@ -33,21 +33,21 @@ ament_target_dependencies(wait_set_random_order rclcpp std_msgs) add_executable(executor_random_order executor_random_order.cpp) ament_target_dependencies(executor_random_order rclcpp std_msgs) -add_executable(wait_set_different_rates_topics wait_set_different_rates_topics.cpp) -ament_target_dependencies(wait_set_different_rates_topics rclcpp std_msgs) +add_executable(wait_set_topics_with_different wait_set_topics_with_different.cpp) +ament_target_dependencies(wait_set_topics_with_different rclcpp std_msgs) -add_executable(wait_set_composition wait_set_composition.cpp) -ament_target_dependencies(wait_set_composition rclcpp std_msgs) +add_executable(wait_set_and_executor_composition wait_set_and_executor_composition.cpp) +ament_target_dependencies(wait_set_and_executor_composition rclcpp std_msgs) install(TARGETS wait_set static_wait_set thread_safe_wait_set - wait_set_example + wait_set_topics_and_timer wait_set_random_order executor_random_order - wait_set_different_rates_topics - wait_set_composition + wait_set_topics_with_different + wait_set_and_executor_composition DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/rclcpp/wait_set/wait_set_composition.cpp b/rclcpp/wait_set/wait_set_and_executor_composition.cpp similarity index 100% rename from rclcpp/wait_set/wait_set_composition.cpp rename to rclcpp/wait_set/wait_set_and_executor_composition.cpp diff --git a/rclcpp/wait_set/wait_set_example.cpp b/rclcpp/wait_set/wait_set_topics_and_timer.cpp similarity index 100% rename from rclcpp/wait_set/wait_set_example.cpp rename to rclcpp/wait_set/wait_set_topics_and_timer.cpp diff --git a/rclcpp/wait_set/wait_set_different_rates_topics.cpp b/rclcpp/wait_set/wait_set_topics_with_different.cpp similarity index 100% rename from rclcpp/wait_set/wait_set_different_rates_topics.cpp rename to rclcpp/wait_set/wait_set_topics_with_different.cpp From f47701054d8934415d134ac64800bb80f7f73598 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 1 Jul 2021 09:44:56 +0200 Subject: [PATCH 24/59] Add minimal subscriber example using a timer triggered node Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 12 +-- .../waitset_time_triggered.cpp | 79 +++++++++++++++++++ .../topics/minimal_subscriber/CMakeLists.txt | 4 + rclcpp/topics/minimal_subscriber/README.md | 2 + .../wait_set_topics_with_different.cpp | 7 +- 5 files changed, 94 insertions(+), 10 deletions(-) create mode 100644 rclcpp/minimal_subscriber/waitset_time_triggered.cpp diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index 7b175656..db24f48c 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -47,9 +47,9 @@ class MinimalSubscriber : public rclcpp::Node if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; - bool message_received = subscription_->take(msg, msg_info); - if (message_received) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); + if (subscription_->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscription_->handle_message(type_erased_msg, msg_info); } } } @@ -76,9 +76,9 @@ int main(int argc, char * argv[]) if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; - bool message_received = sub->take(msg, msg_info); - if (message_received) { - RCLCPP_INFO(node->get_logger(), "I heard: '%s'", msg.data.c_str()); + if (sub->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + sub->handle_message(type_erased_msg, msg_info); } } } diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp new file mode 100644 index 00000000..29b7ec1d --- /dev/null +++ b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp @@ -0,0 +1,79 @@ +// 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; + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + subscription_ = this->create_subscription( + "topic", + 1, + [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }); + // add timer to directly take a message if available + 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_INFO(this->get_logger(), "No message available"); + } + }; + timer_ = create_wall_timer(500ms, timer_callback); + wait_set_.add_timer(timer_); + } + + void run() + { + while (rclcpp::ok()) { + const auto wait_result = wait_set_.wait(510ms); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { + timer_->execute_callback(); + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout"); + } + } + } + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::WaitSet wait_set_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->run(); + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index 2c9ae2cf..d1585017 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -35,6 +35,9 @@ ament_target_dependencies(subscriber_not_composable rclcpp std_msgs) add_executable(subscriber_waitset waitset.cpp) ament_target_dependencies(subscriber_waitset rclcpp std_msgs) +add_executable(subscriber_waitset_time_triggered waitset_time_triggered.cpp) +ament_target_dependencies(subscriber_waitset_time_triggered rclcpp std_msgs) + install(TARGETS subscriber_lambda subscriber_member_function @@ -43,6 +46,7 @@ install(TARGETS subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable subscriber_waitset + subscriber_waitset_time_triggered DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 5fb9cfd2..7fb767fb 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -5,6 +5,8 @@ This package contains a few different strategies for creating nodes which receiv * `member_function.cpp` uses a C++ member function callback * `not_composable.cpp` uses a global function callback without a Node subclass * `waitset.cpp` uses a `rclcpp::waitset` to wait and poll for data + * `waitset_time_triggered.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. diff --git a/rclcpp/wait_set/wait_set_topics_with_different.cpp b/rclcpp/wait_set/wait_set_topics_with_different.cpp index cfe1ddf7..443d356b 100644 --- a/rclcpp/wait_set/wait_set_topics_with_different.cpp +++ b/rclcpp/wait_set/wait_set_topics_with_different.cpp @@ -85,18 +85,17 @@ int32_t main(const int32_t argc, char ** const argv) 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]; - // topics A and B and handled together, but only on a topic B message arrival + // topic B is used as a trigger condition for topic A and B data handling if (sub2_has_data) { std_msgs::msg::String msg1; std_msgs::msg::String msg2; rclcpp::MessageInfo msg_info; std::string handled_data; - // this simulates a case where topics A and B must be handled together - // since topic A is published at a faster rate we expect to have multiple messages in queue - // depending on the application we may want to take the latest one, take all, etc if (sub2->take(msg2, msg_info)) { // take all the messages received from topic A + // 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); } From 59a6c65925872a2fe2e263680a3b9a1bfbc75bcb Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 1 Jul 2021 15:49:58 +0200 Subject: [PATCH 25/59] Fix incomplete executable name Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 6 +++--- rclcpp/wait_set/random_listener.hpp | 2 +- rclcpp/wait_set/wait_set_topics_and_timer.cpp | 12 ++++++------ ....cpp => wait_set_topics_with_different_rates.cpp} | 4 ++-- 4 files changed, 12 insertions(+), 12 deletions(-) rename rclcpp/wait_set/{wait_set_topics_with_different.cpp => wait_set_topics_with_different_rates.cpp} (96%) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index c1bd5643..044870b4 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -33,8 +33,8 @@ ament_target_dependencies(wait_set_random_order rclcpp std_msgs) add_executable(executor_random_order executor_random_order.cpp) ament_target_dependencies(executor_random_order rclcpp std_msgs) -add_executable(wait_set_topics_with_different wait_set_topics_with_different.cpp) -ament_target_dependencies(wait_set_topics_with_different rclcpp std_msgs) +add_executable(wait_set_topics_with_different_rates wait_set_topics_with_different_rates.cpp) +ament_target_dependencies(wait_set_topics_with_different_rates rclcpp std_msgs) add_executable(wait_set_and_executor_composition wait_set_and_executor_composition.cpp) ament_target_dependencies(wait_set_and_executor_composition rclcpp std_msgs) @@ -46,7 +46,7 @@ install(TARGETS wait_set_topics_and_timer wait_set_random_order executor_random_order - wait_set_topics_with_different + wait_set_topics_with_different_rates wait_set_and_executor_composition DESTINATION lib/${PROJECT_NAME} ) diff --git a/rclcpp/wait_set/random_listener.hpp b/rclcpp/wait_set/random_listener.hpp index e704627a..822055bc 100644 --- a/rclcpp/wait_set/random_listener.hpp +++ b/rclcpp/wait_set/random_listener.hpp @@ -30,7 +30,7 @@ class Listener : public rclcpp::Node : 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()); + 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); diff --git a/rclcpp/wait_set/wait_set_topics_and_timer.cpp b/rclcpp/wait_set/wait_set_topics_and_timer.cpp index 88cf77d2..8d0ffba3 100644 --- a/rclcpp/wait_set/wait_set_topics_and_timer.cpp +++ b/rclcpp/wait_set/wait_set_topics_and_timer.cpp @@ -51,9 +51,9 @@ int32_t main(const int32_t argc, char ** const argv) // 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()); + 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); @@ -87,7 +87,7 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::MessageInfo msg_info; if (sub1->take(msg, msg_info)) { ++num_recv; - RCLCPP_INFO(node->get_logger(), "msg1 data: %s", msg.data.c_str()); + RCLCPP_INFO(node->get_logger(), "msg1 data: '%s'", msg.data.c_str()); } } @@ -96,7 +96,7 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::MessageInfo msg_info; if (sub2->take(msg, msg_info)) { ++num_recv; - RCLCPP_INFO(node->get_logger(), "msg2 data: %s", msg.data.c_str()); + RCLCPP_INFO(node->get_logger(), "msg2 data: '%s'", msg.data.c_str()); } } @@ -105,7 +105,7 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::MessageInfo msg_info; 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(), "msg3 data: '%s'", msg.data.c_str()); } } RCLCPP_INFO(node->get_logger(), "Number of messages already got: %zu of 3", num_recv); diff --git a/rclcpp/wait_set/wait_set_topics_with_different.cpp b/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp similarity index 96% rename from rclcpp/wait_set/wait_set_topics_with_different.cpp rename to rclcpp/wait_set/wait_set_topics_with_different_rates.cpp index 443d356b..13fb617e 100644 --- a/rclcpp/wait_set/wait_set_topics_with_different.cpp +++ b/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp @@ -100,7 +100,7 @@ int32_t main(const int32_t argc, char ** const argv) handled_data.append(msg1.data); } handled_data.append(msg2.data); - RCLCPP_INFO(node->get_logger(), "I heard: %s", handled_data.c_str()); + 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."); } @@ -111,7 +111,7 @@ int32_t main(const int32_t argc, char ** const argv) 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()); + 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."); } From e0cf1d653b28579f42463ba683488942e34f63c8 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 6 Jul 2021 15:26:20 +0200 Subject: [PATCH 26/59] Complete README.md Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/README.md | 17 ++++- rclcpp/wait_set/wait_set.cpp | 124 ----------------------------------- 2 files changed, 14 insertions(+), 127 deletions(-) delete mode 100644 rclcpp/wait_set/wait_set.cpp diff --git a/rclcpp/README.md b/rclcpp/README.md index 35b81da1..8e4dfde7 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -1,4 +1,15 @@ -# rclcpp examples +# Minimal rclcpp wait-set cookbook recipes -This directory contains many examples of how to do common tasks with rclcpp. -The intent is that this material will be easy to copy-and-paste into your own projects. +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_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. diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp deleted file mode 100644 index b7fe8b5d..00000000 --- a/rclcpp/wait_set/wait_set.cpp +++ /dev/null @@ -1,124 +0,0 @@ -// 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 -#include -#include -// #include -// #include - -#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, - const std::shared_ptr, - const std::shared_ptr) - {assert(false);}; - - auto node = std::make_shared("wait_set_example_node"); - - auto guard_condition = std::make_shared(); - auto guard_condition2 = std::make_shared(); - - auto sub = node->create_subscription("~/chatter", 10, do_nothing); - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); - - auto pub = node->create_publisher("~/chatter", 10); - rclcpp::PublisherOptions po; - po.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - auto pub2 = node->create_publisher("~/chatter", 10, po); - - auto client = node->create_client("~/add_two_ints"); - auto service = - node->create_service("~/add_two_ints", add_two_ints); - - rclcpp::WaitSet wait_set( - std::vector{{sub}}, - std::vector{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("~/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; -} From ca67af001c0f5092592c82e158a44a675cb26971 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 6 Jul 2021 15:27:21 +0200 Subject: [PATCH 27/59] Refactor wait-set policies examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/static_wait_set.cpp | 81 +++++++++++--- rclcpp/wait_set/thread_safe_wait_set.cpp | 137 ++++++++++++----------- rclcpp/wait_set/wait_set.cpp | 111 ++++++++++++++++++ 3 files changed, 244 insertions(+), 85 deletions(-) create mode 100644 rclcpp/wait_set/wait_set.cpp diff --git a/rclcpp/wait_set/static_wait_set.cpp b/rclcpp/wait_set/static_wait_set.cpp index 8c75a4b5..71b66a1d 100644 --- a/rclcpp/wait_set/static_wait_set.cpp +++ b/rclcpp/wait_set/static_wait_set.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -21,28 +22,74 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - // auto do_nothing = [](std_msgs::msg::String::UniquePtr){}; + auto node = std::make_shared("static_wait_set_example_node"); - auto guard_condition = std::make_shared(); + 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<0, 1, 0, 0, 0, 0> static_wait_set( - std::array::SubscriptionEntry, 0>{}, - std::array{{guard_condition}}, + 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>{}); - // 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); - } + 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) { + int guard_conditions_num = static_wait_set.get_rcl_wait_set().size_of_guard_conditions; + int subscriptions_num = static_wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (int 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 %d triggered", i + 1); + } + } + for (int i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + RCLCPP_INFO(node->get_logger(), "subscription %d 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 %d: I heard '%s'", i + 1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %d: 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/thread_safe_wait_set.cpp b/rclcpp/wait_set/thread_safe_wait_set.cpp index 963e536e..f4f8ab2d 100644 --- a/rclcpp/wait_set/thread_safe_wait_set.cpp +++ b/rclcpp/wait_set/thread_safe_wait_set.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. @@ -15,10 +15,7 @@ #include #include #include -// #include -// #include -#include "example_interfaces/srv/add_two_ints.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -26,85 +23,89 @@ 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("wait_set_example_node"); - auto guard_condition = std::make_shared(); + 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(); - auto sub = node->create_subscription("~/chatter", 10, do_nothing); - rclcpp::SubscriptionOptions so; - so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; - auto sub2 = node->create_subscription("~/chatter", 10, do_nothing, so); - + // 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_condition}); + // 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)); - 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); - } + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; + int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (int i=0; iget_logger(), "guard_condition %d triggered", i+1); + } + } + for (int i=0; iget_logger(), "subscription %d 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 %d: I heard '%s'", i+1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %d: 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(); - { - // 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); - } - + RCLCPP_INFO(node->get_logger(), "Action: Subscription 2 removed"); wait_set.remove_subscription(sub2); + wait_and_print_results(); - { - // 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("~/chatter", 1); - pub->publish(std_msgs::msg::String().set__data("test")); + RCLCPP_INFO(node->get_logger(), "Action: Subscription 1 removed"); + wait_set.remove_subscription(sub1); + wait_and_print_results(); - { - 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; } diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp new file mode 100644 index 00000000..34bcadf6 --- /dev/null +++ b/rclcpp/wait_set/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) { + int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; + int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (int i=0; iget_logger(), "guard_condition %d triggered", i+1); + } + } + for (int i=0; iget_logger(), "subscription %d 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 %d: I heard '%s'", i+1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %d: 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; +} From fd993eb5a03c45c5eba13a73558a0ba34d5e7a6b Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 7 Jul 2021 09:42:44 +0200 Subject: [PATCH 28/59] Refactor MinimalSubscriber wait-set examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 38 ++++++------------- .../waitset_time_triggered.cpp | 25 ++++++------ 2 files changed, 26 insertions(+), 37 deletions(-) diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index db24f48c..d4ed0960 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -18,6 +18,9 @@ #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 and handle + * messages from the subscriber */ + class MinimalSubscriber : public rclcpp::Node { public: @@ -30,6 +33,7 @@ class MinimalSubscriber : public rclcpp::Node [this](std_msgs::msg::String::UniquePtr msg) { RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); }); + wait_set_.add_subscription(subscription_); } rclcpp::Subscription::SharedPtr get_subscription() const @@ -39,10 +43,9 @@ class MinimalSubscriber : public rclcpp::Node void run() { - rclcpp::WaitSet wait_set; - wait_set.add_subscription(subscription_); while (rclcpp::ok()) { - const auto wait_result = wait_set.wait(std::chrono::milliseconds(500)); + // 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)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; @@ -52,41 +55,24 @@ class MinimalSubscriber : public rclcpp::Node subscription_->handle_message(type_erased_msg, msg_info); } } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Timeout. No message received after given wait-time"); + } } } } private: rclcpp::Subscription::SharedPtr subscription_; + rclcpp::WaitSet wait_set_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); auto node = std::make_shared(); - - - // Option 1: loop in main - auto sub = node->get_subscription(); - rclcpp::WaitSet wait_set; - wait_set.add_subscription(sub); - while (rclcpp::ok()) { - const auto wait_result = wait_set.wait(std::chrono::milliseconds(500)); - 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 (sub->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - sub->handle_message(type_erased_msg, msg_info); - } - } - } - } - - // Option 2: run node - // node->run(); - + node->run(); rclcpp::shutdown(); return 0; } diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp index 29b7ec1d..86f0f38d 100644 --- a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp +++ b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp @@ -20,6 +20,9 @@ using namespace std::chrono_literals; +/* This example creates a subclass of Node and uses a wait-set based loop and polls periodically + * for messages in a timer callback using a wait-set based loop. */ + class MinimalSubscriber : public rclcpp::Node { public: @@ -32,17 +35,16 @@ class MinimalSubscriber : public rclcpp::Node [this](std_msgs::msg::String::UniquePtr msg) { RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); }); - // add timer to directly take a message if available 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_INFO(this->get_logger(), "No message available"); - } - }; + 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_INFO(this->get_logger(), "No message available"); + } + }; timer_ = create_wall_timer(500ms, timer_callback); wait_set_.add_timer(timer_); } @@ -50,7 +52,8 @@ class MinimalSubscriber : public rclcpp::Node void run() { while (rclcpp::ok()) { - const auto wait_result = wait_set_.wait(510ms); + // Wait for the timer event to trigger. Set a 1 ms margin to trigger a timeout. + const auto wait_result = wait_set_.wait(501ms); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { timer_->execute_callback(); From d7d700676538fc6875e39d7ab1d75ed1e7f40376 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 7 Jul 2021 09:48:44 +0200 Subject: [PATCH 29/59] Document missing wait-set examples Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rclcpp/README.md b/rclcpp/README.md index 8e4dfde7..6dbc82fd 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -4,6 +4,13 @@ This package contains a few different strategies for creating nodes which use `r 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 static 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 From 8c70ccb7dbcd1505d99437870d98609347926de2 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 7 Jul 2021 15:57:57 +0200 Subject: [PATCH 30/59] Fix incorrect path for README.md Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/README.md | 24 +++--------------------- rclcpp/wait_set/README.md | 26 ++++++++++++++++++++++++++ 2 files changed, 29 insertions(+), 21 deletions(-) create mode 100644 rclcpp/wait_set/README.md diff --git a/rclcpp/README.md b/rclcpp/README.md index 6dbc82fd..133defd6 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -1,22 +1,4 @@ -# Minimal rclcpp wait-set cookbook recipes +# rclcpp examples -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 static 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. +This directory contains many examples of how to do common tasks with rclcpp. +The intent is that this material will be easy to copy-and-paste into your own projects. \ No newline at end of file diff --git a/rclcpp/wait_set/README.md b/rclcpp/wait_set/README.md new file mode 100644 index 00000000..19e72059 --- /dev/null +++ b/rclcpp/wait_set/README.md @@ -0,0 +1,26 @@ +# rclcpp examples + +This directory contains many examples of how to do common tasks with rclcpp. +The intent is that this material will be easy to cop# 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. + and-paste into your own projects. \ No newline at end of file From e910755f33456facc3119411d4449da4cd4fa499 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 7 Jul 2021 16:28:50 +0200 Subject: [PATCH 31/59] Style fixes Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/README.md | 5 +- rclcpp/wait_set/executor_random_order.cpp | 2 +- rclcpp/wait_set/thread_safe_wait_set.cpp | 61 ++++++++++--------- rclcpp/wait_set/wait_set.cpp | 23 +++---- .../wait_set_and_executor_composition.cpp | 4 +- rclcpp/wait_set/wait_set_random_order.cpp | 28 ++++----- rclcpp/wait_set/wait_set_topics_and_timer.cpp | 24 ++------ .../wait_set_topics_with_different_rates.cpp | 19 +++--- 8 files changed, 73 insertions(+), 93 deletions(-) diff --git a/rclcpp/wait_set/README.md b/rclcpp/wait_set/README.md index 19e72059..abb1928a 100644 --- a/rclcpp/wait_set/README.md +++ b/rclcpp/wait_set/README.md @@ -1,7 +1,4 @@ -# rclcpp examples - -This directory contains many examples of how to do common tasks with rclcpp. -The intent is that this material will be easy to cop# Minimal rclcpp wait-set cookbook recipes +# 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 diff --git a/rclcpp/wait_set/executor_random_order.cpp b/rclcpp/wait_set/executor_random_order.cpp index 606db758..285642c7 100644 --- a/rclcpp/wait_set/executor_random_order.cpp +++ b/rclcpp/wait_set/executor_random_order.cpp @@ -35,7 +35,7 @@ int32_t main(const int32_t argc, char ** const 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 difference threads the handling order depends on the message arrival and + // 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()); diff --git a/rclcpp/wait_set/thread_safe_wait_set.cpp b/rclcpp/wait_set/thread_safe_wait_set.cpp index f4f8ab2d..2153c553 100644 --- a/rclcpp/wait_set/thread_safe_wait_set.cpp +++ b/rclcpp/wait_set/thread_safe_wait_set.cpp @@ -36,45 +36,46 @@ int main(int argc, char * argv[]) // 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}); + // std::vector{{sub}}, + {}, + std::vector{guard_condition1}); - wait_set.add_subscription(sub1); // FIXME: add it in the ctor + 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) { - int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; - int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; - - for (int i=0; iget_logger(), "guard_condition %d triggered", i+1); + RCLCPP_INFO(node->get_logger(), "Waiting..."); + auto wait_result = wait_set.wait(std::chrono::seconds(1)); + if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { + int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; + int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + + for (int 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 %d triggered", i + 1); + } } - } - for (int i=0; iget_logger(), "subscription %d 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 %d: I heard '%s'", i+1, msg.data.c_str()); - } else { - RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i+1); + for (int i = 0; i < subscriptions_num; i++) { + if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) { + RCLCPP_INFO(node->get_logger(), "subscription %d 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 %d: I heard '%s'", i + 1, msg.data.c_str()); + } else { + RCLCPP_INFO(node->get_logger(), "subscription %d: 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"); } - } 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(); diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 34bcadf6..6fc28043 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -37,11 +37,11 @@ int main(int argc, char * argv[]) // 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}); + // std::vector{{sub}}, + {}, + std::vector{guard_condition1}); - wait_set.add_subscription(sub1); // FIXME: add it in the ctor + wait_set.add_subscription(sub1); // FIXME: add it in the ctor wait_set.add_subscription(sub2); wait_set.add_guard_condition(guard_condition2); @@ -52,21 +52,22 @@ int main(int argc, char * argv[]) int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; - for (int i=0; iget_logger(), "guard_condition %d triggered", i+1); + RCLCPP_INFO(node->get_logger(), "guard_condition %d triggered", i + 1); } } - for (int i=0; iget_logger(), "subscription %d triggered", i+1); + RCLCPP_INFO(node->get_logger(), "subscription %d 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 %d: I heard '%s'", i+1, msg.data.c_str()); + RCLCPP_INFO( + node->get_logger(), + "subscription %d: I heard '%s'", i + 1, msg.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i+1); + RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i + 1); } } } diff --git a/rclcpp/wait_set/wait_set_and_executor_composition.cpp b/rclcpp/wait_set/wait_set_and_executor_composition.cpp index d2ab6e04..ff7c678c 100644 --- a/rclcpp/wait_set/wait_set_and_executor_composition.cpp +++ b/rclcpp/wait_set/wait_set_and_executor_composition.cpp @@ -96,12 +96,14 @@ int main(int argc, char * argv[]) auto listener = std::make_shared(); exec.add_node(listener); - // Create a static wait-set that will wait on the subscription that will excluded from the + // Create a static wait-set that will wait on the subscription excluded from the // default executor callback group rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{listener->get_subscription()}}}); + // Run the executor in a separate thread auto thread = std::thread([&exec]() {exec.spin();}); + // Run the wait-set loop in the main thread while (rclcpp::ok()) { // Waiting up to 5s for a message to arrive const auto wait_result = wait_set.wait(std::chrono::seconds(5)); diff --git a/rclcpp/wait_set/wait_set_random_order.cpp b/rclcpp/wait_set/wait_set_random_order.cpp index 70a1e46c..18cbcf00 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -24,10 +24,9 @@ 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 directly 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. - */ + * 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) { @@ -56,19 +55,14 @@ int32_t main(const int32_t argc, char ** const argv) if (sub1_has_data && sub2_has_data && sub3_has_data) { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; - - // the messages are taken and handled in a user-defined order - if (subscriptions.at(0)->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - subscriptions.at(0)->handle_message(type_erased_msg, msg_info); - } - if (subscriptions.at(1)->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - subscriptions.at(1)->handle_message(type_erased_msg, msg_info); - } - if (subscriptions.at(2)->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - subscriptions.at(2)->handle_message(type_erased_msg, msg_info); + const int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + for (int 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) { diff --git a/rclcpp/wait_set/wait_set_topics_and_timer.cpp b/rclcpp/wait_set/wait_set_topics_and_timer.cpp index 8d0ffba3..d124023f 100644 --- a/rclcpp/wait_set/wait_set_topics_and_timer.cpp +++ b/rclcpp/wait_set/wait_set_topics_and_timer.cpp @@ -48,7 +48,7 @@ int32_t main(const int32_t argc, char ** const argv) // 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 - // the user to trigger it manually inside the wait-set loop. + // 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()); @@ -63,46 +63,32 @@ int32_t main(const int32_t argc, char ** const argv) one_off_timer = node->create_wall_timer(1s, timer_callback); - /* - // Option 1: add entities after construction - rclcpp::WaitSet wait_set; - wait_set.add_subscription(sub1); - wait_set.add_subscription(sub2); - wait_set.add_subscription(sub3); - wait_set.add_timer(one_off_timer); - */ - // Option 2: add entities in the constructor 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]) { - // we execute manually the timer callback + // 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; 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]) { - std_msgs::msg::String msg; - rclcpp::MessageInfo msg_info; if (sub3->take(msg, msg_info)) { ++num_recv; RCLCPP_INFO(node->get_logger(), "msg3 data: '%s'", msg.data.c_str()); diff --git a/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp b/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp index 13fb617e..90957879 100644 --- a/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp +++ b/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp @@ -23,10 +23,10 @@ using namespace std::chrono_literals; -/* For this example, we will be creating three talkers publishing the topics A, B, C at a different - * rate. The messages are handled by a wait-set loop which handles topics A and B on a topic B - * message arrival and topic C separately. - */ +/* 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 { @@ -41,7 +41,7 @@ class Talker : public rclcpp::Node publisher_ = this->create_publisher(topic_name, 10); auto timer_callback = [this, message_data]() -> void { - auto message = std_msgs::msg::String(); + std_msgs::msg::String message; message.data = message_data; RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); this->publisher_->publish(message); @@ -67,7 +67,7 @@ int32_t main(const int32_t argc, char ** const argv) rclcpp::WaitSet wait_set({{sub1}, {sub2}, {sub3}}); - // Creates three talkers publishing in topics A, B, and C with different publishing rates + // 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); @@ -85,7 +85,8 @@ int32_t main(const int32_t argc, char ** const argv) 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 B is used as a trigger condition for topic A and B data handling + // 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; @@ -93,9 +94,7 @@ int32_t main(const int32_t argc, char ** const argv) std::string handled_data; if (sub2->take(msg2, msg_info)) { - // take all the messages received from topic A // 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); } @@ -106,7 +105,7 @@ int32_t main(const int32_t argc, char ** const argv) } } - // topics C is handled independently + // topic C handling if (sub3_has_data) { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; From 5c82c7d9f8b79fb3d43e967b68467343f36ec969 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Wed, 7 Jul 2021 17:02:13 +0200 Subject: [PATCH 32/59] Replace RCLCPP_ERROR with RCLCPP_WARN Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 2 +- rclcpp/minimal_subscriber/waitset_time_triggered.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index d4ed0960..d02e8992 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -57,7 +57,7 @@ class MinimalSubscriber : public rclcpp::Node } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { if (rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Timeout. No message received after given wait-time"); + RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time"); } } } diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp index 86f0f38d..73ababbc 100644 --- a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp +++ b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp @@ -42,7 +42,7 @@ class MinimalSubscriber : public rclcpp::Node std::shared_ptr type_erased_msg = std::make_shared(msg); subscription_->handle_message(type_erased_msg, msg_info); } else { - RCLCPP_INFO(this->get_logger(), "No message available"); + RCLCPP_WARN(this->get_logger(), "No message available"); } }; timer_ = create_wall_timer(500ms, timer_callback); From f3116d10c7d835dac43f23c0db1ee04bea835c23 Mon Sep 17 00:00:00 2001 From: carlossvg Date: Thu, 8 Jul 2021 19:28:39 +0200 Subject: [PATCH 33/59] Update rclcpp/wait_set/wait_set_topics_and_timer.cpp Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set_topics_and_timer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/wait_set/wait_set_topics_and_timer.cpp b/rclcpp/wait_set/wait_set_topics_and_timer.cpp index d124023f..3b61bf1b 100644 --- a/rclcpp/wait_set/wait_set_topics_and_timer.cpp +++ b/rclcpp/wait_set/wait_set_topics_and_timer.cpp @@ -19,7 +19,7 @@ using namespace std::chrono_literals; -/* This example creates a node with three publishers, three subscriptions and one-off timer. The +/* 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 */ From 6d4c362957426e2ecd3239ac2b0f2ec183598df1 Mon Sep 17 00:00:00 2001 From: carlossvg Date: Thu, 8 Jul 2021 20:03:42 +0200 Subject: [PATCH 34/59] Update rclcpp/minimal_subscriber/README.md Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 7fb767fb..56da2649 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -4,7 +4,7 @@ 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 - * `waitset.cpp` uses a `rclcpp::waitset` to wait and poll for data + * `waitset.cpp` uses a `rclcpp::WaitSet` to wait and poll for data * `waitset_time_triggered.cpp` uses a `rclcpp::waitset` and a timer to poll for data periodically From 2792baa029c287386c7a0cbeec93317ab3a7cf73 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 21:36:05 +0200 Subject: [PATCH 35/59] Spin nodes in executor Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 25 ++++++++++-------- .../waitset_time_triggered.cpp | 26 +++++++++++++------ 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index d02e8992..a12a2499 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -27,21 +27,24 @@ class MinimalSubscriber : public rclcpp::Node MinimalSubscriber() : Node("minimal_subscriber") { + rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto options = rclcpp::SubscriptionOptions(); + 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, - [this](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); - }); + subscription_callback, + options); wait_set_.add_subscription(subscription_); + thread_ = std::thread([this]() -> void {run_waitset_loo();}); } - rclcpp::Subscription::SharedPtr get_subscription() const - { - return subscription_; - } - - void run() + void run_waitset_loo() { while (rclcpp::ok()) { // Wait for the subscriber event to trigger. Set a 1 ms margin to trigger a timeout. @@ -66,13 +69,13 @@ class MinimalSubscriber : public rclcpp::Node private: rclcpp::Subscription::SharedPtr subscription_; rclcpp::WaitSet wait_set_; + std::thread thread_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->run(); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp index 73ababbc..e17b5067 100644 --- a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp +++ b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp @@ -29,12 +29,21 @@ class MinimalSubscriber : public rclcpp::Node MinimalSubscriber() : Node("minimal_subscriber") { + rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + auto options = rclcpp::SubscriptionOptions(); + 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", - 1, - [this](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); - }); + 10, + subscription_callback, + options); + auto timer_callback = [this]() -> void { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; @@ -45,11 +54,12 @@ class MinimalSubscriber : public rclcpp::Node RCLCPP_WARN(this->get_logger(), "No message available"); } }; - timer_ = create_wall_timer(500ms, timer_callback); + timer_ = create_wall_timer(500ms, timer_callback, cb_group_waitset); wait_set_.add_timer(timer_); + thread_ = std::thread([this]() -> void {run_waitset_loo();}); } - void run() + void run_waitset_loo() { while (rclcpp::ok()) { // Wait for the timer event to trigger. Set a 1 ms margin to trigger a timeout. @@ -70,13 +80,13 @@ class MinimalSubscriber : public rclcpp::Node rclcpp::Subscription::SharedPtr subscription_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::WaitSet wait_set_; + std::thread thread_; }; int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); - node->run(); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From 514acdc3bbe1c216637145af47efe17400eae774 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 21:37:49 +0200 Subject: [PATCH 36/59] Remove out of place line Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp/wait_set/README.md b/rclcpp/wait_set/README.md index abb1928a..bf497dd7 100644 --- a/rclcpp/wait_set/README.md +++ b/rclcpp/wait_set/README.md @@ -19,5 +19,4 @@ conditions and waitables. * `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. - and-paste into your own projects. \ No newline at end of file + condition to handle topics with different topic rates. \ No newline at end of file From 1da4bd2f681480b52a1102f7aaeeab2e454590cd Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 21:39:10 +0200 Subject: [PATCH 37/59] Rename Listener to RandomListener Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/random_listener.hpp | 4 ++-- rclcpp/wait_set/wait_set_random_order.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rclcpp/wait_set/random_listener.hpp b/rclcpp/wait_set/random_listener.hpp index 822055bc..a5804077 100644 --- a/rclcpp/wait_set/random_listener.hpp +++ b/rclcpp/wait_set/random_listener.hpp @@ -21,12 +21,12 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -class Listener : public rclcpp::Node +class RandomListener : public rclcpp::Node { using subscription_list = std::vector::SharedPtr>; public: - Listener() + RandomListener() : Node("random_listener") { auto print_msg = [this](std_msgs::msg::String::UniquePtr msg) { diff --git a/rclcpp/wait_set/wait_set_random_order.cpp b/rclcpp/wait_set/wait_set_random_order.cpp index 18cbcf00..77735d4f 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/wait_set_random_order.cpp @@ -32,8 +32,8 @@ int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); - auto listener = std::make_shared(); - auto subscriptions = listener->get_subscriptions(); + auto random_listener = std::make_shared(); + auto subscriptions = random_listener->get_subscriptions(); // Create a wait-set and add the subscriptions rclcpp::WaitSet wait_set; @@ -67,7 +67,7 @@ int32_t main(const int32_t argc, char ** const argv) } } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { if (rclcpp::ok()) { - RCLCPP_ERROR(listener->get_logger(), "Wait-set failed with timeout"); + RCLCPP_ERROR(random_listener->get_logger(), "Wait-set failed with timeout"); } } } From a8dd4fb9689432f60c7e5f68d5b6619f6942e603 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 21:50:38 +0200 Subject: [PATCH 38/59] Move wait-set loop thread to the node Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- .../wait_set_and_executor_composition.cpp | 74 +++++++++---------- 1 file changed, 34 insertions(+), 40 deletions(-) diff --git a/rclcpp/wait_set/wait_set_and_executor_composition.cpp b/rclcpp/wait_set/wait_set_and_executor_composition.cpp index ff7c678c..9781cae9 100644 --- a/rclcpp/wait_set/wait_set_and_executor_composition.cpp +++ b/rclcpp/wait_set/wait_set_and_executor_composition.cpp @@ -52,35 +52,55 @@ class Listener : public rclcpp::Node Listener() : Node("listener") { - subscription_executor_ = this->create_subscription( + auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); + }; + sub1_ = this->create_subscription( "topic", 10, - [this](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s' (executor)", msg->data.c_str()); - }); + subscription_callback + ); - // creates a subscription which is not automatically added to an executor rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - auto options = rclcpp::SubscriptionOptions(); options.callback_group = cb_group_waitset; - auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);}; - subscription_waitset_ = this->create_subscription( + sub2_ = this->create_subscription( "topic", 10, - do_nothing, + subscription_callback, options); + wait_set_.add_subscription(sub2_); + thread_ = std::thread([this]() -> void {spin_wait_set();}); } - rclcpp::Subscription::SharedPtr get_subscription() const + void spin_wait_set() { - return subscription_waitset_; + while (rclcpp::ok()) { + // Waiting up to 5s for a message to arrive + const auto wait_result = wait_set_.wait(std::chrono::seconds(5)); + 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 (sub2_->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + sub2_->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"); + } + } + } } private: - rclcpp::Subscription::SharedPtr subscription_executor_; - rclcpp::Subscription::SharedPtr subscription_waitset_; + rclcpp::Subscription::SharedPtr sub1_; + rclcpp::Subscription::SharedPtr sub2_; + rclcpp::WaitSet wait_set_; + std::thread thread_; }; int main(int argc, char * argv[]) @@ -96,34 +116,8 @@ int main(int argc, char * argv[]) auto listener = std::make_shared(); exec.add_node(listener); - // Create a static wait-set that will wait on the subscription excluded from the - // default executor callback group - rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0> wait_set({{{listener->get_subscription()}}}); - - // Run the executor in a separate thread - auto thread = std::thread([&exec]() {exec.spin();}); - - // Run the wait-set loop in the main thread - while (rclcpp::ok()) { - // Waiting up to 5s for a message to arrive - const auto wait_result = wait_set.wait(std::chrono::seconds(5)); - - 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 (listener->get_subscription()->take(msg, msg_info)) { - RCLCPP_INFO(listener->get_logger(), "I heard: '%s' (wait-set)", msg.data.c_str()); - } - } - } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - if (rclcpp::ok()) { - RCLCPP_ERROR(listener->get_logger(), "Wait-set failed with timeout"); - } - } - } + exec.spin(); rclcpp::shutdown(); - thread.join(); return 0; } From de22d002dd95247cf4dde836c1950e43cc5afddb Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 21:52:28 +0200 Subject: [PATCH 39/59] Minimize vertical whitespace Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/wait_set.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/wait_set.cpp index 6fc28043..9e206c79 100644 --- a/rclcpp/wait_set/wait_set.cpp +++ b/rclcpp/wait_set/wait_set.cpp @@ -19,7 +19,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" - int main(int argc, char * argv[]) { rclcpp::init(argc, argv); From 11837648c3648f1db0ae8f21c4acff58a2fbbf63 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 8 Jul 2021 23:42:35 +0200 Subject: [PATCH 40/59] Register nodes as components Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/waitset.cpp | 9 +- .../waitset_time_triggered.cpp | 9 +- rclcpp/wait_set/CMakeLists.txt | 36 +++-- rclcpp/wait_set/include/wait_set/listener.hpp | 37 ++++++ .../wait_set}/random_listener.hpp | 6 +- .../{ => include/wait_set}/random_talker.hpp | 6 +- rclcpp/wait_set/include/wait_set/talker.hpp | 33 +++++ rclcpp/wait_set/include/wait_set/visibility.h | 66 ++++++++++ rclcpp/wait_set/package.xml | 1 + .../{ => src}/executor_random_order.cpp | 6 +- rclcpp/wait_set/src/listener.cpp | 81 ++++++++++++ rclcpp/wait_set/{ => src}/static_wait_set.cpp | 0 rclcpp/wait_set/src/talker.cpp | 39 ++++++ .../{ => src}/thread_safe_wait_set.cpp | 0 rclcpp/wait_set/{ => src}/wait_set.cpp | 0 rclcpp/wait_set/src/wait_set_composed.cpp | 32 +++++ .../{ => src}/wait_set_random_order.cpp | 4 +- .../{ => src}/wait_set_topics_and_timer.cpp | 0 .../wait_set_topics_with_different_rates.cpp | 3 - .../wait_set_and_executor_composition.cpp | 123 ------------------ 20 files changed, 340 insertions(+), 151 deletions(-) create mode 100644 rclcpp/wait_set/include/wait_set/listener.hpp rename rclcpp/wait_set/{ => include/wait_set}/random_listener.hpp (91%) rename rclcpp/wait_set/{ => include/wait_set}/random_talker.hpp (94%) create mode 100644 rclcpp/wait_set/include/wait_set/talker.hpp create mode 100644 rclcpp/wait_set/include/wait_set/visibility.h rename rclcpp/wait_set/{ => src}/executor_random_order.cpp (93%) create mode 100644 rclcpp/wait_set/src/listener.cpp rename rclcpp/wait_set/{ => src}/static_wait_set.cpp (100%) create mode 100644 rclcpp/wait_set/src/talker.cpp rename rclcpp/wait_set/{ => src}/thread_safe_wait_set.cpp (100%) rename rclcpp/wait_set/{ => src}/wait_set.cpp (100%) create mode 100644 rclcpp/wait_set/src/wait_set_composed.cpp rename rclcpp/wait_set/{ => src}/wait_set_random_order.cpp (97%) rename rclcpp/wait_set/{ => src}/wait_set_topics_and_timer.cpp (100%) rename rclcpp/wait_set/{ => src}/wait_set_topics_with_different_rates.cpp (98%) delete mode 100644 rclcpp/wait_set/wait_set_and_executor_composition.cpp diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/waitset.cpp index a12a2499..380323e1 100644 --- a/rclcpp/minimal_subscriber/waitset.cpp +++ b/rclcpp/minimal_subscriber/waitset.cpp @@ -41,10 +41,15 @@ class MinimalSubscriber : public rclcpp::Node subscription_callback, options); wait_set_.add_subscription(subscription_); - thread_ = std::thread([this]() -> void {run_waitset_loo();}); + thread_ = std::thread([this]() -> void {spin_wait_set();}); } - void run_waitset_loo() + ~MinimalSubscriber() + { + 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. diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp index e17b5067..fd9d19c7 100644 --- a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp +++ b/rclcpp/minimal_subscriber/waitset_time_triggered.cpp @@ -56,10 +56,15 @@ class MinimalSubscriber : public rclcpp::Node }; timer_ = create_wall_timer(500ms, timer_callback, cb_group_waitset); wait_set_.add_timer(timer_); - thread_ = std::thread([this]() -> void {run_waitset_loo();}); + thread_ = std::thread([this]() -> void {spin_wait_set();}); } - void run_waitset_loo() + ~MinimalSubscriber() + { + 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. diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 044870b4..1451670d 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -13,31 +13,47 @@ 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) -add_executable(wait_set wait_set.cpp) +include_directories(include) + +add_library(talker SHARED src/talker.cpp) +target_compile_definitions(talker PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(talker "rclcpp" "rclcpp_components" "std_msgs") +rclcpp_components_register_nodes(talker "wait_set::Talker") +set(node_plugins "${node_plugins}wait_set::Talker;$\n") + +add_library(listener SHARED src/listener.cpp) +target_compile_definitions(listener PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(listener "rclcpp" "rclcpp_components" "std_msgs") +rclcpp_components_register_nodes(listener "wait_set::Listener") +set(node_plugins "${node_plugins}wait_set::Listener;$\n") + +add_executable(wait_set src/wait_set.cpp) ament_target_dependencies(wait_set example_interfaces rclcpp std_msgs) -add_executable(static_wait_set static_wait_set.cpp) +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 thread_safe_wait_set.cpp) +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 wait_set_topics_and_timer.cpp) +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 wait_set_random_order.cpp) +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 executor_random_order.cpp) +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 wait_set_topics_with_different_rates.cpp) +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_and_executor_composition wait_set_and_executor_composition.cpp) -ament_target_dependencies(wait_set_and_executor_composition 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 wait_set @@ -47,7 +63,7 @@ install(TARGETS wait_set_random_order executor_random_order wait_set_topics_with_different_rates - wait_set_and_executor_composition + wait_set_composed DESTINATION lib/${PROJECT_NAME} ) ament_package() 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..6ae55491 --- /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(); + +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/random_listener.hpp b/rclcpp/wait_set/include/wait_set/random_listener.hpp similarity index 91% rename from rclcpp/wait_set/random_listener.hpp rename to rclcpp/wait_set/include/wait_set/random_listener.hpp index a5804077..79589ad7 100644 --- a/rclcpp/wait_set/random_listener.hpp +++ b/rclcpp/wait_set/include/wait_set/random_listener.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ -#define RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ +#ifndef WAIT_SET__RANDOM_LISTENER_HPP_ +#define WAIT_SET__RANDOM_LISTENER_HPP_ #include #include @@ -47,4 +47,4 @@ class RandomListener : public rclcpp::Node rclcpp::Subscription::SharedPtr sub2_; rclcpp::Subscription::SharedPtr sub3_; }; -#endif // RCLCPP__WAIT_SET__RANDOM_LISTENER_HPP_ +#endif // WAIT_SET__RANDOM_LISTENER_HPP_ diff --git a/rclcpp/wait_set/random_talker.hpp b/rclcpp/wait_set/include/wait_set/random_talker.hpp similarity index 94% rename from rclcpp/wait_set/random_talker.hpp rename to rclcpp/wait_set/include/wait_set/random_talker.hpp index 08dbb79f..1e5016e1 100644 --- a/rclcpp/wait_set/random_talker.hpp +++ b/rclcpp/wait_set/include/wait_set/random_talker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ -#define RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ +#ifndef WAIT_SET__RANDOM_TALKER_HPP_ +#define WAIT_SET__RANDOM_TALKER_HPP_ #include #include @@ -68,4 +68,4 @@ class RandomTalker : public rclcpp::Node std::vector> publish_functions_; std::default_random_engine rand_engine_; }; -#endif // RCLCPP__WAIT_SET__RANDOM_TALKER_HPP_ +#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 index 8455ea56..da971efb 100644 --- a/rclcpp/wait_set/package.xml +++ b/rclcpp/wait_set/package.xml @@ -13,6 +13,7 @@ example_interfaces rclcpp + rclcpp_components std_msgs example_interfaces diff --git a/rclcpp/wait_set/executor_random_order.cpp b/rclcpp/wait_set/src/executor_random_order.cpp similarity index 93% rename from rclcpp/wait_set/executor_random_order.cpp rename to rclcpp/wait_set/src/executor_random_order.cpp index 285642c7..20a00b65 100644 --- a/rclcpp/wait_set/executor_random_order.cpp +++ b/rclcpp/wait_set/src/executor_random_order.cpp @@ -17,8 +17,8 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "random_listener.hpp" -#include "random_talker.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 @@ -38,7 +38,7 @@ int32_t main(const int32_t argc, char ** const argv) // 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::spin(std::make_shared()); rclcpp::shutdown(); thread.join(); diff --git a/rclcpp/wait_set/src/listener.cpp b/rclcpp/wait_set/src/listener.cpp new file mode 100644 index 00000000..52cec13a --- /dev/null +++ b/rclcpp/wait_set/src/listener.cpp @@ -0,0 +1,81 @@ +// 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() +{ + 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/static_wait_set.cpp b/rclcpp/wait_set/src/static_wait_set.cpp similarity index 100% rename from rclcpp/wait_set/static_wait_set.cpp rename to rclcpp/wait_set/src/static_wait_set.cpp 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/thread_safe_wait_set.cpp b/rclcpp/wait_set/src/thread_safe_wait_set.cpp similarity index 100% rename from rclcpp/wait_set/thread_safe_wait_set.cpp rename to rclcpp/wait_set/src/thread_safe_wait_set.cpp diff --git a/rclcpp/wait_set/wait_set.cpp b/rclcpp/wait_set/src/wait_set.cpp similarity index 100% rename from rclcpp/wait_set/wait_set.cpp rename to rclcpp/wait_set/src/wait_set.cpp 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/wait_set_random_order.cpp b/rclcpp/wait_set/src/wait_set_random_order.cpp similarity index 97% rename from rclcpp/wait_set/wait_set_random_order.cpp rename to rclcpp/wait_set/src/wait_set_random_order.cpp index 77735d4f..296a2c70 100644 --- a/rclcpp/wait_set/wait_set_random_order.cpp +++ b/rclcpp/wait_set/src/wait_set_random_order.cpp @@ -17,8 +17,8 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "random_listener.hpp" -#include "random_talker.hpp" +#include "wait_set/random_listener.hpp" +#include "wait_set/random_talker.hpp" using namespace std::chrono_literals; diff --git a/rclcpp/wait_set/wait_set_topics_and_timer.cpp b/rclcpp/wait_set/src/wait_set_topics_and_timer.cpp similarity index 100% rename from rclcpp/wait_set/wait_set_topics_and_timer.cpp rename to rclcpp/wait_set/src/wait_set_topics_and_timer.cpp diff --git a/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp b/rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp similarity index 98% rename from rclcpp/wait_set/wait_set_topics_with_different_rates.cpp rename to rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp index 90957879..3a4f42a0 100644 --- a/rclcpp/wait_set/wait_set_topics_with_different_rates.cpp +++ b/rclcpp/wait_set/src/wait_set_topics_with_different_rates.cpp @@ -18,9 +18,6 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" -#include "random_listener.hpp" -#include "random_talker.hpp" - using namespace std::chrono_literals; /* For this example, we will be creating three talkers publishing the topics A, B, C at different diff --git a/rclcpp/wait_set/wait_set_and_executor_composition.cpp b/rclcpp/wait_set/wait_set_and_executor_composition.cpp deleted file mode 100644 index 9781cae9..00000000 --- a/rclcpp/wait_set/wait_set_and_executor_composition.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// 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 -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -class Talker : public rclcpp::Node -{ -public: - Talker() - : Node("talker"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - auto timer_callback = - [this]() -> void { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(this->count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - this->publisher_->publish(message); - }; - timer_ = this->create_wall_timer(500ms, timer_callback); - } - -private: - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -class Listener : public rclcpp::Node -{ -public: - Listener() - : Node("listener") - { - auto subscription_callback = [this](std_msgs::msg::String::UniquePtr msg) { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); - }; - sub1_ = this->create_subscription( - "topic", - 10, - subscription_callback - ); - - rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - auto options = rclcpp::SubscriptionOptions(); - options.callback_group = cb_group_waitset; - sub2_ = this->create_subscription( - "topic", - 10, - subscription_callback, - options); - wait_set_.add_subscription(sub2_); - thread_ = std::thread([this]() -> void {spin_wait_set();}); - } - - void spin_wait_set() - { - while (rclcpp::ok()) { - // Waiting up to 5s for a message to arrive - const auto wait_result = wait_set_.wait(std::chrono::seconds(5)); - 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 (sub2_->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - sub2_->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"); - } - } - } - } - -private: - rclcpp::Subscription::SharedPtr sub1_; - rclcpp::Subscription::SharedPtr sub2_; - rclcpp::WaitSet wait_set_; - std::thread thread_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - // Create an executor that will be responsible for execution of callbacks for a set of nodes. - // All callbacks will be called except the subscription which was explicitly excluded from it - rclcpp::executors::SingleThreadedExecutor exec; - - auto talker = std::make_shared(); - exec.add_node(talker); - auto listener = std::make_shared(); - exec.add_node(listener); - - exec.spin(); - - rclcpp::shutdown(); - return 0; -} From 23bcb7a3743c3831c570a8e8fcc7d7f5dd061422 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 09:06:27 +0200 Subject: [PATCH 41/59] Rename waitset to wait_set Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- .../{waitset.cpp => static_wait_set.cpp} | 0 rclcpp/minimal_subscriber/wait_set.cpp | 86 +++++++++++++++++++ ...ggered.cpp => wait_set_time_triggered.cpp} | 0 .../topics/minimal_subscriber/CMakeLists.txt | 12 +-- rclcpp/topics/minimal_subscriber/README.md | 4 +- 5 files changed, 94 insertions(+), 8 deletions(-) rename rclcpp/minimal_subscriber/{waitset.cpp => static_wait_set.cpp} (100%) create mode 100644 rclcpp/minimal_subscriber/wait_set.cpp rename rclcpp/minimal_subscriber/{waitset_time_triggered.cpp => wait_set_time_triggered.cpp} (100%) diff --git a/rclcpp/minimal_subscriber/waitset.cpp b/rclcpp/minimal_subscriber/static_wait_set.cpp similarity index 100% rename from rclcpp/minimal_subscriber/waitset.cpp rename to rclcpp/minimal_subscriber/static_wait_set.cpp diff --git a/rclcpp/minimal_subscriber/wait_set.cpp b/rclcpp/minimal_subscriber/wait_set.cpp new file mode 100644 index 00000000..380323e1 --- /dev/null +++ b/rclcpp/minimal_subscriber/wait_set.cpp @@ -0,0 +1,86 @@ +// 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 and handle + * messages from the subscriber */ + +class MinimalSubscriber : public rclcpp::Node +{ +public: + MinimalSubscriber() + : Node("minimal_subscriber") + { + rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + auto options = rclcpp::SubscriptionOptions(); + 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, + options); + wait_set_.add_subscription(subscription_); + thread_ = std::thread([this]() -> void {spin_wait_set();}); + } + + ~MinimalSubscriber() + { + 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)); + 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 (subscription_->take(msg, msg_info)) { + std::shared_ptr type_erased_msg = std::make_shared(msg); + subscription_->handle_message(type_erased_msg, msg_info); + } + } + } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { + if (rclcpp::ok()) { + RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time"); + } + } + } + } + +private: + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::WaitSet wait_set_; + std::thread thread_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/minimal_subscriber/waitset_time_triggered.cpp b/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp similarity index 100% rename from rclcpp/minimal_subscriber/waitset_time_triggered.cpp rename to rclcpp/minimal_subscriber/wait_set_time_triggered.cpp diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index d1585017..466c0596 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -32,11 +32,11 @@ 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_executable(subscriber_waitset waitset.cpp) -ament_target_dependencies(subscriber_waitset rclcpp std_msgs) +add_executable(subscriber_wait_set wait_set.cpp) +ament_target_dependencies(subscriber_wait_set rclcpp std_msgs) -add_executable(subscriber_waitset_time_triggered waitset_time_triggered.cpp) -ament_target_dependencies(subscriber_waitset_time_triggered rclcpp std_msgs) +add_executable(subscriber_wait_set_time_triggered wait_set_time_triggered.cpp) +ament_target_dependencies(subscriber_wait_set_time_triggered rclcpp std_msgs) install(TARGETS subscriber_lambda @@ -45,8 +45,8 @@ install(TARGETS subscriber_member_function_with_type_adapter subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable - subscriber_waitset - subscriber_waitset_time_triggered + subscriber_wait_set + subscriber_wait_set_time_triggered DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 56da2649..c8eb5d32 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -4,8 +4,8 @@ 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 - * `waitset.cpp` uses a `rclcpp::WaitSet` to wait and poll for data - * `waitset_time_triggered.cpp` uses a `rclcpp::waitset` and a timer to poll + * `wait_set.cpp` uses a `rclcpp::WaitSet` to wait and poll for data + * `wait_set_time_triggered.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. From 252deef194418e710d8f64aec91ac29e1bd32207 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 09:07:46 +0200 Subject: [PATCH 42/59] Add minimal siatic wait-set subscriber example Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/static_wait_set.cpp | 11 +++++++---- rclcpp/topics/minimal_subscriber/CMakeLists.txt | 4 ++++ rclcpp/topics/minimal_subscriber/README.md | 1 + 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/rclcpp/minimal_subscriber/static_wait_set.cpp b/rclcpp/minimal_subscriber/static_wait_set.cpp index 380323e1..5842ecc3 100644 --- a/rclcpp/minimal_subscriber/static_wait_set.cpp +++ b/rclcpp/minimal_subscriber/static_wait_set.cpp @@ -18,7 +18,7 @@ #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 and handle +/* This example creates a subclass of Node and uses static a wait-set based loop to wait and handle * messages from the subscriber */ class MinimalSubscriber : public rclcpp::Node @@ -40,7 +40,10 @@ class MinimalSubscriber : public rclcpp::Node 10, subscription_callback, options); - wait_set_.add_subscription(subscription_); + + wait_set_ptr_ = std::make_shared>( + std::array::SubscriptionEntry, 1>{{{subscription_}}}); + thread_ = std::thread([this]() -> void {spin_wait_set();}); } @@ -53,7 +56,7 @@ class MinimalSubscriber : public rclcpp::Node { 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)); + const auto wait_result = wait_set_ptr_->wait(std::chrono::milliseconds(501)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0U]) { std_msgs::msg::String msg; @@ -73,7 +76,7 @@ class MinimalSubscriber : public rclcpp::Node private: rclcpp::Subscription::SharedPtr subscription_; - rclcpp::WaitSet wait_set_; + std::shared_ptr> wait_set_ptr_; std::thread thread_; }; diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index 466c0596..bedb1f46 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -35,6 +35,9 @@ ament_target_dependencies(subscriber_not_composable rclcpp std_msgs) add_executable(subscriber_wait_set wait_set.cpp) ament_target_dependencies(subscriber_wait_set rclcpp std_msgs) +add_executable(subscriber_static_wait_set static_wait_set.cpp) +ament_target_dependencies(subscriber_static_wait_set rclcpp std_msgs) + add_executable(subscriber_wait_set_time_triggered wait_set_time_triggered.cpp) ament_target_dependencies(subscriber_wait_set_time_triggered rclcpp std_msgs) @@ -46,6 +49,7 @@ install(TARGETS subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable subscriber_wait_set + subscriber_static_wait_set subscriber_wait_set_time_triggered DESTINATION lib/${PROJECT_NAME}) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index c8eb5d32..314417b0 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -5,6 +5,7 @@ This package contains a few different strategies for creating nodes which receiv * `member_function.cpp` uses a C++ member function callback * `not_composable.cpp` uses a global function callback without a Node subclass * `wait_set.cpp` uses a `rclcpp::WaitSet` to wait and poll for data + * `wait_set.cpp` uses a `rclcpp::StaticWaitSet` to wait and poll for data * `wait_set_time_triggered.cpp` uses a `rclcpp::Waitset` and a timer to poll for data periodically From fbe10269b1e596a13907485477e68da9a1b6a3c6 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 09:14:35 +0200 Subject: [PATCH 43/59] Refactor CMakeLists.txt Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/CMakeLists.txt | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 1451670d..0ae62e45 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -19,16 +19,14 @@ find_package(std_msgs REQUIRED) include_directories(include) add_library(talker SHARED src/talker.cpp) -target_compile_definitions(talker PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(talker "rclcpp" "rclcpp_components" "std_msgs") -rclcpp_components_register_nodes(talker "wait_set::Talker") -set(node_plugins "${node_plugins}wait_set::Talker;$\n") +target_compile_definitions(talker PRIVATE WAIT_SET_DLL) +ament_target_dependencies(talker rclcpp rclcpp_components std_msgs) +rclcpp_components_register_nodes(talker Talker) add_library(listener SHARED src/listener.cpp) -target_compile_definitions(listener PRIVATE "COMPOSITION_BUILDING_DLL") -ament_target_dependencies(listener "rclcpp" "rclcpp_components" "std_msgs") -rclcpp_components_register_nodes(listener "wait_set::Listener") -set(node_plugins "${node_plugins}wait_set::Listener;$\n") +target_compile_definitions(listener PRIVATE WAIT_SET_DLL) +ament_target_dependencies(listener rclcpp rclcpp_components std_msgs) +rclcpp_components_register_nodes(listener Listener) add_executable(wait_set src/wait_set.cpp) ament_target_dependencies(wait_set example_interfaces rclcpp std_msgs) @@ -53,7 +51,7 @@ 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") +ament_target_dependencies(wait_set_composed rclcpp) install(TARGETS wait_set From 53212bfbff43ee08d13cc8dcead590f947445fd7 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 09:14:53 +0200 Subject: [PATCH 44/59] Remove comment about simulating inter-process communications Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/wait_set/src/executor_random_order.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rclcpp/wait_set/src/executor_random_order.cpp b/rclcpp/wait_set/src/executor_random_order.cpp index 20a00b65..c2dfb907 100644 --- a/rclcpp/wait_set/src/executor_random_order.cpp +++ b/rclcpp/wait_set/src/executor_random_order.cpp @@ -30,8 +30,6 @@ int32_t main(const int32_t argc, char ** const argv) { rclcpp::init(argc, argv); - // We run the talker and the listener in different threads to simulate inter-process - // communications. // 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 From 79711861bf3d749226705e6dd5af0d7e37a9016c Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 13:24:57 +0200 Subject: [PATCH 45/59] Add joinable Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/static_wait_set.cpp | 4 +++- rclcpp/minimal_subscriber/wait_set.cpp | 4 +++- rclcpp/wait_set/include/wait_set/listener.hpp | 2 +- rclcpp/wait_set/src/listener.cpp | 4 +++- 4 files changed, 10 insertions(+), 4 deletions(-) diff --git a/rclcpp/minimal_subscriber/static_wait_set.cpp b/rclcpp/minimal_subscriber/static_wait_set.cpp index 5842ecc3..52e7bcc1 100644 --- a/rclcpp/minimal_subscriber/static_wait_set.cpp +++ b/rclcpp/minimal_subscriber/static_wait_set.cpp @@ -49,7 +49,9 @@ class MinimalSubscriber : public rclcpp::Node ~MinimalSubscriber() { - thread_.join(); + if (thread_.joinable()) { + thread_.join(); + } } void spin_wait_set() diff --git a/rclcpp/minimal_subscriber/wait_set.cpp b/rclcpp/minimal_subscriber/wait_set.cpp index 380323e1..fb1213a2 100644 --- a/rclcpp/minimal_subscriber/wait_set.cpp +++ b/rclcpp/minimal_subscriber/wait_set.cpp @@ -46,7 +46,9 @@ class MinimalSubscriber : public rclcpp::Node ~MinimalSubscriber() { - thread_.join(); + if (thread_.joinable()) { + thread_.join(); + } } void spin_wait_set() diff --git a/rclcpp/wait_set/include/wait_set/listener.hpp b/rclcpp/wait_set/include/wait_set/listener.hpp index 6ae55491..f4d9d7c5 100644 --- a/rclcpp/wait_set/include/wait_set/listener.hpp +++ b/rclcpp/wait_set/include/wait_set/listener.hpp @@ -23,7 +23,7 @@ class Listener : public rclcpp::Node { public: WAIT_SET_PUBLIC explicit Listener(rclcpp::NodeOptions options); - WAIT_SET_PUBLIC ~Listener(); + WAIT_SET_PUBLIC ~Listener() override; private: void spin_wait_set(); diff --git a/rclcpp/wait_set/src/listener.cpp b/rclcpp/wait_set/src/listener.cpp index 52cec13a..e56bda06 100644 --- a/rclcpp/wait_set/src/listener.cpp +++ b/rclcpp/wait_set/src/listener.cpp @@ -51,7 +51,9 @@ Listener::Listener(rclcpp::NodeOptions options) Listener::~Listener() { - thread_.join(); + if (thread_.joinable()) { + thread_.join(); + } } void Listener::spin_wait_set() From 4eb028aef7614d0d465b57aacd0b3cb61c7cc125 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Fri, 9 Jul 2021 13:32:38 +0200 Subject: [PATCH 46/59] README.md format Signed-off-by: Carlos San Vicente Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 314417b0..452464e3 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -4,10 +4,9 @@ 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.cpp` uses a `rclcpp::WaitSet` to wait and poll for data - * `wait_set.cpp` uses a `rclcpp::StaticWaitSet` to wait and poll for data - * `wait_set_time_triggered.cpp` uses a `rclcpp::Waitset` and a timer to poll - for data periodically + * `wait_set.cpp` uses a `rclcpp::WaitSet` to wait and poll for data + * `static_wait_set.cpp` uses a `rclcpp::StaticWaitSet` to wait and poll for data + * `wait_set_time_triggered.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. From 06caa2a95da89ad517ddd22b80e78c794781d0a5 Mon Sep 17 00:00:00 2001 From: carlossvg Date: Tue, 20 Jul 2021 12:36:41 +0200 Subject: [PATCH 47/59] Update rclcpp/minimal_subscriber/static_wait_set.cpp Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/static_wait_set.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/minimal_subscriber/static_wait_set.cpp b/rclcpp/minimal_subscriber/static_wait_set.cpp index 52e7bcc1..2d7783c3 100644 --- a/rclcpp/minimal_subscriber/static_wait_set.cpp +++ b/rclcpp/minimal_subscriber/static_wait_set.cpp @@ -18,8 +18,8 @@ #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 and handle - * messages from the subscriber */ +/* 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 MinimalSubscriber : public rclcpp::Node { From 642ecf630c79e508eeb01f6d0de2ba44d34750f5 Mon Sep 17 00:00:00 2001 From: carlossvg Date: Tue, 20 Jul 2021 12:37:34 +0200 Subject: [PATCH 48/59] Update rclcpp/minimal_subscriber/wait_set.cpp Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/wait_set.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/minimal_subscriber/wait_set.cpp b/rclcpp/minimal_subscriber/wait_set.cpp index fb1213a2..43f011a9 100644 --- a/rclcpp/minimal_subscriber/wait_set.cpp +++ b/rclcpp/minimal_subscriber/wait_set.cpp @@ -18,8 +18,8 @@ #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 and handle - * messages from the subscriber */ +/* 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 MinimalSubscriber : public rclcpp::Node { From 07679a487a1501d333268a00f26fc2aa1e9b1691 Mon Sep 17 00:00:00 2001 From: carlossvg Date: Tue, 20 Jul 2021 12:37:45 +0200 Subject: [PATCH 49/59] Update rclcpp/minimal_subscriber/wait_set_time_triggered.cpp Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/wait_set_time_triggered.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp b/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp index fd9d19c7..3f5193b6 100644 --- a/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp +++ b/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp @@ -20,7 +20,7 @@ using namespace std::chrono_literals; -/* This example creates a subclass of Node and uses a wait-set based loop and polls periodically +/* 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 MinimalSubscriber : public rclcpp::Node From 555a590fab4333d0df1e7dfb9c6c33d66b32b7e8 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 13:19:38 +0200 Subject: [PATCH 50/59] Adress reviewer comments for minimal_subscriber Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/minimal_subscriber/static_wait_set.cpp | 91 ---------------- .../topics/minimal_subscriber/CMakeLists.txt | 18 ++-- .../static_wait_set_subscriber.cpp | 100 ++++++++++++++++++ .../time_triggered_wait_set_subscriber.cpp} | 33 +++--- .../wait_set_subscriber.cpp} | 39 ++++--- 5 files changed, 151 insertions(+), 130 deletions(-) delete mode 100644 rclcpp/minimal_subscriber/static_wait_set.cpp create mode 100644 rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp rename rclcpp/{minimal_subscriber/wait_set_time_triggered.cpp => topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp} (76%) rename rclcpp/{minimal_subscriber/wait_set.cpp => topics/minimal_subscriber/wait_set_subscriber.cpp} (70%) diff --git a/rclcpp/minimal_subscriber/static_wait_set.cpp b/rclcpp/minimal_subscriber/static_wait_set.cpp deleted file mode 100644 index 2d7783c3..00000000 --- a/rclcpp/minimal_subscriber/static_wait_set.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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 MinimalSubscriber : public rclcpp::Node -{ -public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - auto options = rclcpp::SubscriptionOptions(); - 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, - options); - - wait_set_ptr_ = std::make_shared>( - std::array::SubscriptionEntry, 1>{{{subscription_}}}); - - thread_ = std::thread([this]() -> void {spin_wait_set();}); - } - - ~MinimalSubscriber() - { - 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_ptr_->wait(std::chrono::milliseconds(501)); - 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 (subscription_->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - subscription_->handle_message(type_erased_msg, msg_info); - } - } - } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - if (rclcpp::ok()) { - RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time"); - } - } - } - } - -private: - rclcpp::Subscription::SharedPtr subscription_; - std::shared_ptr> wait_set_ptr_; - std::thread thread_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index bedb1f46..1b959a4a 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -32,14 +32,14 @@ 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_executable(subscriber_wait_set wait_set.cpp) -ament_target_dependencies(subscriber_wait_set rclcpp std_msgs) +add_executable(wait_set_subscriber wait_set_subscriber.cpp) +ament_target_dependencies(wait_set_subscriber rclcpp std_msgs) -add_executable(subscriber_static_wait_set static_wait_set.cpp) -ament_target_dependencies(subscriber_static_wait_set rclcpp std_msgs) +add_executable(static_wait_set_subscriber static_wait_set_subscriber.cpp) +ament_target_dependencies(static_wait_set_subscriber rclcpp std_msgs) -add_executable(subscriber_wait_set_time_triggered wait_set_time_triggered.cpp) -ament_target_dependencies(subscriber_wait_set_time_triggered rclcpp std_msgs) +add_executable(time_triggered_wait_set_subscriber time_triggered_wait_set_subscriber.cpp) +ament_target_dependencies(time_triggered_wait_set_subscriber rclcpp std_msgs) install(TARGETS subscriber_lambda @@ -48,9 +48,9 @@ install(TARGETS subscriber_member_function_with_type_adapter subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable - subscriber_wait_set - subscriber_static_wait_set - subscriber_wait_set_time_triggered + wait_set_subscriber + static_wait_set_subscriber + time_triggered_wait_set_subscriber DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) 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..7864b740 --- /dev/null +++ b/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp @@ -0,0 +1,100 @@ +// 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: + StaticWaitSetSubscriber() + : Node("static_wait_set_subscriber"), + 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 options = rclcpp::SubscriptionOptions(); + 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, + 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_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp similarity index 76% rename from rclcpp/minimal_subscriber/wait_set_time_triggered.cpp rename to rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp index 3f5193b6..cad4dde5 100644 --- a/rclcpp/minimal_subscriber/wait_set_time_triggered.cpp +++ b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp @@ -23,11 +23,11 @@ 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 MinimalSubscriber : public rclcpp::Node +class TimeTriggeredWaitSetSubscriber : public rclcpp::Node { public: - MinimalSubscriber() - : Node("minimal_subscriber") + TimeTriggeredWaitSetSubscriber() + : Node("time_triggered_wait_set_subscriber") { rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -59,7 +59,7 @@ class MinimalSubscriber : public rclcpp::Node thread_ = std::thread([this]() -> void {spin_wait_set();}); } - ~MinimalSubscriber() + ~TimeTriggeredWaitSetSubscriber() { thread_.join(); } @@ -69,14 +69,21 @@ class MinimalSubscriber : public rclcpp::Node 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); - if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - if (wait_result.get_wait_set().get_rcl_wait_set().timers[0U]) { - timer_->execute_callback(); - } - } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - if (rclcpp::ok()) { - RCLCPP_ERROR(this->get_logger(), "Wait-set failed with timeout"); - } + 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."); } } } @@ -91,7 +98,7 @@ class MinimalSubscriber : public rclcpp::Node int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/rclcpp/minimal_subscriber/wait_set.cpp b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp similarity index 70% rename from rclcpp/minimal_subscriber/wait_set.cpp rename to rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp index 43f011a9..23b05605 100644 --- a/rclcpp/minimal_subscriber/wait_set.cpp +++ b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp @@ -21,11 +21,11 @@ /* 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 MinimalSubscriber : public rclcpp::Node +class WaitSetSubscriber : public rclcpp::Node { public: - MinimalSubscriber() - : Node("minimal_subscriber") + WaitSetSubscriber() + : Node("wait_set_subscriber") { rclcpp::CallbackGroup::SharedPtr cb_group_waitset = this->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -44,7 +44,7 @@ class MinimalSubscriber : public rclcpp::Node thread_ = std::thread([this]() -> void {spin_wait_set();}); } - ~MinimalSubscriber() + ~WaitSetSubscriber() { if (thread_.joinable()) { thread_.join(); @@ -56,19 +56,24 @@ class MinimalSubscriber : public rclcpp::Node 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)); - 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 (subscription_->take(msg, msg_info)) { - std::shared_ptr type_erased_msg = std::make_shared(msg); - subscription_->handle_message(type_erased_msg, msg_info); + 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; } - } - } else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) { - if (rclcpp::ok()) { - RCLCPP_WARN(this->get_logger(), "Timeout. No message received after given wait-time"); - } + 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."); } } } @@ -82,7 +87,7 @@ class MinimalSubscriber : public rclcpp::Node int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } From 4ec7f9f7b1a4e5b37e3cb0101af31590f963e085 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 15:32:59 +0200 Subject: [PATCH 51/59] Convert wait-set minimal subscribers to components Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- .../topics/minimal_subscriber/CMakeLists.txt | 30 +++++++++++++------ .../static_wait_set_subscriber.cpp | 20 +++++-------- .../time_triggered_wait_set_subscriber.cpp | 20 +++++-------- .../wait_set_subscriber.cpp | 20 +++++-------- 4 files changed, 45 insertions(+), 45 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index 1b959a4a..e271e5ff 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,14 +33,28 @@ 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_executable(wait_set_subscriber wait_set_subscriber.cpp) -ament_target_dependencies(wait_set_subscriber rclcpp std_msgs) +add_library(wait_set_subscriber SHARED wait_set_subscriber.cpp) +target_compile_definitions(wait_set_subscriber PRIVATE WAIT_SET_DLL) +ament_target_dependencies(wait_set_subscriber rclcpp rclcpp_components std_msgs) +rclcpp_components_register_nodes(wait_set_subscriber WaitSetSubscriber) -add_executable(static_wait_set_subscriber static_wait_set_subscriber.cpp) -ament_target_dependencies(static_wait_set_subscriber rclcpp std_msgs) +add_library(static_wait_set_subscriber SHARED static_wait_set_subscriber.cpp) +target_compile_definitions(static_wait_set_subscriber PRIVATE WAIT_SET_DLL) +ament_target_dependencies(static_wait_set_subscriber rclcpp rclcpp_components std_msgs) +rclcpp_components_register_nodes(static_wait_set_subscriber StaticWaitSetSubscriber) -add_executable(time_triggered_wait_set_subscriber time_triggered_wait_set_subscriber.cpp) -ament_target_dependencies(time_triggered_wait_set_subscriber rclcpp std_msgs) +add_library(time_triggered_wait_set_subscriber SHARED time_triggered_wait_set_subscriber.cpp) +target_compile_definitions(time_triggered_wait_set_subscriber PRIVATE WAIT_SET_DLL) +ament_target_dependencies(time_triggered_wait_set_subscriber rclcpp rclcpp_components std_msgs) +rclcpp_components_register_nodes(time_triggered_wait_set_subscriber TimeTriggeredWaitSetSubscriber) + +install(TARGETS + wait_set_subscriber + time_triggered_wait_set_subscriber + static_wait_set_subscriber + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) install(TARGETS subscriber_lambda @@ -48,9 +63,6 @@ install(TARGETS subscriber_member_function_with_type_adapter subscriber_member_function_with_unique_network_flow_endpoints subscriber_not_composable - wait_set_subscriber - static_wait_set_subscriber - time_triggered_wait_set_subscriber DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp index 7864b740..18eeb72e 100644 --- a/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp @@ -26,16 +26,16 @@ class StaticWaitSetSubscriber : public rclcpp::Node using MyStaticWaitSet = rclcpp::StaticWaitSet<1, 0, 0, 0, 0, 0>; public: - StaticWaitSetSubscriber() - : Node("static_wait_set_subscriber"), + 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 options = rclcpp::SubscriptionOptions(); - options.callback_group = cb_group_waitset; + 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()); }; @@ -43,7 +43,7 @@ class StaticWaitSetSubscriber : public rclcpp::Node "topic", 10, subscription_callback, - options); + subscription_options); } () ), wait_set_(std::array{{{subscription_}}}), @@ -91,10 +91,6 @@ class StaticWaitSetSubscriber : public rclcpp::Node std::thread thread_; }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(StaticWaitSetSubscriber) \ No newline at end of file diff --git a/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp index cad4dde5..20b69f4e 100644 --- a/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp @@ -26,14 +26,14 @@ using namespace std::chrono_literals; class TimeTriggeredWaitSetSubscriber : public rclcpp::Node { public: - TimeTriggeredWaitSetSubscriber() - : Node("time_triggered_wait_set_subscriber") + 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 options = rclcpp::SubscriptionOptions(); - options.callback_group = cb_group_waitset; + 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()); }; @@ -42,7 +42,7 @@ class TimeTriggeredWaitSetSubscriber : public rclcpp::Node "topic", 10, subscription_callback, - options); + subscription_options); auto timer_callback = [this]() -> void { std_msgs::msg::String msg; @@ -95,10 +95,6 @@ class TimeTriggeredWaitSetSubscriber : public rclcpp::Node std::thread thread_; }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#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 index 23b05605..3987ce07 100644 --- a/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp @@ -24,13 +24,13 @@ class WaitSetSubscriber : public rclcpp::Node { public: - WaitSetSubscriber() - : Node("wait_set_subscriber") + 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 options = rclcpp::SubscriptionOptions(); - options.callback_group = cb_group_waitset; + 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()); }; @@ -39,7 +39,7 @@ class WaitSetSubscriber : public rclcpp::Node "topic", 10, subscription_callback, - options); + subscription_options); wait_set_.add_subscription(subscription_); thread_ = std::thread([this]() -> void {spin_wait_set();}); } @@ -84,10 +84,6 @@ class WaitSetSubscriber : public rclcpp::Node std::thread thread_; }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(WaitSetSubscriber) From 44f2239697ee615a15e8ab322f14f2ff83c2eb7c Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 15:43:13 +0200 Subject: [PATCH 52/59] Add note in README for wait-set use cases Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 452464e3..a8f78efd 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -15,3 +15,10 @@ 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_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.cpp`, `static_wait_set.cpp` and `wait_set_time_triggered.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, create custom triggering conditions or use the timeouts provided by the +wait-sets. \ No newline at end of file From 8831dddfda784d7dcd03ea19245a1258fd7cde16 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 17:16:33 +0200 Subject: [PATCH 53/59] Restore rclcpp/README.md modified indent Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/README.md b/rclcpp/README.md index 133defd6..35b81da1 100644 --- a/rclcpp/README.md +++ b/rclcpp/README.md @@ -1,4 +1,4 @@ # rclcpp examples This directory contains many examples of how to do common tasks with rclcpp. -The intent is that this material will be easy to copy-and-paste into your own projects. \ No newline at end of file +The intent is that this material will be easy to copy-and-paste into your own projects. From 6027fb73b460d648e9b11b6287821620a078b33f Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 17:25:00 +0200 Subject: [PATCH 54/59] Remove dll compile definitions Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index e271e5ff..69aa659c 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -34,17 +34,14 @@ add_executable(subscriber_not_composable not_composable.cpp) ament_target_dependencies(subscriber_not_composable rclcpp std_msgs) add_library(wait_set_subscriber SHARED wait_set_subscriber.cpp) -target_compile_definitions(wait_set_subscriber PRIVATE WAIT_SET_DLL) ament_target_dependencies(wait_set_subscriber rclcpp rclcpp_components std_msgs) rclcpp_components_register_nodes(wait_set_subscriber WaitSetSubscriber) add_library(static_wait_set_subscriber SHARED static_wait_set_subscriber.cpp) -target_compile_definitions(static_wait_set_subscriber PRIVATE WAIT_SET_DLL) ament_target_dependencies(static_wait_set_subscriber rclcpp rclcpp_components std_msgs) rclcpp_components_register_nodes(static_wait_set_subscriber StaticWaitSetSubscriber) add_library(time_triggered_wait_set_subscriber SHARED time_triggered_wait_set_subscriber.cpp) -target_compile_definitions(time_triggered_wait_set_subscriber PRIVATE WAIT_SET_DLL) ament_target_dependencies(time_triggered_wait_set_subscriber rclcpp rclcpp_components std_msgs) rclcpp_components_register_nodes(time_triggered_wait_set_subscriber TimeTriggeredWaitSetSubscriber) From b747282bf3702233fa093b106ab372ee3cf0143c Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Tue, 20 Jul 2021 17:37:11 +0200 Subject: [PATCH 55/59] Minor fixes Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 19 ++++++++++--------- rclcpp/topics/minimal_subscriber/package.xml | 2 ++ .../static_wait_set_subscriber.cpp | 2 +- .../time_triggered_wait_set_subscriber.cpp | 2 -- .../wait_set_subscriber.cpp | 1 - rclcpp/wait_set/CMakeLists.txt | 8 ++++++++ rclcpp/wait_set/package.xml | 1 + 7 files changed, 22 insertions(+), 13 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index a8f78efd..4075613f 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -4,9 +4,10 @@ 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.cpp` uses a `rclcpp::WaitSet` to wait and poll for data - * `static_wait_set.cpp` uses a `rclcpp::StaticWaitSet` to wait and poll for data - * `wait_set_time_triggered.cpp` uses a `rclcpp::Waitset` and a timer to poll for data periodically + * `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. @@ -16,9 +17,9 @@ All of these nodes do the same thing: they create a node called `minimal_subscri 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.cpp`, `static_wait_set.cpp` and `wait_set_time_triggered.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, create custom triggering conditions or use the timeouts provided by the -wait-sets. \ No newline at end of file +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. \ No newline at end of file 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 index 18eeb72e..d72c7ab3 100644 --- a/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/static_wait_set_subscriber.cpp @@ -93,4 +93,4 @@ class StaticWaitSetSubscriber : public rclcpp::Node #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(StaticWaitSetSubscriber) \ No newline at end of file +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 index 20b69f4e..7b5d85dc 100644 --- a/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/time_triggered_wait_set_subscriber.cpp @@ -37,13 +37,11 @@ class TimeTriggeredWaitSetSubscriber : public rclcpp::Node 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; diff --git a/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp index 3987ce07..79dddce2 100644 --- a/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp +++ b/rclcpp/topics/minimal_subscriber/wait_set_subscriber.cpp @@ -34,7 +34,6 @@ class WaitSetSubscriber : public rclcpp::Node 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, diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index 0ae62e45..eed3ff51 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -53,6 +53,14 @@ 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 diff --git a/rclcpp/wait_set/package.xml b/rclcpp/wait_set/package.xml index da971efb..74d0c554 100644 --- a/rclcpp/wait_set/package.xml +++ b/rclcpp/wait_set/package.xml @@ -18,6 +18,7 @@ example_interfaces rclcpp + rclcpp_components std_msgs From cf714a98bb461d45921e6475eb9a5556a05ec9f2 Mon Sep 17 00:00:00 2001 From: Carlos San Vicente Date: Thu, 22 Jul 2021 09:35:42 +0200 Subject: [PATCH 56/59] Use rclcpp_components_register_node to register components Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- .../topics/minimal_subscriber/CMakeLists.txt | 29 +++++++++++-------- rclcpp/wait_set/CMakeLists.txt | 10 +++++-- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/CMakeLists.txt b/rclcpp/topics/minimal_subscriber/CMakeLists.txt index 69aa659c..2edca97f 100644 --- a/rclcpp/topics/minimal_subscriber/CMakeLists.txt +++ b/rclcpp/topics/minimal_subscriber/CMakeLists.txt @@ -33,22 +33,27 @@ 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 SHARED wait_set_subscriber.cpp) -ament_target_dependencies(wait_set_subscriber rclcpp rclcpp_components std_msgs) -rclcpp_components_register_nodes(wait_set_subscriber WaitSetSubscriber) -add_library(static_wait_set_subscriber SHARED static_wait_set_subscriber.cpp) -ament_target_dependencies(static_wait_set_subscriber rclcpp rclcpp_components std_msgs) -rclcpp_components_register_nodes(static_wait_set_subscriber StaticWaitSetSubscriber) +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) -add_library(time_triggered_wait_set_subscriber SHARED time_triggered_wait_set_subscriber.cpp) -ament_target_dependencies(time_triggered_wait_set_subscriber rclcpp rclcpp_components std_msgs) -rclcpp_components_register_nodes(time_triggered_wait_set_subscriber TimeTriggeredWaitSetSubscriber) +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 - time_triggered_wait_set_subscriber - static_wait_set_subscriber + wait_set_subscriber_library ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin) diff --git a/rclcpp/wait_set/CMakeLists.txt b/rclcpp/wait_set/CMakeLists.txt index eed3ff51..db6c6c6f 100644 --- a/rclcpp/wait_set/CMakeLists.txt +++ b/rclcpp/wait_set/CMakeLists.txt @@ -21,12 +21,18 @@ 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_nodes(talker Talker) +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_nodes(listener Listener) +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) From 1dff75f076621ad33ead7626c08dbd52563bc85e Mon Sep 17 00:00:00 2001 From: carlossvg Date: Thu, 22 Jul 2021 11:15:18 +0200 Subject: [PATCH 57/59] Update rclcpp/topics/minimal_subscriber/README.md Co-authored-by: William Woodall Signed-off-by: Carlos San Vicente Signed-off-by: William Woodall --- rclcpp/topics/minimal_subscriber/README.md | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rclcpp/topics/minimal_subscriber/README.md b/rclcpp/topics/minimal_subscriber/README.md index 4075613f..5caa8006 100644 --- a/rclcpp/topics/minimal_subscriber/README.md +++ b/rclcpp/topics/minimal_subscriber/README.md @@ -17,9 +17,6 @@ All of these nodes do the same thing: they create a node called `minimal_subscri 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. \ No newline at end of file +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. From 7d63434a81bea1dbf7c486c88af5689470096a1b Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 23 Jul 2021 16:21:22 -0700 Subject: [PATCH 58/59] fix signed converison or potential loss of data warnings Signed-off-by: William Woodall --- rclcpp/wait_set/include/wait_set/random_talker.hpp | 7 +++++-- rclcpp/wait_set/src/wait_set_random_order.cpp | 4 ++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/rclcpp/wait_set/include/wait_set/random_talker.hpp b/rclcpp/wait_set/include/wait_set/random_talker.hpp index 1e5016e1..8cf1316b 100644 --- a/rclcpp/wait_set/include/wait_set/random_talker.hpp +++ b/rclcpp/wait_set/include/wait_set/random_talker.hpp @@ -15,8 +15,9 @@ #ifndef WAIT_SET__RANDOM_TALKER_HPP_ #define WAIT_SET__RANDOM_TALKER_HPP_ -#include +#include #include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -29,7 +30,9 @@ class RandomTalker : public rclcpp::Node pub1_(this->create_publisher("topicA", 10)), pub2_(this->create_publisher("topicB", 10)), pub3_(this->create_publisher("topicC", 10)), - rand_engine_(std::chrono::system_clock::now().time_since_epoch().count()) + rand_engine_(static_cast( + std::abs(std::chrono::system_clock::now().time_since_epoch().count()) + )) { publish_functions_.emplace_back( ([this]() { diff --git a/rclcpp/wait_set/src/wait_set_random_order.cpp b/rclcpp/wait_set/src/wait_set_random_order.cpp index 296a2c70..a71d5a82 100644 --- a/rclcpp/wait_set/src/wait_set_random_order.cpp +++ b/rclcpp/wait_set/src/wait_set_random_order.cpp @@ -55,8 +55,8 @@ int32_t main(const int32_t argc, char ** const argv) if (sub1_has_data && sub2_has_data && sub3_has_data) { std_msgs::msg::String msg; rclcpp::MessageInfo msg_info; - const int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; - for (int i = 0; i < subscriptions_num; i++) { + 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); From de2962fda0107d55f7c85c6ee508b4799718960e Mon Sep 17 00:00:00 2001 From: William Woodall Date: Fri, 30 Jul 2021 17:48:15 -0700 Subject: [PATCH 59/59] avoid conversion warnings by using size_t everywhere Signed-off-by: William Woodall --- rclcpp/wait_set/src/static_wait_set.cpp | 16 ++++++++-------- rclcpp/wait_set/src/thread_safe_wait_set.cpp | 16 ++++++++-------- rclcpp/wait_set/src/wait_set.cpp | 16 ++++++++-------- 3 files changed, 24 insertions(+), 24 deletions(-) diff --git a/rclcpp/wait_set/src/static_wait_set.cpp b/rclcpp/wait_set/src/static_wait_set.cpp index 71b66a1d..79ba4d1a 100644 --- a/rclcpp/wait_set/src/static_wait_set.cpp +++ b/rclcpp/wait_set/src/static_wait_set.cpp @@ -43,25 +43,25 @@ int main(int argc, char * argv[]) RCLCPP_INFO(node->get_logger(), "Waiting..."); auto wait_result = static_wait_set.wait(std::chrono::seconds(1)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - int guard_conditions_num = static_wait_set.get_rcl_wait_set().size_of_guard_conditions; - int subscriptions_num = static_wait_set.get_rcl_wait_set().size_of_subscriptions; + 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 (int i = 0; i < guard_conditions_num; i++) { + 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 %d triggered", i + 1); + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); } } - for (int i = 0; i < subscriptions_num; i++) { + 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 %d triggered", i + 1); + 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 %d: I heard '%s'", i + 1, msg.data.c_str()); + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i + 1); + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); } } } diff --git a/rclcpp/wait_set/src/thread_safe_wait_set.cpp b/rclcpp/wait_set/src/thread_safe_wait_set.cpp index 2153c553..b0543e11 100644 --- a/rclcpp/wait_set/src/thread_safe_wait_set.cpp +++ b/rclcpp/wait_set/src/thread_safe_wait_set.cpp @@ -48,25 +48,25 @@ int main(int argc, char * argv[]) RCLCPP_INFO(node->get_logger(), "Waiting..."); auto wait_result = wait_set.wait(std::chrono::seconds(1)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; - int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + 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 (int i = 0; i < guard_conditions_num; i++) { + 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 %d triggered", i + 1); + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); } } - for (int i = 0; i < subscriptions_num; i++) { + 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 %d triggered", i + 1); + 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 %d: I heard '%s'", i + 1, msg.data.c_str()); + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i + 1); + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); } } } diff --git a/rclcpp/wait_set/src/wait_set.cpp b/rclcpp/wait_set/src/wait_set.cpp index 9e206c79..b6422925 100644 --- a/rclcpp/wait_set/src/wait_set.cpp +++ b/rclcpp/wait_set/src/wait_set.cpp @@ -48,25 +48,25 @@ int main(int argc, char * argv[]) RCLCPP_INFO(node->get_logger(), "Waiting..."); auto wait_result = wait_set.wait(std::chrono::seconds(1)); if (wait_result.kind() == rclcpp::WaitResultKind::Ready) { - int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions; - int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions; + 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 (int i = 0; i < guard_conditions_num; i++) { + 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 %d triggered", i + 1); + RCLCPP_INFO(node->get_logger(), "guard_condition %zu triggered", i + 1); } } - for (int i = 0; i < subscriptions_num; i++) { + 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 %d triggered", i + 1); + 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 %d: I heard '%s'", i + 1, msg.data.c_str()); + "subscription %zu: I heard '%s'", i + 1, msg.data.c_str()); } else { - RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i + 1); + RCLCPP_INFO(node->get_logger(), "subscription %zu: No message", i + 1); } } }