Skip to content
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 12 additions & 4 deletions rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -397,6 +397,7 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
constexpr bool publish_update = true;
State initial_state;
unsigned int current_state_id;
const rcl_lifecycle_transition_t * original_transition{nullptr};

{
std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
Expand All @@ -411,6 +412,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
// keep the initial state to pass to a transition callback
initial_state = State(state_machine_.current_state);

original_transition =
rcl_lifecycle_get_transition_by_id(state_machine_.current_state, transition_id);


if (
rcl_lifecycle_trigger_transition_by_id(
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
Expand Down Expand Up @@ -461,12 +466,15 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state(
// Update the internal current_state_
current_state_ = State(state_machine_.current_state);

// error handling ?!
// error handling
// TODO(karsten1987): iterate over possible ret value
if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
RCLCPP_WARN(
node_logging_interface_->get_logger(),
"Error occurred while doing error handling.");
if (original_transition) {
RCLCPP_WARN(
node_logging_interface_->get_logger(),
"Callback returned ERROR during the transition: %s", original_transition->label);
}


auto error_cb_code = execute_callback(current_state_id, initial_state);
auto error_cb_label = get_label_for_return_code(error_cb_code);
Expand Down