Skip to content

Commit

Permalink
Swap lifecycle nodes to using node loggers
Browse files Browse the repository at this point in the history
  • Loading branch information
dhood committed Nov 30, 2017
1 parent 6615c89 commit addbbf0
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 20 deletions.
4 changes: 2 additions & 2 deletions lifecycle/src/lifecycle_listener.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,12 +51,12 @@ class LifecycleListener : public rclcpp::Node

void data_callback(const std_msgs::msg::String::SharedPtr msg)
{
RCUTILS_LOG_INFO_NAMED(get_name(), "data_callback: %s", msg->data.c_str())
RCLCPP_INFO(get_logger(), "data_callback: %s", msg->data.c_str())
}

void notification_callback(const lifecycle_msgs::msg::TransitionEvent::SharedPtr msg)
{
RCUTILS_LOG_INFO_NAMED(get_name(), "notify callback: Transition from state %s to %s",
RCLCPP_INFO(get_logger(), "notify callback: Transition from state %s to %s",
msg->start_state.label.c_str(), msg->goal_state.label.c_str())
}

Expand Down
30 changes: 17 additions & 13 deletions lifecycle/src/lifecycle_service_client.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,9 @@ class LifecycleServiceClient : public rclcpp::Node
auto request = std::make_shared<lifecycle_msgs::srv::GetState::Request>();

if (!client_get_state_->wait_for_service(time_out)) {
RCUTILS_LOG_ERROR("Service %s is not available.",
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_get_state_->get_service_name().c_str())
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}
Expand All @@ -111,19 +113,19 @@ class LifecycleServiceClient : public rclcpp::Node
auto future_status = wait_for_result(future_result, time_out);

if (future_status != std::future_status::ready) {
RCUTILS_LOG_ERROR_NAMED(
get_name(), "Server time out while getting current state for node %s", lifecycle_node)
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node)
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}

// We have an succesful answer. So let's print the current state.
if (future_result.get()) {
RCUTILS_LOG_INFO_NAMED(get_name(), "Node %s has current state %s.",
RCLCPP_INFO(get_logger(), "Node %s has current state %s.",
lifecycle_node, future_result.get()->current_state.label.c_str())
return future_result.get()->current_state.id;
} else {
RCUTILS_LOG_ERROR_NAMED(
get_name(), "Failed to get current state for node %s", lifecycle_node)
RCLCPP_ERROR(
get_logger(), "Failed to get current state for node %s", lifecycle_node)
return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
}
}
Expand Down Expand Up @@ -151,7 +153,9 @@ class LifecycleServiceClient : public rclcpp::Node
request->transition.id = transition;

if (!client_change_state_->wait_for_service(time_out)) {
RCUTILS_LOG_ERROR("Service %s is not available.",
RCLCPP_ERROR(
get_logger(),
"Service %s is not available.",
client_change_state_->get_service_name().c_str())
return false;
}
Expand All @@ -164,19 +168,19 @@ class LifecycleServiceClient : public rclcpp::Node
auto future_status = wait_for_result(future_result, time_out);

if (future_status != std::future_status::ready) {
RCUTILS_LOG_ERROR_NAMED(
get_name(), "Server time out while getting current state for node %s", lifecycle_node)
RCLCPP_ERROR(
get_logger(), "Server time out while getting current state for node %s", lifecycle_node)
return false;
}

// We have an answer, let's print our success.
if (future_result.get()->success) {
RCUTILS_LOG_INFO_NAMED(
get_name(), "Transition %d successfully triggered.", static_cast<int>(transition))
RCLCPP_INFO(
get_logger(), "Transition %d successfully triggered.", static_cast<int>(transition))
return true;
} else {
RCUTILS_LOG_WARN_NAMED(
get_name(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
RCLCPP_WARN(
get_logger(), "Failed to trigger transition %u", static_cast<unsigned int>(transition));
return false;
}
}
Expand Down
10 changes: 5 additions & 5 deletions lifecycle/src/lifecycle_talker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -78,11 +78,11 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode

// Print the current state for demo purposes
if (!pub_->is_activated()) {
RCUTILS_LOG_INFO_NAMED(
get_name(), "Lifecycle publisher is currently inactive. Messages are not published.")
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is currently inactive. Messages are not published.")
} else {
RCUTILS_LOG_INFO_NAMED(
get_name(), "Lifecycle publisher is active. Publishing: [%s]", msg_->data.c_str())
RCLCPP_INFO(
get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg_->data.c_str())
}

// We independently from the current state call publish on the lifecycle
Expand Down Expand Up @@ -119,7 +119,7 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode
timer_ = this->create_wall_timer(
1s, std::bind(&LifecycleTalker::publish, this));

RCUTILS_LOG_INFO_NAMED(get_name(), "on_configure() is called.")
RCLCPP_INFO(get_logger(), "on_configure() is called.")

// We return a success and hence invoke the transition to the next
// step: "inactive".
Expand Down

0 comments on commit addbbf0

Please sign in to comment.