From 75d6ae0a19a0f82e50999acc9f93ae1e69a8f270 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 12 Jun 2019 09:52:03 +0200 Subject: [PATCH] Move debug output to DEBUG stream --- ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h | 6 +++--- ur_rtde_driver/src/comm/tcp_socket.cpp | 4 ++-- ur_rtde_driver/src/ros/hardware_interface.cpp | 3 +-- ur_rtde_driver/src/ros/hardware_interface_node.cpp | 4 +--- ur_rtde_driver/src/ur/ur_driver.cpp | 9 +++++---- 5 files changed, 12 insertions(+), 14 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h index 608da29..6fdd0f4 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/pipeline.h @@ -154,7 +154,7 @@ public: virtual ~Pipeline() { - LOG_INFO("Destructing pipeline"); + LOG_DEBUG("Destructing pipeline"); stop(); } @@ -176,7 +176,7 @@ public: if (!running_) return; - LOG_INFO("Stopping pipeline! <%s>", name_.c_str()); + LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); if (consumer_ != nullptr) consumer_->stopConsumer(); @@ -208,7 +208,7 @@ private: void runProducer() { - LOG_INFO("Starting up producer"); + LOG_DEBUG("Starting up producer"); std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); bool has_realtime; realtime_file >> has_realtime; diff --git a/ur_rtde_driver/src/comm/tcp_socket.cpp b/ur_rtde_driver/src/comm/tcp_socket.cpp index a8337e3..93f6a84 100644 --- a/ur_rtde_driver/src/comm/tcp_socket.cpp +++ b/ur_rtde_driver/src/comm/tcp_socket.cpp @@ -56,7 +56,7 @@ bool TCPSocket::setup(std::string& host, int port) if (state_ == SocketState::Connected) return false; - LOG_INFO("Setting up connection: %s:%d", host.c_str(), port); + LOG_DEBUG("Setting up connection: %s:%d", host.c_str(), port); // gethostbyname() is deprecated so use getadderinfo() as described in: // http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo @@ -100,7 +100,7 @@ bool TCPSocket::setup(std::string& host, int port) { setOptions(socket_fd_); state_ = SocketState::Connected; - LOG_INFO("Connection established for %s:%d", host.c_str(), port); + LOG_DEBUG("Connection established for %s:%d", host.c_str(), port); } return connected; } diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index c27a770..a5b4de5 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -154,7 +154,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h // Create ros_control interfaces for (std::size_t i = 0; i < joint_positions_.size(); ++i) { - ROS_INFO_STREAM("Registing handles for joint " << joint_names_[i]); + ROS_DEBUG_STREAM("Registing handles for joint " << joint_names_[i]); // Create joint state interface for all joints js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i], &joint_velocities_[i], &joint_efforts_[i])); @@ -275,7 +275,6 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PLAYING) || runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSING))) { - // ROS_INFO_STREAM("Writing command: " << joint_position_command_); ur_driver_->writeJointCommand(joint_position_command_); } } diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp index de33da5..8924d4c 100644 --- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp @@ -69,9 +69,8 @@ int main(int argc, char** argv) ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting"); exit(1); } - ROS_INFO_STREAM("initialized hw interface"); + ROS_DEBUG_STREAM("initialized hw interface"); controller_manager::ControllerManager cm(g_hw_interface.get(), nh); - ROS_INFO_STREAM("started controller manager"); // Get current time and elapsed time since last read timestamp = ros::Time::now(); @@ -130,7 +129,6 @@ int main(int argc, char** argv) double expected_cycle_time = 1.0 / (static_cast(g_hw_interface->getControlFrequency())); - ROS_INFO("Starting control loop"); // Run as fast as possible while (ros::ok()) { diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index d7cd03c..479fe09 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -53,7 +53,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc , reverse_interface_active_(false) , handle_program_state_(handle_program_state) { - LOG_INFO("Initializing RTDE client"); + LOG_DEBUG("Initializing urdriver"); + LOG_DEBUG("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file)); if (!rtde_client_->init()) @@ -106,13 +107,13 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc script_sender_.reset(new comm::ScriptSender(script_sender_port, prog)); script_sender_->start(); - LOG_INFO("Created script sender"); + LOG_DEBUG("Created script sender"); reverse_port_ = reverse_port; watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this); rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface) - LOG_INFO("Initialization done"); + LOG_DEBUG("Initialization done"); } std::unique_ptr ur_driver::UrDriver::getDataPackage() @@ -152,7 +153,7 @@ void UrDriver::startWatchdog() handle_program_state_(false); reverse_interface_.reset(new comm::ReverseInterface(reverse_port_)); reverse_interface_active_ = true; - LOG_INFO("Created reverse interface"); + LOG_DEBUG("Created reverse interface"); while (true) {