void
GraphListener::start_if_not_started()
{
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (is_shutdown_.load()) {
throw GraphListenerShutdownError();
}
if (!is_started_) {
// Initialize the wait set before starting.
rcl_ret_t ret = rcl_wait_set_init(
&wait_set_,
0, // number_of_subscriptions
2, // number_of_guard_conditions
0, // number_of_timers
0, // number_of_clients
0, // number_of_services
rcl_get_default_allocator());
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to initialize wait set");
}
// Register an on_shutdown hook to shtudown the graph listener.
// This is important to ensure that the wait set is finalized before
// destruction of static objects occurs.
std::weak_ptr<GraphListener> weak_this = shared_from_this();
rclcpp::utilities::on_shutdown([weak_this]() {
auto shared_this = weak_this.lock();
if (shared_this) {
shared_this->shutdown();
}
});
// Start the listener thread.
listener_thread_ = std::thread(&GraphListener::run, this);
is_started_ = true;
}
}
void
GraphListener::shutdown()
{
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (!is_shutdown_.exchange(true)) {
if (is_started_) {
interrupt_(&interrupt_guard_condition_);
listener_thread_.join();
}
rcl_ret_t ret = rcl_guard_condition_fini(&interrupt_guard_condition_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to finalize interrupt guard condition");
}
if (shutdown_guard_condition_) {
rclcpp::utilities::release_sigint_guard_condition(&wait_set_);
shutdown_guard_condition_ = nullptr;
}
if (is_started_) {
ret = rcl_wait_set_fini(&wait_set_);
if (RCL_RET_OK != ret) {
throw_from_rcl_error(ret, "failed to finalize wait set");
}
}
}
}
void
GraphListener::add_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
if (!node_graph) {
throw std::invalid_argument("node is nullptr");
}
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (is_shutdown_.load()) {
throw GraphListenerShutdownError();
}
// Acquire the nodes mutex using the barrier to prevent the run loop from
// re-locking the nodes mutex after being interrupted.
acquire_nodes_lock_(
&node_graph_interfaces_barrier_mutex_,
&node_graph_interfaces_mutex_,
&interrupt_guard_condition_);
// Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
if (has_node_(&node_graph_interfaces_, node_graph)) {
throw NodeAlreadyAddedError();
}
node_graph_interfaces_.push_back(node_graph);
// The run loop has already been interrupted by acquire_nodes_lock_() and
// will evaluate the new node when nodes_lock releases the node_graph_interfaces_mutex_.
}
void
GraphListener::remove_node(rclcpp::node_interfaces::NodeGraphInterface * node_graph)
{
if (!node_graph) {
throw std::invalid_argument("node is nullptr");
}
std::lock_guard<std::mutex> shutdown_lock(shutdown_mutex_);
if (is_shutdown()) {
// If shutdown, then the run loop has been joined, so we can remove them directly.
return remove_node_(&node_graph_interfaces_, node_graph);
}
// Otherwise, first interrupt and lock against the run loop to safely remove the node.
// Acquire the nodes mutex using the barrier to prevent the run loop from
// re-locking the nodes mutex after being interrupted.
acquire_nodes_lock_(
&node_graph_interfaces_barrier_mutex_,
&node_graph_interfaces_mutex_,
&interrupt_guard_condition_);
// Store the now acquired node_graph_interfaces_mutex_ in the scoped lock using adopt_lock.
std::lock_guard<std::mutex> nodes_lock(node_graph_interfaces_mutex_, std::adopt_lock);
remove_node_(&node_graph_interfaces_, node_graph);
}
/// blocks until the shutdown condition has been signaled
inline void wait(void) {
boost::mutex::scoped_lock shutdown_lock(m_shutdown_mutex);
while (! m_shutdown_now)
m_shutdown_cond.wait(shutdown_lock);
}
/// signals the shutdown condition
inline void shutdown(void) {
boost::mutex::scoped_lock shutdown_lock(m_shutdown_mutex);
m_shutdown_now = true;
m_shutdown_cond.notify_all();
}
