niklas/rclcpp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

expose error handling for state changes (#344)

Browse source 
* remove fprintf, use logging

* expose lifecycle error code

* address comments
This commit is contained in:
Karsten Knese 2017-07-27 07:55:15 -07:00 • committed by GitHub
parent 40b09b5b14
commit 0c26dd99b6
5 changed files with 160 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

28
rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp
View file

@ -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<rcl_lifecycle_ret_t(const State &)> fcn);

49
rclcpp_lifecycle/src/lifecycle_node.cpp
View file

@ -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<rclcpp_lifecycle::LifecyclePublisherInterface> pub)

67
rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
View file

@ -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();
}

19
rclcpp_lifecycle/test/test_callback_exceptions.cpp
View file

@ -69,6 +69,15 @@ TEST_F(TestCallbackExceptions, positive_on_error) {
EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
}
TEST_F(TestCallbackExceptions, positive_on_error_with_code) {
auto test_node = std::make_shared<PositiveCallbackExceptionNode>("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<NegativeCallbackExceptionNode>("testnode");
@ -100,3 +110,12 @@ TEST_F(TestCallbackExceptions, negative_on_error) {
// check if all callbacks were successfully overwritten
EXPECT_EQ(static_cast<size_t>(2), test_node->number_of_callbacks);
}
TEST_F(TestCallbackExceptions, negative_on_error_with_code) {
auto test_node = std::make_shared<NegativeCallbackExceptionNode>("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);
}

28
rclcpp_lifecycle/test/test_lifecycle_node.cpp
View file

@ -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<EmptyLifecycleNode>("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<MoodyLifecycleNode<GoodMood>>("testnode");