Skip to content

Commit d77bac1

Browse files
mjcarrollmjcarroll
committed
Add base executor objects that can be used by implementors
Signed-off-by: Michael Carroll <[email protected]>
1 parent 5718fe1 commit d77bac1

7 files changed

+1079
-0
lines changed

‎rclcpp/CMakeLists.txt

+3
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,9 @@ set(${PROJECT_NAME}_SRCS
5454
src/rclcpp/executable_list.cpp
5555
src/rclcpp/executor.cpp
5656
src/rclcpp/executors.cpp
57+
src/rclcpp/executors/executor_notify_waitable.cpp
58+
src/rclcpp/executors/executor_entities_collector.cpp
59+
src/rclcpp/executors/executor_entities_collection.cpp
5760
src/rclcpp/executors/multi_threaded_executor.cpp
5861
src/rclcpp/executors/single_threaded_executor.cpp
5962
src/rclcpp/executors/static_executor_entities_collector.cpp

‎rclcpp/include/rclcpp/executors/executor_entities_collection.hpp

+126
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,126 @@
1+
// Copyright 2023 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
16+
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
17+
18+
#include <deque>
19+
#include <unordered_map>
20+
#include <vector>
21+
22+
#include "rcpputils/thread_safety_annotations.hpp"
23+
24+
#include <rclcpp/any_executable.hpp>
25+
#include <rclcpp/node_interfaces/node_base.hpp>
26+
#include <rclcpp/callback_group.hpp>
27+
#include <rclcpp/executors/executor_notify_waitable.hpp>
28+
#include <rclcpp/visibility_control.hpp>
29+
#include <rclcpp/wait_result.hpp>
30+
#include <rclcpp/wait_set.hpp>
31+
32+
namespace rclcpp
33+
{
34+
namespace executors
35+
{
36+
37+
struct ExecutorEntitiesCollection
38+
{
39+
struct TimerEntry {
40+
rclcpp::TimerBase::WeakPtr timer;
41+
rclcpp::CallbackGroup::WeakPtr callback_group;
42+
};
43+
using TimerCollection = std::unordered_map<const rcl_timer_t *, TimerEntry>;
44+
45+
struct SubscriptionEntry{
46+
rclcpp::SubscriptionBase::WeakPtr subscription;
47+
rclcpp::CallbackGroup::WeakPtr callback_group;
48+
};
49+
using SubscriptionCollection = std::unordered_map<const rcl_subscription_t *, SubscriptionEntry>;
50+
51+
struct ClientEntry {
52+
rclcpp::ClientBase::WeakPtr client;
53+
rclcpp::CallbackGroup::WeakPtr callback_group;
54+
};
55+
using ClientCollection = std::unordered_map<const rcl_client_t *, ClientEntry>;
56+
57+
struct ServiceEntry {
58+
rclcpp::ServiceBase::WeakPtr service;
59+
rclcpp::CallbackGroup::WeakPtr callback_group;
60+
};
61+
using ServiceCollection = std::unordered_map<const rcl_service_t *, ServiceEntry>;
62+
63+
struct WaitableEntry {
64+
rclcpp::Waitable::WeakPtr waitable;
65+
rclcpp::CallbackGroup::WeakPtr callback_group;
66+
};
67+
using WaitableCollection = std::unordered_map<const rclcpp::Waitable *, WaitableEntry>;
68+
69+
using GuardConditionCollection = std::unordered_map<const rcl_guard_condition_t *,
70+
rclcpp::GuardCondition::WeakPtr>;
71+
72+
TimerCollection timers;
73+
SubscriptionCollection subscriptions;
74+
ClientCollection clients;
75+
ServiceCollection services;
76+
GuardConditionCollection guard_conditions;
77+
WaitableCollection waitables;
78+
79+
void clear();
80+
81+
using TimerUpdatedFunc = std::function<void(rclcpp::TimerBase::SharedPtr)>;
82+
void update_timers(const ExecutorEntitiesCollection::TimerCollection & other,
83+
TimerUpdatedFunc timer_added,
84+
TimerUpdatedFunc timer_removed);
85+
86+
using SubscriptionUpdatedFunc = std::function<void(rclcpp::SubscriptionBase::SharedPtr)>;
87+
void update_subscriptions(const ExecutorEntitiesCollection::SubscriptionCollection & other,
88+
SubscriptionUpdatedFunc subscription_added,
89+
SubscriptionUpdatedFunc subscription_removed);
90+
91+
using ClientUpdatedFunc = std::function<void(rclcpp::ClientBase::SharedPtr)>;
92+
void update_clients(const ExecutorEntitiesCollection::ClientCollection & other,
93+
ClientUpdatedFunc client_added,
94+
ClientUpdatedFunc client_removed);
95+
96+
using ServiceUpdatedFunc = std::function<void(rclcpp::ServiceBase::SharedPtr)>;
97+
void update_services(const ExecutorEntitiesCollection::ServiceCollection & other,
98+
ServiceUpdatedFunc service_added,
99+
ServiceUpdatedFunc service_removed);
100+
101+
using GuardConditionUpdatedFunc = std::function<void(rclcpp::GuardCondition::SharedPtr)>;
102+
void update_guard_conditions(const ExecutorEntitiesCollection::GuardConditionCollection & other,
103+
GuardConditionUpdatedFunc guard_condition_added,
104+
GuardConditionUpdatedFunc guard_condition_removed);
105+
106+
using WaitableUpdatedFunc = std::function<void(rclcpp::Waitable::SharedPtr)>;
107+
void update_waitables(const ExecutorEntitiesCollection::WaitableCollection & other,
108+
WaitableUpdatedFunc waitable_added,
109+
WaitableUpdatedFunc waitable_removed);
110+
};
111+
112+
void
113+
build_entities_collection(
114+
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
115+
ExecutorEntitiesCollection & collection);
116+
117+
std::deque<rclcpp::AnyExecutable>
118+
ready_executables(
119+
const ExecutorEntitiesCollection & collection,
120+
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result
121+
);
122+
123+
} // namespace executors
124+
} // namespace rclcpp
125+
126+
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_

‎rclcpp/include/rclcpp/executors/executor_entities_collector.hpp

+149
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
// Copyright 2023 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
16+
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_
17+
18+
#include <list>
19+
#include <memory>
20+
#include <set>
21+
#include <vector>
22+
23+
#include "rcpputils/thread_safety_annotations.hpp"
24+
25+
#include <rclcpp/any_executable.hpp>
26+
#include <rclcpp/node_interfaces/node_base.hpp>
27+
#include <rclcpp/callback_group.hpp>
28+
#include <rclcpp/executors/executor_notify_waitable.hpp>
29+
#include <rclcpp/visibility_control.hpp>
30+
#include <rclcpp/wait_set.hpp>
31+
#include <rclcpp/wait_result.hpp>
32+
33+
namespace rclcpp
34+
{
35+
namespace executors
36+
{
37+
38+
class ExecutorEntitiesCollector
39+
{
40+
public:
41+
RCLCPP_PUBLIC
42+
ExecutorEntitiesCollector();
43+
44+
RCLCPP_PUBLIC
45+
~ExecutorEntitiesCollector();
46+
47+
/// Add a node to the entity collector
48+
/**
49+
* \param[in] node_ptr a shared pointer that points to a node base interface
50+
* \throw std::runtime_error if the node is associated with an executor
51+
*/
52+
RCLCPP_PUBLIC
53+
void
54+
add_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
55+
56+
/// Remove a node from the entity collector
57+
/**
58+
* \param[in] node_ptr a shared pointer that points to a node base interface
59+
* \throw std::runtime_error if the node is associated with an executor
60+
* \throw std::runtime_error if the node is associated with this executor
61+
*/
62+
RCLCPP_PUBLIC
63+
void
64+
remove_node(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
65+
66+
RCLCPP_PUBLIC
67+
bool
68+
has_node(const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
69+
70+
/// Add a callback group to the entity collector
71+
/**
72+
* \param[in] group_ptr a shared pointer that points to a callback group
73+
* \throw std::runtime_error if the callback_group is associated with an executor
74+
*/
75+
RCLCPP_PUBLIC
76+
void
77+
add_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
78+
79+
/// Remove a callback group from the entity collector
80+
/**
81+
* \param[in] group_ptr a shared pointer that points to a callback group
82+
* \throw std::runtime_error if the callback_group is not associated with an executor
83+
* \throw std::runtime_error if the callback_group is not associated with this executor
84+
*/
85+
RCLCPP_PUBLIC
86+
void
87+
remove_callback_group(rclcpp::CallbackGroup::SharedPtr group_ptr);
88+
89+
RCLCPP_PUBLIC
90+
std::vector<rclcpp::CallbackGroup::WeakPtr>
91+
get_all_callback_groups();
92+
93+
RCLCPP_PUBLIC
94+
std::vector<rclcpp::CallbackGroup::WeakPtr>
95+
get_manually_added_callback_groups();
96+
97+
RCLCPP_PUBLIC
98+
std::vector<rclcpp::CallbackGroup::WeakPtr>
99+
get_automatically_added_callback_groups();
100+
101+
RCLCPP_PUBLIC
102+
void
103+
update_collections();
104+
105+
RCLCPP_PUBLIC
106+
std::shared_ptr<ExecutorNotifyWaitable>
107+
get_executor_notify_waitable();
108+
109+
protected:
110+
using CallbackGroupCollection = std::set<
111+
rclcpp::CallbackGroup::WeakPtr,
112+
std::owner_less<rclcpp::CallbackGroup::WeakPtr>>;
113+
114+
RCLCPP_PUBLIC
115+
void
116+
add_callback_group_to_collection(rclcpp::CallbackGroup::SharedPtr group_ptr,
117+
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
118+
119+
RCLCPP_PUBLIC
120+
void
121+
remove_callback_group_from_collection(rclcpp::CallbackGroup::SharedPtr group_ptr,
122+
CallbackGroupCollection & collection) RCPPUTILS_TSA_REQUIRES(mutex_);
123+
124+
RCLCPP_PUBLIC
125+
void
126+
add_automatically_associated_callback_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
127+
128+
RCLCPP_PUBLIC
129+
void
130+
prune_invalid_nodes_and_groups() RCPPUTILS_TSA_REQUIRES(mutex_);
131+
132+
mutable std::mutex mutex_;
133+
134+
CallbackGroupCollection
135+
manually_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
136+
137+
CallbackGroupCollection
138+
automatically_added_groups_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
139+
140+
/// nodes that are associated with the executor
141+
std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr>
142+
weak_nodes_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
143+
144+
std::shared_ptr<ExecutorNotifyWaitable> notify_waitable_ RCPPUTILS_TSA_GUARDED_BY(mutex_);
145+
};
146+
} // namespace executors
147+
} // namespace rclcpp
148+
149+
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTOR_HPP_

‎rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

+73
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
// Copyright 2023 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
16+
#define RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_
17+
18+
#include <list>
19+
#include <memory>
20+
21+
#include "rclcpp/guard_condition.hpp"
22+
#include "rclcpp/waitable.hpp"
23+
24+
namespace rclcpp
25+
{
26+
namespace executors
27+
{
28+
29+
class ExecutorNotifyWaitable: rclcpp::Waitable
30+
{
31+
public:
32+
RCLCPP_SMART_PTR_DEFINITIONS(ExecutorNotifyWaitable)
33+
34+
// Constructor
35+
RCLCPP_PUBLIC
36+
ExecutorNotifyWaitable() = default;
37+
38+
// Destructor
39+
RCLCPP_PUBLIC
40+
~ExecutorNotifyWaitable() override = default;
41+
42+
RCLCPP_PUBLIC
43+
void
44+
add_to_wait_set(rcl_wait_set_t * wait_set) override;
45+
46+
RCLCPP_PUBLIC
47+
bool
48+
is_ready(rcl_wait_set_t * wait_set) override;
49+
50+
RCLCPP_PUBLIC
51+
void
52+
execute(std::shared_ptr<void> & data) override;
53+
54+
RCLCPP_PUBLIC
55+
std::shared_ptr<void>
56+
take_data() override;
57+
58+
RCLCPP_PUBLIC
59+
void
60+
add_guard_condition(rclcpp::GuardCondition * guard_condition);
61+
62+
RCLCPP_PUBLIC
63+
void
64+
remove_guard_condition(rclcpp::GuardCondition * guard_condition);
65+
66+
private:
67+
std::list<const rclcpp::GuardCondition *> notify_guard_conditions_;
68+
};
69+
70+
} // namespace executors
71+
} // namespace rclcpp
72+
73+
#endif // RCLCPP__EXECUTORS__EXECUTOR_NOTIFY_WAITABLE_HPP_

0 commit comments

Comments
 (0)