From e4ce07be985794436fc6a8ffef90232fadab3cba Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 2 Aug 2017 14:05:19 -0700 Subject: [PATCH] Rename data types (#155) * change RCL_LIFECYCLE_RET_T to lifecycle_msgs * logging macros --- lifecycle/src/lifecycle_listener.cpp | 10 ++- lifecycle/src/lifecycle_service_client.cpp | 34 ++++----- lifecycle/src/lifecycle_talker.cpp | 83 ++++++++++++---------- 3 files changed, 71 insertions(+), 56 deletions(-) diff --git a/lifecycle/src/lifecycle_listener.cpp b/lifecycle/src/lifecycle_listener.cpp index 4f85f3da3..2ab24c578 100644 --- a/lifecycle/src/lifecycle_listener.cpp +++ b/lifecycle/src/lifecycle_listener.cpp @@ -16,7 +16,11 @@ #include #include "lifecycle_msgs/msg/transition_event.hpp" + #include "rclcpp/rclcpp.hpp" + +#include "rcutils/logging_macros.h" + #include "std_msgs/msg/string.hpp" /// LifecycleListener class as a simple listener node @@ -47,13 +51,13 @@ class LifecycleListener : public rclcpp::node::Node void data_callback(const std_msgs::msg::String::SharedPtr msg) { - printf("[%s] data_callback: %s\n", get_name(), msg->data.c_str()); + RCUTILS_LOG_INFO_NAMED(get_name(), "data_callback: %s", msg->data.c_str()) } void notification_callback(const lifecycle_msgs::msg::TransitionEvent::SharedPtr msg) { - printf("[%s] notify callback: Transition from state %s to %s\n", get_name(), - msg->start_state.label.c_str(), msg->goal_state.label.c_str()); + RCUTILS_LOG_INFO_NAMED(get_name(), "notify callback: Transition from state %s to %s", + msg->start_state.label.c_str(), msg->goal_state.label.c_str()) } private: diff --git a/lifecycle/src/lifecycle_service_client.cpp b/lifecycle/src/lifecycle_service_client.cpp index bef117bf1..43847d9c2 100644 --- a/lifecycle/src/lifecycle_service_client.cpp +++ b/lifecycle/src/lifecycle_service_client.cpp @@ -24,6 +24,8 @@ #include "rclcpp/rclcpp.hpp" +#include "rcutils/logging_macros.h" + using namespace std::chrono_literals; // which node to handle @@ -95,8 +97,8 @@ class LifecycleServiceClient : public rclcpp::node::Node auto request = std::make_shared(); if (!client_get_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_get_state_->get_service_name().c_str()); + RCUTILS_LOG_ERROR("Service %s is not available.", + client_get_state_->get_service_name().c_str()) return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } @@ -109,19 +111,19 @@ class LifecycleServiceClient : public rclcpp::node::Node auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { - fprintf(stderr, "[%s] Failed to get current state for node %s. Server timed out.\n", - get_name(), lifecycle_node); + RCUTILS_LOG_ERROR_NAMED( + get_name(), "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()) { - printf("[%s] Node %s has current state %s.\n", - get_name(), lifecycle_node, future_result.get()->current_state.label.c_str()); + RCUTILS_LOG_INFO_NAMED(get_name(), "Node %s has current state %s.", + lifecycle_node, future_result.get()->current_state.label.c_str()) return future_result.get()->current_state.id; } else { - fprintf(stderr, "[%s] Failed to get current state for node %s\n", - get_name(), lifecycle_node); + RCUTILS_LOG_ERROR_NAMED( + get_name(), "Failed to get current state for node %s", lifecycle_node) return lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN; } } @@ -149,8 +151,8 @@ class LifecycleServiceClient : public rclcpp::node::Node request->transition.id = transition; if (!client_change_state_->wait_for_service(time_out)) { - fprintf(stderr, "Service %s is not available.\n", - client_change_state_->get_service_name().c_str()); + RCUTILS_LOG_ERROR("Service %s is not available.", + client_change_state_->get_service_name().c_str()) return false; } @@ -162,19 +164,19 @@ class LifecycleServiceClient : public rclcpp::node::Node auto future_status = wait_for_result(future_result, time_out); if (future_status != std::future_status::ready) { - fprintf(stderr, "[%s] Failed to change state for node %s. Server timed out.\n", - get_name(), lifecycle_node); + RCUTILS_LOG_ERROR_NAMED( + get_name(), "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) { - printf("[%s] Transition %d successfully triggered.\n", - get_name(), static_cast(transition)); + RCUTILS_LOG_INFO_NAMED( + get_name(), "Transition %d successfully triggered.", static_cast(transition)) return true; } else { - fprintf(stderr, "[%s] Failed to trigger transition %u\n", - get_name(), static_cast(transition)); + RCUTILS_LOG_WARN_NAMED( + get_name(), "Failed to trigger transition %u", static_cast(transition)); return false; } } diff --git a/lifecycle/src/lifecycle_talker.cpp b/lifecycle/src/lifecycle_talker.cpp index 7f9f24d60..5c0e4eebb 100644 --- a/lifecycle/src/lifecycle_talker.cpp +++ b/lifecycle/src/lifecycle_talker.cpp @@ -18,12 +18,16 @@ #include #include +#include "lifecycle_msgs/msg/transition.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp/publisher.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rcutils/logging_macros.h" + #include "std_msgs/msg/string.hpp" using namespace std::chrono_literals; @@ -66,18 +70,19 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * lifecycle publisher is not activate, we still invoke publish, but * the communication is blocked so that no messages is actually transferred. */ - void publish() + void + publish() { static size_t count = 0; msg_->data = "Lifecycle HelloWorld #" + std::to_string(++count); // Print the current state for demo purposes if (!pub_->is_activated()) { - printf("[%s] Lifecycle publisher is currently inactive. Messages are not published.\n", - get_name()); + RCUTILS_LOG_INFO_NAMED( + get_name(), "Lifecycle publisher is currently inactive. Messages are not published.") } else { - printf("[%s] Lifecycle publisher is active. Publishing: [%s]\n", - get_name(), msg_->data.c_str()); + RCUTILS_LOG_INFO_NAMED( + get_name(), "Lifecycle publisher is active. Publishing: [%s]", msg_->data.c_str()) } // We independently from the current state call publish on the lifecycle @@ -94,11 +99,12 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * Depending on the return value of this function, the state machine * either invokes a transition to the "inactive" state or stays * in "unconfigured". - * RCL_LIFECYCLE_RET_OK transitions to "inactive" - * RCL_LIFECYCLE_RET_FAILURE transitions to "unconfigured" - * RCL_LIFECYCLE_RET_ERROR or any uncaught exceptions to "errorprocessing" + * TRANSITION_CALLBACK_SUCCESS transitions to "inactive" + * TRANSITION_CALLBACK_FAILURE transitions to "unconfigured" + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" */ - rcl_lifecycle_ret_t on_configure(const rclcpp_lifecycle::State &) + rcl_lifecycle_transition_key_t + on_configure(const rclcpp_lifecycle::State &) { // This callback is supposed to be used for initialization and // configuring purposes. @@ -113,15 +119,15 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode timer_ = this->create_wall_timer( 1s, std::bind(&LifecycleTalker::publish, this)); - printf("[%s] on_configure() is called.\n", get_name()); + RCUTILS_LOG_INFO_NAMED(get_name(), "on_configure() is called.") // We return a success and hence invoke the transition to the next // step: "inactive". - // If we returned RCL_LIFECYCLE_RET_FAILURE instead, the state machine + // If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine // would stay in the "unconfigured" state. - // In case of RCL_LIFECYCLE_RET_ERROR or any thrown exception within + // In case of TRANSITION_CALLBACK_ERROR or any thrown exception within // this callback, the state machine transitions to state "errorprocessing". - return RCL_LIFECYCLE_RET_OK; + return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; } /// Transition callback for state activating @@ -131,18 +137,19 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * Depending on the return value of this function, the state machine * either invokes a transition to the "active" state or stays * in "inactive". - * RCL_LIFECYCLE_RET_OK transitions to "active" - * RCL_LIFECYCLE_RET_FAILURE transitions to "inactive" - * RCL_LIFECYCLE_RET_ERROR or any uncaught exceptions to "errorprocessing" + * TRANSITION_CALLBACK_SUCCESS transitions to "active" + * TRANSITION_CALLBACK_FAILURE transitions to "inactive" + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" */ - rcl_lifecycle_ret_t on_activate(const rclcpp_lifecycle::State &) + rcl_lifecycle_transition_key_t + on_activate(const rclcpp_lifecycle::State &) { // We explicitly activate the lifecycle publisher. // Starting from this point, all messages are no longer // ignored but sent into the network. pub_->on_activate(); - printf("[%s] on_activate() is called.\n", get_name()); + RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called.") // Let's sleep for 2 seconds. // We emulate we are doing important @@ -151,11 +158,11 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode // We return a success and hence invoke the transition to the next // step: "active". - // If we returned RCL_LIFECYCLE_RET_FAILURE instead, the state machine + // If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine // would stay in the "inactive" state. - // In case of RCL_LIFECYCLE_RET_ERROR or any thrown exception within + // In case of TRANSITION_CALLBACK_ERROR or any thrown exception within // this callback, the state machine transitions to state "errorprocessing". - return RCL_LIFECYCLE_RET_OK; + return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; } /// Transition callback for state activating @@ -165,26 +172,27 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * Depending on the return value of this function, the state machine * either invokes a transition to the "inactive" state or stays * in "active". - * RCL_LIFECYCLE_RET_OK transitions to "inactive" - * RCL_LIFECYCLE_RET_FAILURE transitions to "active" - * RCL_LIFECYCLE_RET_ERROR or any uncaught exceptions to "errorprocessing" + * TRANSITION_CALLBACK_SUCCESS transitions to "inactive" + * TRANSITION_CALLBACK_FAILURE transitions to "active" + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" */ - rcl_lifecycle_ret_t on_deactivate(const rclcpp_lifecycle::State &) + rcl_lifecycle_transition_key_t + on_deactivate(const rclcpp_lifecycle::State &) { // We explicitly deactivate the lifecycle publisher. // Starting from this point, all messages are no longer // sent into the network. pub_->on_deactivate(); - printf("[%s] on_deactivate() is called.\n", get_name()); + RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.") // We return a success and hence invoke the transition to the next // step: "inactive". - // If we returned RCL_LIFECYCLE_RET_FAILURE instead, the state machine + // If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine // would stay in the "active" state. - // In case of RCL_LIFECYCLE_RET_ERROR or any thrown exception within + // In case of TRANSITION_CALLBACK_ERROR or any thrown exception within // this callback, the state machine transitions to state "errorprocessing". - return RCL_LIFECYCLE_RET_OK; + return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; } /// Transition callback for state deactivating @@ -194,11 +202,12 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode * Depending on the return value of this function, the state machine * either invokes a transition to the "uncofigured" state or stays * in "inactive". - * RCL_LIFECYCLE_RET_OK transitions to "unconfigured" - * RCL_LIFECYCLE_RET_FAILURE transitions to "inactive" - * RCL_LIFECYCLE_RET_ERROR or any uncaught exceptions to "errorprocessing" + * TRANSITION_CALLBACK_SUCCESS transitions to "unconfigured" + * TRANSITION_CALLBACK_FAILURE transitions to "inactive" + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" */ - rcl_lifecycle_ret_t on_cleanup(const rclcpp_lifecycle::State &) + rcl_lifecycle_transition_key_t + on_cleanup(const rclcpp_lifecycle::State &) { // In our cleanup phase, we release the shared pointers to the // timer and publisher. These entities are no longer available @@ -206,15 +215,15 @@ class LifecycleTalker : public rclcpp_lifecycle::LifecycleNode timer_.reset(); pub_.reset(); - printf("[%s] on cleanup is called.\n", get_name()); + RCUTILS_LOG_INFO_NAMED(get_name(), "on cleanup is called.") // We return a success and hence invoke the transition to the next // step: "unconfigured". - // If we returned RCL_LIFECYCLE_RET_FAILURE instead, the state machine + // If we returned TRANSITION_CALLBACK_FAILURE instead, the state machine // would stay in the "inactive" state. - // In case of RCL_LIFECYCLE_RET_ERROR or any thrown exception within + // In case of TRANSITION_CALLBACK_ERROR or any thrown exception within // this callback, the state machine transitions to state "errorprocessing". - return RCL_LIFECYCLE_RET_OK; + return lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS; } private: