diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index fecdcb3b2..3a6686a20 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -164,6 +164,17 @@ class HardwareInterface : public hardware_interface::RobotHW */ void shouldLogTemperature(bool value); + struct readStats { + std::chrono::duration get_data_elapsed; + std::chrono::duration read_data_elapsed; + std::chrono::duration pub_io_elapsed; + std::chrono::duration pub_tool_elapsed; + std::chrono::duration pub_pose_elapsed; + std::chrono::duration pub_robot_elapsed; + std::chrono::duration pub_temp_elapsed; + }; + + readStats get_read_stats(); protected: /*! @@ -302,6 +313,8 @@ class HardwareInterface : public hardware_interface::RobotHW std::string tf_prefix_; bool enable_temperature_log_; + + readStats read_stats_; }; } // namespace ur_driver diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 0d76e77b3..ca04dc3e6 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -442,10 +442,21 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) robot_status_resource_.in_motion = TriState::UNKNOWN; robot_status_resource_.in_error = TriState::UNKNOWN; robot_status_resource_.error_code = 0; - + read_stats_.get_data_elapsed = std::chrono::duration::zero(); + read_stats_.read_data_elapsed = std::chrono::duration::zero(); + read_stats_.pub_io_elapsed = std::chrono::duration::zero(); + read_stats_.pub_tool_elapsed = std::chrono::duration::zero(); + read_stats_.pub_pose_elapsed = std::chrono::duration::zero(); + read_stats_.pub_robot_elapsed = std::chrono::duration::zero(); + read_stats_.pub_temp_elapsed = std::chrono::duration::zero(); + + const std::chrono::steady_clock::time_point get_data_start = std::chrono::steady_clock::now(); std::unique_ptr data_pkg = ur_driver_->getDataPackage(); + read_stats_.get_data_elapsed = std::chrono::steady_clock::now() - get_data_start; + if (data_pkg) { + const std::chrono::steady_clock::time_point read_data_start = std::chrono::steady_clock::now(); packet_read_ = true; readData(data_pkg, "actual_q", joint_positions_); readData(data_pkg, "actual_qd", joint_velocities_); @@ -476,17 +487,28 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) readData(data_pkg, "joint_temperatures", joint_temperatures_); extractRobotStatus(); + read_stats_.read_data_elapsed = std::chrono::steady_clock::now() - read_data_start; + const std::chrono::steady_clock::time_point pub_io_start = std::chrono::steady_clock::now(); publishIOData(); + read_stats_.pub_io_elapsed = std::chrono::steady_clock::now() - pub_io_start; + const std::chrono::steady_clock::time_point pub_tool_start = std::chrono::steady_clock::now(); publishToolData(); + read_stats_.pub_tool_elapsed = std::chrono::steady_clock::now() - pub_tool_start; + const std::chrono::steady_clock::time_point pub_pose_start = std::chrono::steady_clock::now(); // Transform fts measurements to tool frame extractToolPose(time); transformForceTorque(); publishPose(); + read_stats_.pub_pose_elapsed = std::chrono::steady_clock::now() - pub_pose_start; + const std::chrono::steady_clock::time_point pub_robot_start = std::chrono::steady_clock::now(); publishRobotAndSafetyMode(); + read_stats_.pub_robot_elapsed = std::chrono::steady_clock::now() - pub_robot_start; if (this->enable_temperature_log_) { + const std::chrono::steady_clock::time_point pub_temp_start = std::chrono::steady_clock::now(); publishJointTemperatures(time); + read_stats_.pub_temp_elapsed = std::chrono::steady_clock::now() - pub_temp_start; } // pausing state follows runtime state when pausing @@ -693,6 +715,11 @@ bool HardwareInterface::shouldResetControllers() } } +HardwareInterface::readStats HardwareInterface::get_read_stats() +{ + return read_stats_; +} + void HardwareInterface::extractToolPose(const ros::Time& timestamp) { double tcp_angle = std::sqrt(std::pow(tcp_pose_[3], 2) + std::pow(tcp_pose_[4], 2) + std::pow(tcp_pose_[5], 2)); diff --git a/ur_robot_driver/src/ros/hardware_interface_node.cpp b/ur_robot_driver/src/ros/hardware_interface_node.cpp index 3187bc486..381ea878f 100644 --- a/ur_robot_driver/src/ros/hardware_interface_node.cpp +++ b/ur_robot_driver/src/ros/hardware_interface_node.cpp @@ -27,6 +27,10 @@ #include #include +#include +#include +#include + #include #include @@ -43,6 +47,46 @@ void signalHandler(int signum) exit(signum); } +struct msgStats { + std::chrono::duration min_stat; + std::chrono::duration max_stat; + std::chrono::duration total_stat; +}; + +void reset_msg_stats(msgStats &stats) +{ + stats.min_stat = std::chrono::duration::zero(); + stats.max_stat = std::chrono::duration::zero(); + stats.total_stat = std::chrono::duration::zero(); +} + +void update_msg_stats(std::chrono::duration stat_elapsed, + msgStats &stats) +{ + if (stats.min_stat == std::chrono::duration::zero() || stat_elapsed < stats.min_stat) stats.min_stat = stat_elapsed; + if (stats.max_stat == std::chrono::duration::zero() || stat_elapsed > stats.max_stat) stats.max_stat = stat_elapsed; + stats.total_stat += stat_elapsed; +} + +void push_msg_stats(diagnostic_msgs::DiagnosticStatus &stat, + std::string stat_name, + msgStats msg_stats, + long num_loops) +{ + diagnostic_msgs::KeyValue min_stat_kv; + min_stat_kv.key = "min " + stat_name + " (s)"; + min_stat_kv.value = std::to_string(msg_stats.min_stat.count()); + stat.values.push_back(min_stat_kv); + diagnostic_msgs::KeyValue max_stat_kv; + max_stat_kv.key = "max " + stat_name + " (s)"; + max_stat_kv.value = std::to_string(msg_stats.max_stat.count()); + stat.values.push_back(max_stat_kv); + diagnostic_msgs::KeyValue avg_stat_kv; + avg_stat_kv.key = "avg " + stat_name + " (s)"; + avg_stat_kv.value = std::to_string(msg_stats.total_stat.count() / num_loops); + stat.values.push_back(avg_stat_kv); +} + int main(int argc, char** argv) { // Set up ROS. @@ -53,6 +97,8 @@ int main(int argc, char** argv) ros::NodeHandle nh; ros::NodeHandle nh_priv("~"); + ros::Publisher diagnostic_pub = nh.advertise("/diagnostics", 1); + // register signal SIGINT and signal handler signal(SIGINT, signalHandler); @@ -129,6 +175,35 @@ int main(int argc, char** argv) double expected_cycle_time = 1.0 / (static_cast(g_hw_interface->getControlFrequency())); + msgStats period_stats; + reset_msg_stats(period_stats); + msgStats read_stats; + reset_msg_stats(read_stats); + msgStats cm_update_stats; + reset_msg_stats(cm_update_stats); + msgStats write_stats; + reset_msg_stats(write_stats); + + // sub-stats of the read method + msgStats get_data_stats; + reset_msg_stats(get_data_stats); + msgStats read_data_stats; + reset_msg_stats(read_data_stats); + msgStats pub_io_stats; + reset_msg_stats(pub_io_stats); + msgStats pub_tool_stats; + reset_msg_stats(pub_tool_stats); + msgStats pub_pose_stats; + reset_msg_stats(pub_pose_stats); + msgStats pub_robot_stats; + reset_msg_stats(pub_robot_stats); + msgStats pub_temp_stats; + reset_msg_stats(pub_temp_stats); + + std::chrono::duration last_diagnostics_duration = std::chrono::duration::zero(); + + long debug_loops = 0; + // Debug timing printout every 5 seconds const std::chrono::seconds debug_timing_period{5}; std::chrono::steady_clock::time_point debug_timing_start = std::chrono::steady_clock::now(); @@ -141,26 +216,103 @@ int main(int argc, char** argv) // This is mostly used to track low-frequency information like joint temperature const bool trigger_low_frequency_logging = elapsed_since_debug > debug_timing_period; g_hw_interface->shouldLogTemperature(trigger_low_frequency_logging); - if (trigger_low_frequency_logging) debug_timing_start = debug_timing_now; // Receive current state from robot + const std::chrono::steady_clock::time_point read_start = std::chrono::steady_clock::now(); g_hw_interface->read(timestamp, period); + const std::chrono::duration read_elapsed = std::chrono::steady_clock::now() - read_start; + update_msg_stats(read_elapsed, read_stats); + ur_driver::HardwareInterface::readStats read_substats = g_hw_interface->get_read_stats(); + update_msg_stats(read_substats.get_data_elapsed, get_data_stats); + update_msg_stats(read_substats.read_data_elapsed, read_data_stats); + update_msg_stats(read_substats.pub_io_elapsed, pub_io_stats); + update_msg_stats(read_substats.pub_tool_elapsed, pub_tool_stats); + update_msg_stats(read_substats.pub_pose_elapsed, pub_pose_stats); + update_msg_stats(read_substats.pub_robot_elapsed, pub_robot_stats); + update_msg_stats(read_substats.pub_temp_elapsed, pub_temp_stats); + // Get current time and elapsed time since last read timestamp = ros::Time::now(); stopwatch_now = std::chrono::steady_clock::now(); - period.fromSec(std::chrono::duration_cast>(stopwatch_now - stopwatch_last).count()); + const std::chrono::duration period_chrono = stopwatch_now - stopwatch_last; + update_msg_stats(period_chrono, period_stats); + period.fromSec(period_chrono.count()); stopwatch_last = stopwatch_now; + const std::chrono::steady_clock::time_point cm_update_start = std::chrono::steady_clock::now(); cm.update(timestamp, period, g_hw_interface->shouldResetControllers()); + const std::chrono::duration cm_update_elapsed = std::chrono::steady_clock::now() - cm_update_start; + update_msg_stats(cm_update_elapsed, cm_update_stats); + const std::chrono::steady_clock::time_point write_start = std::chrono::steady_clock::now(); g_hw_interface->write(timestamp, period); + const std::chrono::duration write_elapsed = std::chrono::steady_clock::now() - write_start; + update_msg_stats(write_elapsed, write_stats); + + ++debug_loops; + + // Check if it's time to print + if (trigger_low_frequency_logging) { + const std::chrono::steady_clock::time_point diagnostic_start = std::chrono::steady_clock::now(); + diagnostic_msgs::DiagnosticArray dia_array; + diagnostic_msgs::DiagnosticStatus robot_status; + robot_status.name = "ur_hardware_interface: Overall health"; + robot_status.level = diagnostic_msgs::DiagnosticStatus::OK; + robot_status.message = "Looping"; + robot_status.hardware_id = "none"; + + diagnostic_msgs::KeyValue loop_durations_last; + loop_durations_last.key = "Loop durations last (s)"; + loop_durations_last.value = std::to_string(elapsed_since_debug.count()); + robot_status.values.push_back(loop_durations_last); + diagnostic_msgs::KeyValue num_loops; + num_loops.key = "Number of loops"; + num_loops.value = std::to_string(debug_loops); + robot_status.values.push_back(num_loops); + + push_msg_stats(robot_status, "period loop", period_stats, debug_loops); + push_msg_stats(robot_status, "read", read_stats, debug_loops); + push_msg_stats(robot_status, "cm_update", cm_update_stats, debug_loops); + push_msg_stats(robot_status, "write", write_stats, debug_loops); + push_msg_stats(robot_status, "read: get_data", get_data_stats, debug_loops); + push_msg_stats(robot_status, "read: read_data", read_data_stats, debug_loops); + push_msg_stats(robot_status, "read: pub_io", pub_io_stats, debug_loops); + push_msg_stats(robot_status, "read: pub_tool", pub_tool_stats, debug_loops); + push_msg_stats(robot_status, "read: pub_pose", pub_pose_stats, debug_loops); + push_msg_stats(robot_status, "read: pub_robot", pub_robot_stats, debug_loops); + push_msg_stats(robot_status, "read: pub_temp", pub_temp_stats, debug_loops); + + diagnostic_msgs::KeyValue diagnostic_duration_kv; + diagnostic_duration_kv.key = "Last diagnostic duration (s)"; + diagnostic_duration_kv.value = std::to_string(last_diagnostics_duration.count()); + robot_status.values.push_back(diagnostic_duration_kv); + + dia_array.status.push_back(robot_status); + diagnostic_pub.publish(dia_array); + + reset_msg_stats(period_stats); + reset_msg_stats(read_stats); + reset_msg_stats(cm_update_stats); + reset_msg_stats(write_stats); + reset_msg_stats(get_data_stats); + reset_msg_stats(read_data_stats); + reset_msg_stats(pub_io_stats); + reset_msg_stats(pub_tool_stats); + reset_msg_stats(pub_pose_stats); + reset_msg_stats(pub_robot_stats); + reset_msg_stats(pub_temp_stats); + debug_loops = 0; + last_diagnostics_duration = std::chrono::steady_clock::now() - diagnostic_start; + } + if (trigger_low_frequency_logging) debug_timing_start = debug_timing_now; + // if (!control_rate.sleep()) - if (period.toSec() > expected_cycle_time) - { + // if (period.toSec() > expected_cycle_time) + // { // ROS_WARN_STREAM("Could not keep cycle rate of " << expected_cycle_time * 1000 << "ms"); // ROS_WARN_STREAM("Actual cycle time:" << period.toNSec() / 1000000.0 << "ms"); - } + // } } spinner.stop();