Skip to content

Commit 57148df

Browse files
carlos-apexcarlos-apex
committed
Refactor wait-set policies examples
Signed-off-by: Carlos San Vicente <[email protected]>
1 parent 9864211 commit 57148df

File tree

2 files changed

+133
-85
lines changed

2 files changed

+133
-85
lines changed

‎rclcpp/wait_set/static_wait_set.cpp‎

Lines changed: 64 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2020 Open Source Robotics Foundation, Inc.
1+
// Copyright 2021 Open Source Robotics Foundation, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -13,6 +13,7 @@
1313
// limitations under the License.
1414

1515
#include <memory>
16+
#include <vector>
1617

1718
#include "rclcpp/rclcpp.hpp"
1819
#include "std_msgs/msg/string.hpp"
@@ -21,28 +22,74 @@ int main(int argc, char * argv[])
2122
{
2223
rclcpp::init(argc, argv);
2324

24-
// auto do_nothing = [](std_msgs::msg::String::UniquePtr){};
25+
auto node = std::make_shared<rclcpp::Node>("static_wait_set_example_node");
2526

26-
auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
27+
auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);};
28+
auto sub1 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
29+
auto sub2 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
30+
std::vector<decltype(sub1)> sub_vector = {sub1, sub2};
31+
auto guard_condition1 = std::make_shared<rclcpp::GuardCondition>();
2732
auto guard_condition2 = std::make_shared<rclcpp::GuardCondition>();
2833

29-
rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0> static_wait_set(
30-
std::array<rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>::SubscriptionEntry, 0>{},
31-
std::array<rclcpp::GuardCondition::SharedPtr, 1>{{guard_condition}},
34+
rclcpp::StaticWaitSet<2, 2, 0, 0, 0, 0> static_wait_set(
35+
std::array<rclcpp::StaticWaitSet<2, 2, 0, 0, 0, 0>::SubscriptionEntry, 2>{{{sub1}, {sub2}}},
36+
std::array<rclcpp::GuardCondition::SharedPtr, 2>{{{guard_condition1}, {guard_condition2}}},
3237
std::array<rclcpp::TimerBase::SharedPtr, 0>{},
3338
std::array<rclcpp::ClientBase::SharedPtr, 0>{},
3439
std::array<rclcpp::ServiceBase::SharedPtr, 0>{},
35-
std::array<rclcpp::StaticWaitSet<0, 1, 0, 0, 0, 0>::WaitableEntry, 0>{});
36-
// Note: The following line will result in a compiler error, since the
37-
// static storage policy prevents editing after construction.
38-
// static_wait_set.add_guard_condition(guard_condition2);
39-
// static_wait_set.remove_guard_condition(guard_condition2);
40-
(void)guard_condition2;
41-
42-
{
43-
auto wait_result = static_wait_set.wait(std::chrono::seconds(1));
44-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
45-
}
40+
std::array<rclcpp::StaticWaitSet<2, 2, 0, 0, 0, 0>::WaitableEntry, 0>{});
41+
42+
auto wait_and_print_results = [&]() {
43+
RCLCPP_INFO(node->get_logger(), "Waiting...");
44+
auto wait_result = static_wait_set.wait(std::chrono::seconds(1));
45+
if (wait_result.kind() == rclcpp::WaitResultKind::Ready) {
46+
int guard_conditions_num = static_wait_set.get_rcl_wait_set().size_of_guard_conditions;
47+
int subscriptions_num = static_wait_set.get_rcl_wait_set().size_of_subscriptions;
48+
49+
for (int i = 0; i < guard_conditions_num; i++) {
50+
if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[i]) {
51+
RCLCPP_INFO(node->get_logger(), "guard_condition %d triggered", i + 1);
52+
}
53+
}
54+
for (int i = 0; i < subscriptions_num; i++) {
55+
if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) {
56+
RCLCPP_INFO(node->get_logger(), "subscription %d triggered", i + 1);
57+
std_msgs::msg::String msg;
58+
rclcpp::MessageInfo msg_info;
59+
if (sub_vector.at(i)->take(msg, msg_info)) {
60+
RCLCPP_INFO(
61+
node->get_logger(),
62+
"subscription %d: I heard '%s'", i + 1, msg.data.c_str());
63+
} else {
64+
RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i + 1);
65+
}
66+
}
67+
}
68+
} else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) {
69+
RCLCPP_INFO(node->get_logger(), "wait-set waiting failed with timeout");
70+
} else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) {
71+
RCLCPP_INFO(node->get_logger(), "wait-set waiting failed because wait-set is empty");
72+
}
73+
};
74+
75+
RCLCPP_INFO(node->get_logger(), "Action: Nothing triggered");
76+
wait_and_print_results();
77+
78+
RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 1");
79+
guard_condition1->trigger();
80+
wait_and_print_results();
81+
82+
RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 2");
83+
guard_condition2->trigger();
84+
wait_and_print_results();
85+
86+
RCLCPP_INFO(node->get_logger(), "Action: Message published");
87+
auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 1);
88+
pub->publish(std_msgs::msg::String().set__data("test"));
89+
wait_and_print_results();
90+
91+
// Note the static wait-set does not allow adding or removing entities dynamically.
92+
// It will result in a compilation error.
4693

