forked from ros2/rclcpp
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathgraph_listener.hpp
More file actions
197 lines (168 loc) · 5.42 KB
/
graph_listener.hpp
File metadata and controls
197 lines (168 loc) · 5.42 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
// 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.
#ifndef RCLCPP__GRAPH_LISTENER_HPP_
#define RCLCPP__GRAPH_LISTENER_HPP_
#include <atomic>
#include <memory>
#include <mutex>
#include <thread>
#include <vector>
#include "rcl/guard_condition.h"
#include "rcl/wait.h"
#include "rclcpp/context.hpp"
#include "rclcpp/macros.hpp"
#include "rclcpp/node_interfaces/node_graph_interface.hpp"
#include "rclcpp/visibility_control.hpp"
namespace rclcpp
{
namespace graph_listener
{
/// Thrown when a function is called on a GraphListener that is already shutdown.
class GraphListenerShutdownError : public std::runtime_error
{
public:
GraphListenerShutdownError()
: std::runtime_error("GraphListener already shutdown") {}
};
/// Thrown when a node has already been added to the GraphListener.
class NodeAlreadyAddedError : public std::runtime_error
{
public:
NodeAlreadyAddedError()
: std::runtime_error("node already added") {}
};
/// Thrown when the given node is not in the GraphListener.
class NodeNotFoundError : public std::runtime_error
{
public:
NodeNotFoundError()
: std::runtime_error("node not found") {}
};
/// Notifies many nodes of graph changes by listening in a thread.
class GraphListener : public std::enable_shared_from_this<GraphListener>
{
public:
RCLCPP_PUBLIC
explicit GraphListener(const rclcpp::Context::SharedPtr & parent_context);
RCLCPP_PUBLIC
virtual ~GraphListener();
/// Start the graph listener's listen thread if it hasn't been started.
/**
* This function is thread-safe.
*
* \throws GraphListenerShutdownError if the GraphListener is shutdown
* \throws std::runtime if the parent context was destroyed
* \throws anything rclcpp::exceptions::throw_from_rcl_error can throw.
*/
RCLCPP_PUBLIC
virtual
void
start_if_not_started();
/// Add a node to the graph listener's list of nodes.
/**
* \throws GraphListenerShutdownError if the GraphListener is shutdown
* \throws NodeAlreadyAddedError if the given node is already in the list
* \throws std::invalid_argument if node is nullptr
* \throws std::system_error anything std::mutex::lock() throws
*/
RCLCPP_PUBLIC
virtual
void
add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Return true if the given node is in the graph listener's list of nodes.
/**
* Also return false if given nullptr.
*
* \throws std::system_error anything std::mutex::lock() throws
*/
RCLCPP_PUBLIC
virtual
bool
has_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Remove a node from the graph listener's list of nodes.
/**
*
* \throws NodeNotFoundError if the given node is not in the list
* \throws std::invalid_argument if node is nullptr
* \throws std::system_error anything std::mutex::lock() throws
*/
RCLCPP_PUBLIC
virtual
void
remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph);
/// Stop the listening thread.
/**
* The thread cannot be restarted, and the class is defunct after calling.
* This function is called by the ~GraphListener() and does nothing if
* shutdown() was already called.
* This function exists separately from the ~GraphListener() so that it can
* be called before and exceptions can be caught.
*
* If start_if_not_started() was never called, this function still succeeds,
* but start_if_not_started() still cannot be called after this function.
*
* \throws rclcpp::execptions::RCLError from rcl_guard_condition_fini()
* \throws rclcpp::execptions::RCLError from rcl_wait_set_fini()
* \throws std::system_error anything std::mutex::lock() throws
*/
RCLCPP_PUBLIC
virtual
void
shutdown();
/// Nothrow version of shutdown(), logs to RCLCPP_ERROR instead.
RCLCPP_PUBLIC
virtual
void
shutdown(const std::nothrow_t &) noexcept;
/// Return true if shutdown() has been called, else false.
RCLCPP_PUBLIC
virtual
bool
is_shutdown();
protected:
/// Main function for the listening thread.
RCLCPP_PUBLIC
virtual
void
run();
RCLCPP_PUBLIC
virtual
void
run_loop();
RCLCPP_PUBLIC
void
init_wait_set();
RCLCPP_PUBLIC
void
cleanup_wait_set();
private:
RCLCPP_DISABLE_COPY(GraphListener)
/** \internal */
void
__shutdown();
std::weak_ptr<rclcpp::Context> weak_parent_context_;
std::shared_ptr<rcl_context_t> rcl_parent_context_;
std::thread listener_thread_;
bool is_started_;
std::atomic_bool is_shutdown_;
mutable std::mutex shutdown_mutex_;
mutable std::mutex node_graph_interfaces_barrier_mutex_;
mutable std::mutex node_graph_interfaces_mutex_;
std::vector<rclcpp::node_interfaces::NodeGraphInterface *> node_graph_interfaces_;
rcl_guard_condition_t interrupt_guard_condition_ = rcl_get_zero_initialized_guard_condition();
rcl_wait_set_t wait_set_ = rcl_get_zero_initialized_wait_set();
};
} // namespace graph_listener
} // namespace rclcpp
#endif // RCLCPP__GRAPH_LISTENER_HPP_