diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp index f9087fcb..f815aa66 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/communication_components.hpp @@ -52,6 +52,32 @@ class MinimalCartesianInput : public ComponentT { std::promise received_; }; +template +class ExceptionCartesianInput : public ComponentT { +public: + ExceptionCartesianInput(const rclcpp::NodeOptions& node_options, const std::string& topic) + : ComponentT(node_options, "exception_cartesian_input"), thrown_(false) { + this->received_future = this->received_.get_future(); + this->template add_input( + "cartesian_state", + [this](const std::shared_ptr) { + if (!this->thrown_) { + this->thrown_ = true; + throw std::runtime_error("Error"); + } else { + this->received_.set_value(); + } + }, + topic); + } + + std::shared_future received_future; + +private: + bool thrown_; + std::promise received_; +}; + template class MinimalTwistOutput : public ComponentT { public: diff --git a/source/modulo_components/test/cpp/test_component_communication.cpp b/source/modulo_components/test/cpp/test_component_communication.cpp index 24596fc4..df9605eb 100644 --- a/source/modulo_components/test/cpp/test_component_communication.cpp +++ b/source/modulo_components/test/cpp/test_component_communication.cpp @@ -33,6 +33,17 @@ TEST_F(ComponentCommunicationTest, InputOutput) { EXPECT_THROW(output_node->publish(), modulo_core::exceptions::CoreException); } +TEST_F(ComponentCommunicationTest, ExceptionInputOutput) { + auto cartesian_state = state_representation::CartesianState::Random("test"); + auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); + auto output_node = + std::make_shared>(rclcpp::NodeOptions(), "/topic", cartesian_state, true); + this->exec_->add_node(input_node); + this->exec_->add_node(output_node); + auto return_code = this->exec_->spin_until_future_complete(input_node->received_future, 500ms); + ASSERT_EQ(return_code, rclcpp::FutureReturnCode::SUCCESS); +} + TEST_F(ComponentCommunicationTest, InputOutputManual) { auto cartesian_state = state_representation::CartesianState::Random("test"); auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp index c1387f61..8514b610 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component_communication.cpp @@ -33,6 +33,17 @@ TEST_F(LifecycleComponentCommunicationTest, InputOutput) { EXPECT_THROW(output_node->publish(), modulo_core::exceptions::CoreException); } +TEST_F(LifecycleComponentCommunicationTest, ExceptionInputOutput) { + auto cartesian_state = state_representation::CartesianState::Random("test"); + auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); + auto output_node = std::make_shared>( + rclcpp::NodeOptions(), "/topic", cartesian_state, true); + add_configure_activate(this->exec_, input_node); + add_configure_activate(this->exec_, output_node); + auto return_code = this->exec_->spin_until_future_complete(input_node->received_future, 500ms); + ASSERT_EQ(return_code, rclcpp::FutureReturnCode::SUCCESS); +} + TEST_F(LifecycleComponentCommunicationTest, InputOutputManual) { auto cartesian_state = state_representation::CartesianState::Random("test"); auto input_node = std::make_shared>(rclcpp::NodeOptions(), "/topic"); diff --git a/source/modulo_components/test/python/conftest.py b/source/modulo_components/test/python/conftest.py index af69a29b..73336dd0 100644 --- a/source/modulo_components/test/python/conftest.py +++ b/source/modulo_components/test/python/conftest.py @@ -1,6 +1,7 @@ import clproto import pytest import state_representation as sr +from types import MethodType from modulo_components.component import Component from modulo_core import EncodedState from rclpy.task import Future @@ -122,6 +123,26 @@ def _make_minimal_cartesian_input(component_type, topic): yield _make_minimal_cartesian_input(request.param[0], request.param[1]) +@pytest.fixture +def exception_cartesian_input(request): + def _make_exception_cartesian_input(component_type, topic): + def callback(self): + if not self.thrown: + self.thrown = True + raise RuntimeError() + else: + self.received_future.set_result(True) + + component = component_type("exception_cartesian_input") + component.received_future = Future() + component.thrown = False + component.callback = MethodType(callback, component) + component.add_input("cartesian_pose", lambda msg: component.callback(), EncodedState, topic) + return component + + yield _make_exception_cartesian_input(request.param[0], request.param[1]) + + @pytest.fixture def minimal_sensor_input(request): def _make_minimal_sensor_input(component_type, topic): diff --git a/source/modulo_components/test/python/test_component_communication.py b/source/modulo_components/test/python/test_component_communication.py index a6f4d8f0..fb2d920b 100644 --- a/source/modulo_components/test/python/test_component_communication.py +++ b/source/modulo_components/test/python/test_component_communication.py @@ -16,6 +16,15 @@ def test_input_output(ros_exec, random_pose, minimal_cartesian_output, minimal_c minimal_cartesian_output.publish() +@pytest.mark.parametrize("exception_cartesian_input", [[Component, "/topic"]], indirect=True) +@pytest.mark.parametrize("minimal_cartesian_output", [[Component, "/topic", True]], indirect=True) +def test_exception_input_output(ros_exec, minimal_cartesian_output, exception_cartesian_input): + ros_exec.add_node(exception_cartesian_input) + ros_exec.add_node(minimal_cartesian_output) + ros_exec.spin_until_future_complete(exception_cartesian_input.received_future, timeout_sec=0.5) + assert exception_cartesian_input.received_future.done() and exception_cartesian_input.received_future.result() + + @pytest.mark.parametrize("minimal_cartesian_input", [[Component, "/topic"]], indirect=True) @pytest.mark.parametrize("minimal_cartesian_output", [[Component, "/topic", False]], indirect=True) def test_input_output_manual(ros_exec, random_pose, minimal_cartesian_output, minimal_cartesian_input): diff --git a/source/modulo_components/test/python/test_lifecycle_component_communication.py b/source/modulo_components/test/python/test_lifecycle_component_communication.py index 624f7cd0..bdad7043 100644 --- a/source/modulo_components/test/python/test_lifecycle_component_communication.py +++ b/source/modulo_components/test/python/test_lifecycle_component_communication.py @@ -25,6 +25,24 @@ def test_input_output(ros_exec, make_lifecycle_change_client, random_pose, minim minimal_cartesian_output.publish() +@pytest.mark.parametrize("exception_cartesian_input", [[LifecycleComponent, "/topic"]], indirect=True) +@pytest.mark.parametrize("minimal_cartesian_output", [[LifecycleComponent, "/topic", True]], indirect=True) +def test_exception_input_output( + ros_exec, make_lifecycle_change_client, minimal_cartesian_output, exception_cartesian_input): + input_change_client = make_lifecycle_change_client("exception_cartesian_input") + output_change_client = make_lifecycle_change_client("minimal_cartesian_output") + ros_exec.add_node(input_change_client) + ros_exec.add_node(output_change_client) + ros_exec.add_node(exception_cartesian_input) + ros_exec.add_node(minimal_cartesian_output) + input_change_client.configure(ros_exec) + output_change_client.configure(ros_exec) + input_change_client.activate(ros_exec) + output_change_client.activate(ros_exec) + ros_exec.spin_until_future_complete(exception_cartesian_input.received_future, timeout_sec=0.5) + assert exception_cartesian_input.received_future.done() and exception_cartesian_input.received_future.result() + + @pytest.mark.parametrize("minimal_cartesian_input", [[LifecycleComponent, "/topic"]], indirect=True) @pytest.mark.parametrize("minimal_cartesian_output", [[LifecycleComponent, "/topic", False]], indirect=True) def test_input_output_manual(ros_exec, make_lifecycle_change_client, random_pose, minimal_cartesian_output,