4794
return 0;
4895
}

‎rclcpp/wait_set/thread_safe_wait_set.cpp‎

Lines changed: 69 additions & 68 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2020 Open Source Robotics Foundation, Inc.
1+
// Copyright 2021 Open Source Robotics Foundation, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -15,96 +15,97 @@
1515
#include <cassert>
1616
#include <memory>
1717
#include <vector>
18-
// #include <string>
19-
// #include <thread>
2018

21-
#include "example_interfaces/srv/add_two_ints.hpp"
2219
#include "rclcpp/rclcpp.hpp"
2320
#include "std_msgs/msg/string.hpp"
2421

2522
int main(int argc, char * argv[])
2623
{
2724
rclcpp::init(argc, argv);
2825

29-
auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);};
30-
3126
auto node = std::make_shared<rclcpp::Node>("wait_set_example_node");
3227

33-
auto guard_condition = std::make_shared<rclcpp::GuardCondition>();
28+
auto do_nothing = [](std_msgs::msg::String::UniquePtr) {assert(false);};
29+
auto sub1 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
30+
auto sub2 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
31+
std::vector<decltype(sub1)> sub_vector = {sub1, sub2};
32+
auto guard_condition1 = std::make_shared<rclcpp::GuardCondition>();
3433
auto guard_condition2 = std::make_shared<rclcpp::GuardCondition>();
3534

36-
auto sub = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing);
37-
rclcpp::SubscriptionOptions so;
38-
so.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable;
39-
auto sub2 = node->create_subscription<std_msgs::msg::String>("~/chatter", 10, do_nothing, so);
40-
35+
// FIXME: removing sub if it was added in the ctor leads to a failure
36+
// terminate called after throwing an instance of 'std::runtime_error'
37+
// what(): waitable not in wait set
4138
rclcpp::ThreadSafeWaitSet wait_set(
42-
std::vector<rclcpp::ThreadSafeWaitSet::SubscriptionEntry>{{sub}},
43-
std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition});
39+
// std::vector<rclcpp::WaitSet::SubscriptionEntry>{{sub}},
40+
{},
41+
std::vector<rclcpp::GuardCondition::SharedPtr>{guard_condition1});
42+
43+
wait_set.add_subscription(sub1); // FIXME: add it in the ctor
4444
wait_set.add_subscription(sub2);
4545
wait_set.add_guard_condition(guard_condition2);
4646

