diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 80ec76a769..50d4717ec3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -839,7 +839,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, */ RCLCPP_LIFECYCLE_PUBLIC const State & - get_current_state(); + get_current_state() const; /// Return a list with the available states. /** diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index f2db5f3950..773febda95 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -535,7 +535,7 @@ LifecycleNode::register_on_error( } const State & -LifecycleNode::get_current_state() +LifecycleNode::get_current_state() const { return impl_->get_current_state(); } diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp index f176cbf535..30c0ba3c0e 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.cpp @@ -182,6 +182,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::init(bool enable_communication_interf nullptr); } } + + current_state_ = State(state_machine_.current_state); } bool @@ -327,9 +329,8 @@ LifecycleNode::LifecycleNodeInterfaceImpl::on_get_transition_graph( } const State & -LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() +LifecycleNode::LifecycleNodeInterfaceImpl::get_current_state() const { - current_state_ = State(state_machine_.current_state); return current_state_; } @@ -396,6 +397,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + auto get_label_for_return_code = [](node_interfaces::LifecycleNodeInterface::CallbackReturn cb_return_code) -> const char *{ auto cb_id = static_cast(cb_return_code); @@ -421,6 +425,9 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // error handling ?! // TODO(karsten1987): iterate over possible ret value if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) { @@ -437,6 +444,10 @@ LifecycleNode::LifecycleNodeInterfaceImpl::change_state( return RCL_RET_ERROR; } } + + // Update the internal current_state_ + current_state_ = State(state_machine_.current_state); + // 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 diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index d973278005..9777bbf3a8 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -65,7 +65,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl final std::function & cb); const State & - get_current_state(); + get_current_state() const; std::vector get_available_states() const;