add some output for knowing whats going on

StoglRobotics-forks · Jan 11, 2023 · 71f4757 · 71f4757
1 parent 167e8dc
commit 71f4757
Show file tree

Hide file tree

Showing 4 changed files with 32 additions and 21 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -453,15 +453,17 @@ 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());
     }
   }
 }
 
 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<controller_manager_msgs::srv::RegisterSubControllerManager>::SharedPtr client =
     create_client<controller_manager_msgs::srv::RegisterSubControllerManager>(
       "/register_sub_controller_manager");
@@ -495,16 +497,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);
@@ -532,23 +536,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
@@ -396,7 +396,7 @@ class DistributedReadWriteHandle : public ReadWriteHandle
     auto msg = std::make_unique<std_msgs::msg::Float64>();
     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/...ware_interface/src/hardware_interface/distributed_control_interface/command_forwarder.cpp b/...ware_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<double>::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
@@ -89,7 +89,7 @@ void StatePublisher::publish_value_on_timer()
     // Make explicit note implicit!!!
     msg->data = std::numeric_limits<double>::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.