47-
{
47+
auto wait_and_print_results = [&]() {
48+
RCLCPP_INFO(node->get_logger(), "Waiting...");
4849
auto wait_result = wait_set.wait(std::chrono::seconds(1));
49-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
50-
}
51-
52-
guard_condition->trigger();
53-
54-
{
55-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
56-
assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
57-
assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[0] != nullptr);
58-
assert(wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[1] == nullptr);
59-
assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] == nullptr);
60-
assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[1] == nullptr);
61-
}
50+
if (wait_result.kind() == rclcpp::WaitResultKind::Ready) {
51+
int guard_conditions_num = wait_set.get_rcl_wait_set().size_of_guard_conditions;
52+
int subscriptions_num = wait_set.get_rcl_wait_set().size_of_subscriptions;
53+
54+
for (int i=0; i<guard_conditions_num; i++) {
55+
if (wait_result.get_wait_set().get_rcl_wait_set().guard_conditions[i]) {
56+
RCLCPP_INFO(node->get_logger(), "guard_condition %d triggered", i+1);
57+
}
58+
}
59+
for (int i=0; i<subscriptions_num; i++) {
60+
if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) {
61+
RCLCPP_INFO(node->get_logger(), "subscription %d triggered", i+1);
62+
std_msgs::msg::String msg;
63+
rclcpp::MessageInfo msg_info;
64+
if (sub_vector.at(i)->take(msg, msg_info)) {
65+
RCLCPP_INFO(node->get_logger(),
66+
"subscription %d: I heard '%s'", i+1, msg.data.c_str());
67+
} else {
68+
RCLCPP_INFO(node->get_logger(), "subscription %d: No message", i+1);
69+
}
70+
}
71+
}
72+
} else if (wait_result.kind() == rclcpp::WaitResultKind::Timeout) {
73+
RCLCPP_INFO(node->get_logger(), "wait-set waiting failed with timeout");
74+
} else if (wait_result.kind() == rclcpp::WaitResultKind::Empty) {
75+
RCLCPP_INFO(node->get_logger(), "wait-set waiting failed because wait-set is empty");
76+
}
77+
};
78+
79+
RCLCPP_INFO(node->get_logger(), "Action: Nothing triggered");
80+
wait_and_print_results();
81+
82+
RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 1");
83+
guard_condition1->trigger();
84+
wait_and_print_results();
85+
86+
RCLCPP_INFO(node->get_logger(), "Action: Trigger Guard condition 2");
87+
guard_condition2->trigger();
88+
wait_and_print_results();
89+
90+
RCLCPP_INFO(node->get_logger(), "Action: Message published");
91+
auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 1);
92+
pub->publish(std_msgs::msg::String().set__data("test"));
93+
wait_and_print_results();
6294

95+
RCLCPP_INFO(node->get_logger(), "Action: Guard condition 1 removed");
96+
RCLCPP_INFO(node->get_logger(), "Action: Guard condition 2 removed");
97+
wait_set.remove_guard_condition(guard_condition1);
6398
wait_set.remove_guard_condition(guard_condition2);
99+
wait_and_print_results();
64100

65-
{
66-
// still fails with timeout
67-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
68-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
69-
}
70-
71-
wait_set.remove_guard_condition(guard_condition);
72-
73-
{
74-
// still fails with timeout
75-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
76-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
77-
}
78-
101+
RCLCPP_INFO(node->get_logger(), "Action: Subscription 2 removed");
79102
wait_set.remove_subscription(sub2);
103+
wait_and_print_results();
80104

81-
{
82-
// still fails with timeout
83-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
84-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
85-
}
86-
87-
auto pub = node->create_publisher<std_msgs::msg::String>("~/chatter", 1);
88-
pub->publish(std_msgs::msg::String().set__data("test"));
105+
RCLCPP_INFO(node->get_logger(), "Action: Subscription 1 removed");
106+
wait_set.remove_subscription(sub1);
107+
wait_and_print_results();
89108

90-
{
91-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
92-
assert(wait_result.kind() == rclcpp::WaitResultKind::Ready);
93-
assert(wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0] != nullptr);
94-
std_msgs::msg::String msg;
95-
rclcpp::MessageInfo msg_info;
96-
assert(sub->take(msg, msg_info));
97-
assert(msg.data == "test");
98-
}
99-
100-
wait_set.remove_subscription(sub);
101-
102-
{
103-
// still will not fail, because thread-safe policy we are using adds its
104-
// own guard condition and therefore it will never be empty
105-
auto wait_result = wait_set.wait(std::chrono::seconds(1));
106-
assert(wait_result.kind() == rclcpp::WaitResultKind::Timeout);
107-
}
108109

109110
return 0;
110111
}

0 commit comments

Comments
 (0)