expose error handling for state changes (#344)

* remove fprintf, use logging

* expose lifecycle error code

* address comments

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

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

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
+  rcl_ret_t
-  change_state(std::uint8_t lifecycle_transition)
+  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",
+      RCUTILS_LOG_ERROR("Unable to change state for state machine for %s: %s",
-        __FILE__, __LINE__, node_base_interface_->get_name(), rcl_get_error_string_safe());
+        node_base_interface_->get_name(), rcl_get_error_string_safe())
-      return false;
+      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",
+      RCUTILS_LOG_ERROR("Unable to start transition %u from current state %s: %s",
-        __FILE__, __LINE__, transition_id,
+        transition_id, state_machine_.current_state->label, rcl_get_error_string_safe())
-        state_machine_.current_state->label, rcl_get_error_string_safe());
+      return RCL_RET_ERROR;
-      return false;
     }
 
-    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",
+      RCUTILS_LOG_ERROR("Failed to finish transition %u. Current state is now: %s",
-        transition_id, state_machine_.current_state->label);
+        transition_id, state_machine_.current_state->label)
-      return false;
+      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) {
+      if (error_resolved == RCL_LIFECYCLE_RET_OK) {
-        // fprintf(stderr, "Exception handling was successful\n");
         // 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");
+          RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
-          return false;
+          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");
+          RCUTILS_LOG_ERROR("Failed to call cleanup on error state")
-          return false;
+          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();
   }
 

rclcpp_lifecycle/test/test_callback_exceptions.cpp

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

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<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");