diff --git a/rclpy/src/rclpy/context.cpp b/rclpy/src/rclpy/context.cpp index 2fa5146b6..079ab02cb 100644 --- a/rclpy/src/rclpy/context.cpp +++ b/rclpy/src/rclpy/context.cpp @@ -151,6 +151,9 @@ void Context::shutdown() { std::lock_guard guard{g_contexts_mutex}; + if (already_shutdown_) { + throw CONTEXT_ALREADY_SHUTDOWN("Context already shutdown."); + } auto iter = std::find(g_contexts.begin(), g_contexts.end(), rcl_context_.get()); if (iter != g_contexts.end()) { g_contexts.erase(iter); @@ -158,6 +161,7 @@ Context::shutdown() if (RCL_RET_OK != ret) { throw RCLError("failed to shutdown"); } + already_shutdown_ = true; } } diff --git a/rclpy/src/rclpy/context.hpp b/rclpy/src/rclpy/context.hpp index cdeb14122..30259e3e4 100644 --- a/rclpy/src/rclpy/context.hpp +++ b/rclpy/src/rclpy/context.hpp @@ -101,6 +101,7 @@ class Context : public Destroyable, public std::enable_shared_from_this private: std::shared_ptr rcl_context_; + bool already_shutdown_{false}; }; /// Define a pybind11 wrapper for an rclpy::Context diff --git a/rclpy/src/rclpy/exceptions.hpp b/rclpy/src/rclpy/exceptions.hpp index 521146fd0..9146b794d 100644 --- a/rclpy/src/rclpy/exceptions.hpp +++ b/rclpy/src/rclpy/exceptions.hpp @@ -80,6 +80,11 @@ class InvalidHandle : public std::runtime_error using std::runtime_error::runtime_error; }; +class CONTEXT_ALREADY_SHUTDOWN : public std::runtime_error +{ + using std::runtime_error::runtime_error; +}; + } // namespace rclpy #endif // RCLPY__EXCEPTIONS_HPP_ diff --git a/rclpy/test/test_init_shutdown.py b/rclpy/test/test_init_shutdown.py index ecce3862e..43a514448 100644 --- a/rclpy/test/test_init_shutdown.py +++ b/rclpy/test/test_init_shutdown.py @@ -74,6 +74,15 @@ def test_double_init(): rclpy.shutdown(context=context) +def test_double_shutdown(): + context = rclpy.context.Context() + rclpy.init(context=context) + assert context.ok() + rclpy.shutdown(context=context) + with pytest.raises(RuntimeError): + rclpy.shutdown(context=context) + + def test_create_node_without_init(): context = rclpy.context.Context() with pytest.raises(NotInitializedException):