1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00

Move debug output to DEBUG stream

This commit is contained in:
Felix Mauch
2019-06-12 09:52:03 +02:00
committed by Tristan Schnell
parent 00d20cb501
commit 75d6ae0a19
5 changed files with 12 additions and 14 deletions

View File

@@ -154,7 +154,7 @@ public:
virtual ~Pipeline() virtual ~Pipeline()
{ {
LOG_INFO("Destructing pipeline"); LOG_DEBUG("Destructing pipeline");
stop(); stop();
} }
@@ -176,7 +176,7 @@ public:
if (!running_) if (!running_)
return; return;
LOG_INFO("Stopping pipeline! <%s>", name_.c_str()); LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
if (consumer_ != nullptr) if (consumer_ != nullptr)
consumer_->stopConsumer(); consumer_->stopConsumer();
@@ -208,7 +208,7 @@ private:
void runProducer() void runProducer()
{ {
LOG_INFO("Starting up producer"); LOG_DEBUG("Starting up producer");
std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in);
bool has_realtime; bool has_realtime;
realtime_file >> has_realtime; realtime_file >> has_realtime;

View File

@@ -56,7 +56,7 @@ bool TCPSocket::setup(std::string& host, int port)
if (state_ == SocketState::Connected) if (state_ == SocketState::Connected)
return false; 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: // gethostbyname() is deprecated so use getadderinfo() as described in:
// http://www.beej.us/guide/bgnet/output/html/multipage/syscalls.html#getaddrinfo // 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_); setOptions(socket_fd_);
state_ = SocketState::Connected; 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; return connected;
} }

View File

@@ -154,7 +154,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
// Create ros_control interfaces // Create ros_control interfaces
for (std::size_t i = 0; i < joint_positions_.size(); ++i) 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 // Create joint state interface for all joints
js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i], js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i],
&joint_velocities_[i], &joint_efforts_[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<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) || (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) ||
runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING))) runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)))
{ {
// ROS_INFO_STREAM("Writing command: " << joint_position_command_);
ur_driver_->writeJointCommand(joint_position_command_); ur_driver_->writeJointCommand(joint_position_command_);
} }
} }

View File

@@ -69,9 +69,8 @@ int main(int argc, char** argv)
ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting"); ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
exit(1); exit(1);
} }
ROS_INFO_STREAM("initialized hw interface"); ROS_DEBUG_STREAM("initialized hw interface");
controller_manager::ControllerManager cm(g_hw_interface.get(), nh); controller_manager::ControllerManager cm(g_hw_interface.get(), nh);
ROS_INFO_STREAM("started controller manager");
// Get current time and elapsed time since last read // Get current time and elapsed time since last read
timestamp = ros::Time::now(); timestamp = ros::Time::now();
@@ -130,7 +129,6 @@ int main(int argc, char** argv)
double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency())); double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
ROS_INFO("Starting control loop");
// Run as fast as possible // Run as fast as possible
while (ros::ok()) while (ros::ok())
{ {

View File

@@ -53,7 +53,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
, reverse_interface_active_(false) , reverse_interface_active_(false)
, handle_program_state_(handle_program_state) , 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)); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file));
if (!rtde_client_->init()) 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_.reset(new comm::ScriptSender(script_sender_port, prog));
script_sender_->start(); script_sender_->start();
LOG_INFO("Created script sender"); LOG_DEBUG("Created script sender");
reverse_port_ = reverse_port; reverse_port_ = reverse_port;
watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this); watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this);
rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface) rtde_client_->start(); // TODO: Add extra start method (also to HW-Interface)
LOG_INFO("Initialization done"); LOG_DEBUG("Initialization done");
} }
std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage() std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
@@ -152,7 +153,7 @@ void UrDriver::startWatchdog()
handle_program_state_(false); handle_program_state_(false);
reverse_interface_.reset(new comm::ReverseInterface(reverse_port_)); reverse_interface_.reset(new comm::ReverseInterface(reverse_port_));
reverse_interface_active_ = true; reverse_interface_active_ = true;
LOG_INFO("Created reverse interface"); LOG_DEBUG("Created reverse interface");
while (true) while (true)
{ {