From 0c26dd99b6aa8aa10e0ba217e5a1f6529060d11d Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Thu, 27 Jul 2017 07:55:15 -0700 Subject: [PATCH] expose error handling for state changes (#344) * remove fprintf, use logging * expose lifecycle error code * address comments --- .../rclcpp_lifecycle/lifecycle_node.hpp | 28 ++++++++ rclcpp_lifecycle/src/lifecycle_node.cpp | 49 ++++++++++++++ .../src/lifecycle_node_interface_impl.hpp | 67 +++++++++++-------- .../test/test_callback_exceptions.cpp | 19 ++++++ rclcpp_lifecycle/test/test_lifecycle_node.cpp | 28 ++++++-- 5 files changed, 160 insertions(+), 31 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 28b38ac..0713ffd 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -371,30 +371,58 @@ public: const State & trigger_transition(const Transition & transition); + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(const Transition & transition, rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & trigger_transition(uint8_t transition_id); + RCLCPP_LIFECYCLE_PUBLIC + const State & + trigger_transition(uint8_t transition_id, rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & configure(); + RCLCPP_LIFECYCLE_PUBLIC + const State & + configure(rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & cleanup(); + RCLCPP_LIFECYCLE_PUBLIC + const State & + cleanup(rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & activate(); + RCLCPP_LIFECYCLE_PUBLIC + const State & + activate(rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & deactivate(); + RCLCPP_LIFECYCLE_PUBLIC + const State & + deactivate(rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC const State & shutdown(); + RCLCPP_LIFECYCLE_PUBLIC + const State & + shutdown(rcl_lifecycle_ret_t & cb_return_code); + RCLCPP_LIFECYCLE_PUBLIC bool register_on_configure(std::function fcn); diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index 44863fd..db9efd5 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -305,12 +305,26 @@ LifecycleNode::trigger_transition(const Transition & transition) return trigger_transition(transition.id()); } +const State & +LifecycleNode::trigger_transition( + const Transition & transition, rcl_lifecycle_ret_t & cb_return_code) +{ + return trigger_transition(transition.id(), cb_return_code); +} + const State & LifecycleNode::trigger_transition(uint8_t transition_id) { return impl_->trigger_transition(transition_id); } +const State & +LifecycleNode::trigger_transition( + uint8_t transition_id, rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition(transition_id, cb_return_code); +} + const State & LifecycleNode::configure() { @@ -318,6 +332,13 @@ LifecycleNode::configure() lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); } +const State & +LifecycleNode::configure(rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, cb_return_code); +} + const State & LifecycleNode::cleanup() { @@ -325,6 +346,13 @@ LifecycleNode::cleanup() lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP); } +const State & +LifecycleNode::cleanup(rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, cb_return_code); +} + const State & LifecycleNode::activate() { @@ -332,6 +360,13 @@ LifecycleNode::activate() lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE); } +const State & +LifecycleNode::activate(rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, cb_return_code); +} + const State & LifecycleNode::deactivate() { @@ -339,6 +374,13 @@ LifecycleNode::deactivate() lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE); } +const State & +LifecycleNode::deactivate(rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, cb_return_code); +} + const State & LifecycleNode::shutdown() { @@ -346,6 +388,13 @@ LifecycleNode::shutdown() lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN); } +const State & +LifecycleNode::shutdown(rcl_lifecycle_ret_t & cb_return_code) +{ + return impl_->trigger_transition( + lifecycle_msgs::msg::Transition::TRANSITION_SHUTDOWN, cb_return_code); +} + void LifecycleNode::add_publisher_handle( std::shared_ptr pub) diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 1ad8726..1a158af 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -40,6 +40,8 @@ #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rcutils/logging_macros.h" + namespace rclcpp_lifecycle { @@ -179,7 +181,13 @@ public: throw std::runtime_error( "Can't get state. State machine is not initialized."); } - resp->success = change_state(req->transition.id); + rcl_lifecycle_ret_t cb_return_code; + auto ret = change_state(req->transition.id, cb_return_code); + (void) ret; + // TODO(karsten1987): Lifecycle msgs have to be extended to keep both returns + // 1. return is the actual transition + // 2. return is whether an error occurred or not + resp->success = (cb_return_code == RCL_RET_OK); } void @@ -226,7 +234,6 @@ public: if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { throw std::runtime_error( "Can't get available transitions. State machine is not initialized."); - return; } for (uint8_t i = 0; i < state_machine_.transition_map.transitions_size; ++i) { @@ -273,13 +280,13 @@ public: return transitions; } - bool - change_state(std::uint8_t lifecycle_transition) + rcl_ret_t + change_state(std::uint8_t lifecycle_transition, rcl_lifecycle_ret_t & cb_return_code) { if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) { - fprintf(stderr, "%s:%d, Unable to change state for state machine for %s: %s \n", - __FILE__, __LINE__, node_base_interface_->get_name(), rcl_get_error_string_safe()); - return false; + RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s", + node_base_interface_->get_name(), rcl_get_error_string_safe()) + return RCL_RET_ERROR; } // keep the initial state to pass to a transition callback @@ -287,49 +294,46 @@ public: uint8_t transition_id = lifecycle_transition; if (rcl_lifecycle_trigger_transition(&state_machine_, transition_id, true) != RCL_RET_OK) { - fprintf(stderr, "%s:%d, Unable to start transition %u from current state %s: %s\n", - __FILE__, __LINE__, transition_id, - state_machine_.current_state->label, rcl_get_error_string_safe()); - return false; + RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s", + transition_id, state_machine_.current_state->label, rcl_get_error_string_safe()) + return RCL_RET_ERROR; } - rcl_lifecycle_ret_t cb_success = execute_callback( + cb_return_code = execute_callback( state_machine_.current_state->id, initial_state); if (rcl_lifecycle_trigger_transition( - &state_machine_, cb_success, true) != RCL_RET_OK) + &state_machine_, cb_return_code, true) != RCL_RET_OK) { - fprintf(stderr, "Failed to finish transition %u. Current state is now: %s\n", - transition_id, state_machine_.current_state->label); - return false; + RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s", + transition_id, state_machine_.current_state->label) + return RCL_RET_ERROR; } // error handling ?! // TODO(karsten1987): iterate over possible ret value - if (cb_success == RCL_LIFECYCLE_RET_ERROR) { + if (cb_return_code == RCL_LIFECYCLE_RET_ERROR) { + RCUTILS_LOG_WARN("Error occurred while doing error handling.") rcl_lifecycle_ret_t error_resolved = execute_callback(state_machine_.current_state->id, initial_state); - if (error_resolved == RCL_RET_OK) { - // fprintf(stderr, "Exception handling was successful\n"); + if (error_resolved == RCL_LIFECYCLE_RET_OK) { // We call cleanup on the error state if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { - fprintf(stderr, "Failed to call cleanup on error state\n"); - return false; + RCUTILS_LOG_ERROR("Failed to call cleanup on error state") + return RCL_RET_ERROR; } - // fprintf(stderr, "current state after error callback%s\n", - // state_machine_.current_state->label); } else { // We call shutdown on the error state if (rcl_lifecycle_trigger_transition(&state_machine_, error_resolved, true) != RCL_RET_OK) { - fprintf(stderr, "Failed to call cleanup on error state\n"); - return false; + RCUTILS_LOG_ERROR("Failed to call cleanup on error state") + return RCL_RET_ERROR; } } } // This true holds in both cases where the actual callback // was successful or not, since at this point we have a valid transistion // to either a new primary state or error state - return true; + return RCL_RET_OK; } rcl_lifecycle_ret_t @@ -360,7 +364,16 @@ public: const State & trigger_transition(uint8_t transition_id) { - change_state(transition_id); + rcl_lifecycle_ret_t error; + change_state(transition_id, error); + (void) error; + return get_current_state(); + } + + const State & + trigger_transition(uint8_t transition_id, rcl_lifecycle_ret_t & cb_return_code) + { + change_state(transition_id, cb_return_code); return get_current_state(); } diff --git a/rclcpp_lifecycle/test/test_callback_exceptions.cpp b/rclcpp_lifecycle/test/test_callback_exceptions.cpp index 60fcfa2..88f0c7a 100644 --- a/rclcpp_lifecycle/test/test_callback_exceptions.cpp +++ b/rclcpp_lifecycle/test/test_callback_exceptions.cpp @@ -69,6 +69,15 @@ TEST_F(TestCallbackExceptions, positive_on_error) { EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); } +TEST_F(TestCallbackExceptions, positive_on_error_with_code) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + rcl_lifecycle_ret_t ret = RCL_LIFECYCLE_RET_OK; + test_node->configure(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_ERROR, ret); +} + class NegativeCallbackExceptionNode : public rclcpp_lifecycle::LifecycleNode { public: @@ -91,6 +100,7 @@ protected: return RCL_LIFECYCLE_RET_FAILURE; } }; + TEST_F(TestCallbackExceptions, negative_on_error) { auto test_node = std::make_shared("testnode"); @@ -100,3 +110,12 @@ TEST_F(TestCallbackExceptions, negative_on_error) { // check if all callbacks were successfully overwritten EXPECT_EQ(static_cast(2), test_node->number_of_callbacks); } + +TEST_F(TestCallbackExceptions, negative_on_error_with_code) { + auto test_node = std::make_shared("testnode"); + + EXPECT_EQ(State::PRIMARY_STATE_UNCONFIGURED, test_node->get_current_state().id()); + rcl_lifecycle_ret_t ret = RCL_RET_OK; + test_node->configure(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_ERROR, ret); +} diff --git a/rclcpp_lifecycle/test/test_lifecycle_node.cpp b/rclcpp_lifecycle/test/test_lifecycle_node.cpp index c6f5e4e..7d0ae4d 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_node.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_node.cpp @@ -35,10 +35,6 @@ struct BadMood { static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_FAILURE; }; -struct VeryBadMood -{ - static constexpr rcl_lifecycle_ret_t cb_ret = RCL_LIFECYCLE_RET_ERROR; -}; class TestDefaultStateMachine : public ::testing::Test { @@ -144,6 +140,30 @@ TEST_F(TestDefaultStateMachine, trigger_transition) { rclcpp_lifecycle::Transition(Transition::TRANSITION_SHUTDOWN)).id()); } +TEST_F(TestDefaultStateMachine, trigger_transition_with_error_code) { + auto test_node = std::make_shared("testnode"); + + rcl_lifecycle_ret_t ret = RCL_LIFECYCLE_RET_ERROR; + test_node->configure(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret); + ret = RCL_LIFECYCLE_RET_ERROR; + + test_node->activate(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret); + ret = RCL_LIFECYCLE_RET_ERROR; + + test_node->deactivate(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret); + ret = RCL_LIFECYCLE_RET_ERROR; + + test_node->cleanup(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret); + ret = RCL_LIFECYCLE_RET_ERROR; + + test_node->shutdown(ret); + EXPECT_EQ(RCL_LIFECYCLE_RET_OK, ret); +} + TEST_F(TestDefaultStateMachine, good_mood) { auto test_node = std::make_shared>("testnode");