From d3feeaa8363676b8a7f306c1bb3794776ea77d71 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 21 Dec 2022 13:40:41 +0100 Subject: [PATCH] add some output for knowing whats going on --- controller_manager/src/controller_manager.cpp | 47 ++++++++++++------- .../include/hardware_interface/handle.hpp | 2 +- .../command_forwarder.cpp | 2 +- .../state_publisher.cpp | 2 +- 4 files changed, 32 insertions(+), 21 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6f4a70cb9d8..be6c9458828 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -459,7 +459,7 @@ void ControllerManager::add_hardware_command_forwarders() catch (const std::runtime_error & e) { RCLCPP_WARN_STREAM( - get_logger(), "ControllerManager: Can't create StatePublishers<" + get_logger(), "ControllerManager: Can't create CommandForwarder<" << command_forwarder->command_interface_name() << ">." << e.what()); } } @@ -467,7 +467,9 @@ void ControllerManager::add_hardware_command_forwarders() void ControllerManager::register_sub_controller_manager() { - RCLCPP_INFO(get_logger(), "SubControllerManager: Trying to register StatePublishers."); + RCLCPP_INFO_STREAM( + get_logger(), + "SubControllerManager:<" << get_namespace() << "/" << get_name() << "> trying to register."); rclcpp::Client::SharedPtr client = create_client( "/register_sub_controller_manager"); @@ -501,16 +503,18 @@ void ControllerManager::register_sub_controller_manager() { if (!rclcpp::ok()) { - RCLCPP_ERROR( - get_logger(), - "SubControllerManager: Interrupted while waiting for central controller managers " - "registration service. Exiting."); + RCLCPP_ERROR_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Interrupted while waiting for central controller managers " + "registration service. Exiting."); return; } - RCLCPP_INFO( - get_logger(), - "SubControllerManager:Central controller managers registration service not available, " - "waiting again..."); + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Central controller managers registration service not available, " + "waiting again..."); } auto result = client->async_send_request(request); @@ -538,23 +542,30 @@ void ControllerManager::register_sub_controller_manager() else { RCLCPP_WARN_STREAM( - get_logger(), "SubControllerManager: Could not find a CommandForwarder for key[" - << key << "]. No subscription to command state possible."); + get_logger(), "SubControllerManager:<" + << get_namespace() << "/" << get_name() + << ">. Could not find a CommandForwarder for key[" << key + << "]. No subscription to command state possible."); } } - RCLCPP_INFO(get_logger(), "SubControllerManager: Successfully registered StatePublishers."); + RCLCPP_INFO_STREAM( + get_logger(), "SubControllerManager:<" << get_namespace() << "/" << get_name() + << ">. Successfully registered."); } else { - RCLCPP_WARN( - get_logger(), - "SubControllerManager: Registration of StatePublishers failed. Central ControllerManager " - "returned error code."); + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" + << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed. Central ControllerManager " + "returned error code."); } } else { - RCLCPP_WARN(get_logger(), "SubControllerManager: Registration of StatePublishers failed."); + RCLCPP_WARN_STREAM( + get_logger(), "SubControllerManager: <" << get_namespace() << "/" << get_name() + << ">. Registration of StatePublishers failed."); } } diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index ac75c91edc8..1e506bf0b24 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -396,7 +396,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle auto msg = std::make_unique(); msg->data = value; - RCLCPP_INFO(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "DistributedCommandInterface Publishing: '%.7lf'", msg->data); std::flush(std::cout); command_value_pub_->publish(std::move(msg)); diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp index fef4114bcce..a8a1eefa983 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp @@ -98,7 +98,7 @@ void CommandForwarder::publish_value_on_timer() { msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware. diff --git a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp index 3504a840980..c84bfee1e5c 100644 --- a/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp +++ b/hardware_interface/src/hardware_interface/distributed_control_interface/state_publisher.cpp @@ -89,7 +89,7 @@ void StatePublisher::publish_value_on_timer() // Make explicit note implicit!!! msg->data = std::numeric_limits::quiet_NaN(); } - RCLCPP_INFO(node_->get_logger(), "Publishing: '%.7lf'", msg->data); + RCLCPP_WARN(node_->get_logger(), "Publishing: '%.7lf'", msg->data); std::flush(std::cout); // Put the message into a queue to be processed by the middleware.