Skip to content

Commit

Permalink
Switch lifecycle to use the RCLCPP macros Signed-off-by: Tony Najjar …
Browse files Browse the repository at this point in the history 
…<tony.najjar.1997@gmail.com>
  • Loading branch information
Tony Najjar committed Jul 7, 2023
1 parent 25263e8 commit 766c69a
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 12 deletions.
2 changes: 1 addition & 1 deletion rclcpp_lifecycle/src/lifecycle_node.cpp
View file
Expand Up @@ -107,7 +107,7 @@ LifecycleNode::LifecycleNode(
)),
node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
node_options_(options),
impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_))
impl_(new LifecycleNodeInterfaceImpl(node_base_, node_services_, node_logging_))
{
impl_->init(enable_communication_interface);

Expand Down
38 changes: 27 additions & 11 deletions rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp
View file
Expand Up @@ -39,6 +39,7 @@
#include "rcl_lifecycle/transition_map.h"

#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"

#include "rcutils/logging_macros.h"
Expand All @@ -57,9 +58,11 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
public:
LifecycleNodeInterfaceImpl(
std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface> node_base_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface)
std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface> node_services_interface,
std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface> node_logging_interface)
: node_base_interface_(node_base_interface),
node_services_interface_(node_services_interface)
node_services_interface_(node_services_interface),
node_logging_interface_(node_logging_interface)
{}

~LifecycleNodeInterfaceImpl()
Expand All @@ -71,8 +74,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
ret = rcl_lifecycle_state_machine_fini(&state_machine_, node_handle);
}
if (ret != RCL_RET_OK) {
RCUTILS_LOG_FATAL_NAMED(
"rclcpp_lifecycle",
RCLCPP_FATAL(
node_logging_interface_->get_logger(),
"failed to destroy rcl_state_machine");
}
}
Expand Down Expand Up @@ -402,7 +405,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
{
std::lock_guard<std::recursive_mutex> lock(state_machine_mutex_);
if (rcl_lifecycle_state_machine_is_initialized(&state_machine_) != RCL_RET_OK) {
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Unable to change state for state machine for %s: %s",
node_base_interface_->get_name(), rcl_get_error_string().str);
return RCL_RET_ERROR;
Expand All @@ -414,7 +418,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
rcl_lifecycle_trigger_transition_by_id(
&state_machine_, transition_id, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Unable to start transition %u from current state %s: %s",
transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
rcutils_reset_error();
Expand Down Expand Up @@ -443,7 +448,8 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
rcl_lifecycle_trigger_transition_by_label(
&state_machine_, transition_label, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR(
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Failed to finish transition %u. Current state is now: %s (%s)",
transition_id, state_machine_.current_state->label, rcl_get_error_string().str);
rcutils_reset_error();
Expand All @@ -455,7 +461,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
// error handling ?!
// TODO(karsten1987): iterate over possible ret value
if (cb_return_code == node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR) {
RCUTILS_LOG_WARN("Error occurred while doing error handling.");
RCLCPP_WARN(
node_logging_interface_->get_logger(),
"Error occurred while doing error handling.");

auto error_cb_code = execute_callback(current_state_id, initial_state);
auto error_cb_label = get_label_for_return_code(error_cb_code);
Expand All @@ -464,7 +472,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
rcl_lifecycle_trigger_transition_by_label(
&state_machine_, error_cb_label, publish_update) != RCL_RET_OK)
{
RCUTILS_LOG_ERROR("Failed to call cleanup on error state: %s", rcl_get_error_string().str);
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Failed to call cleanup on error state: %s", rcl_get_error_string().str);
rcutils_reset_error();
return RCL_RET_ERROR;
}
Expand All @@ -487,8 +497,12 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
try {
cb_success = callback(State(previous_state));
} catch (const std::exception & e) {
RCUTILS_LOG_ERROR("Caught exception in callback for transition %d", it->first);
RCUTILS_LOG_ERROR("Original error: %s", e.what());
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Caught exception in callback for transition %d", it->first);
RCLCPP_ERROR(
node_logging_interface_->get_logger(),
"Original error: %s", e.what());
cb_success = node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
}
Expand Down Expand Up @@ -576,6 +590,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl

using NodeBasePtr = std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>;
using NodeServicesPtr = std::shared_ptr<rclcpp::node_interfaces::NodeServicesInterface>;
using NodeLoggingPtr = std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>;
using ChangeStateSrvPtr = std::shared_ptr<rclcpp::Service<ChangeStateSrv>>;
using GetStateSrvPtr = std::shared_ptr<rclcpp::Service<GetStateSrv>>;
using GetAvailableStatesSrvPtr =
Expand All @@ -587,6 +602,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl

NodeBasePtr node_base_interface_;
NodeServicesPtr node_services_interface_;
NodeLoggingPtr node_logging_interface_;
ChangeStateSrvPtr srv_change_state_;
GetStateSrvPtr srv_get_state_;
GetAvailableStatesSrvPtr srv_get_available_states_;
Expand Down

0 comments on commit 766c69a

Please sign in to comment.