diff --git a/rclcpp/include/rclcpp/exceptions/exceptions.hpp b/rclcpp/include/rclcpp/exceptions/exceptions.hpp index d501acd71c..233b4b67ba 100644 --- a/rclcpp/include/rclcpp/exceptions/exceptions.hpp +++ b/rclcpp/include/rclcpp/exceptions/exceptions.hpp @@ -100,6 +100,15 @@ class InvalidServiceNameError : public NameValidationError {} }; +class UnimplementedError : public std::runtime_error +{ +public: + UnimplementedError() + : std::runtime_error("This code is unimplemented.") {} + explicit UnimplementedError(const std::string & msg) + : std::runtime_error(msg) {} +}; + /// Throw a C++ std::exception which was created based on an rcl error. /** * Passing nullptr for reset_error is safe and will avoid calling any function diff --git a/rclcpp/src/rclcpp/executor.cpp b/rclcpp/src/rclcpp/executor.cpp index 0c83e0deba..5c295f320f 100644 --- a/rclcpp/src/rclcpp/executor.cpp +++ b/rclcpp/src/rclcpp/executor.cpp @@ -22,6 +22,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/executor.hpp" +#include "rclcpp/executors/static_single_threaded_executor.hpp" #include "rclcpp/node.hpp" #include "rclcpp/scope_exit.hpp" #include "rclcpp/utilities.hpp" @@ -215,6 +216,12 @@ Executor::spin_node_some(std::shared_ptr node) void Executor::spin_some(std::chrono::nanoseconds max_duration) { + if (nullptr != dynamic_cast(this)) { + throw rclcpp::exceptions::UnimplementedError( + "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or " + "spin_until_future_complete"); + } + auto start = std::chrono::steady_clock::now(); auto max_duration_not_elapsed = [max_duration, start]() { if (std::chrono::nanoseconds(0) == max_duration) { @@ -256,6 +263,11 @@ Executor::spin_once_impl(std::chrono::nanoseconds timeout) void Executor::spin_once(std::chrono::nanoseconds timeout) { + if (nullptr != dynamic_cast(this)) { + throw rclcpp::exceptions::UnimplementedError( + "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or " + "spin_until_future_complete"); + } if (spinning.exchange(true)) { throw std::runtime_error("spin_once() called while already spinning"); } diff --git a/rclcpp/test/CMakeLists.txt b/rclcpp/test/CMakeLists.txt index cfd878cf32..5022024566 100644 --- a/rclcpp/test/CMakeLists.txt +++ b/rclcpp/test/CMakeLists.txt @@ -458,6 +458,12 @@ if(TARGET test_interface_traits) target_link_libraries(test_interface_traits ${PROJECT_NAME}) endif() +ament_add_gtest(test_static_single_threaded_executor rclcpp/executors/test_static_single_threaded_executor.cpp + APPEND_LIBRARY_DIRS "${append_library_dirs}") +if(TARGET test_static_single_threaded_executor) + target_link_libraries(test_static_single_threaded_executor ${PROJECT_NAME}) +endif() + ament_add_gtest(test_multi_threaded_executor rclcpp/executors/test_multi_threaded_executor.cpp APPEND_LIBRARY_DIRS "${append_library_dirs}") if(TARGET test_multi_threaded_executor) diff --git a/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp new file mode 100644 index 0000000000..7ff72426f9 --- /dev/null +++ b/rclcpp/test/rclcpp/executors/test_static_single_threaded_executor.cpp @@ -0,0 +1,48 @@ +// Copyright 2020 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. + +#include + +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/executors.hpp" + +using namespace std::chrono_literals; + +class TestStaticSingleThreadedExecutor : public ::testing::Test +{ +public: + void SetUp() + { + rclcpp::init(0, nullptr); + } + + void TearDown() + { + rclcpp::shutdown(); + } +}; + +TEST_F(TestStaticSingleThreadedExecutor, check_unimplemented) { + rclcpp::executors::StaticSingleThreadedExecutor executor; + auto node = std::make_shared("node", "ns"); + executor.add_node(node); + + EXPECT_THROW(executor.spin_some(), rclcpp::exceptions::UnimplementedError); + EXPECT_THROW(executor.spin_once(0ns), rclcpp::exceptions::UnimplementedError); +}