refactor. add substats for read

Author: turniphead

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -157,6 +157,18 @@ class HardwareInterface : public hardware_interface::RobotHW
    */
   bool shouldResetControllers();
 
+  struct readStats {
+    std::chrono::duration<double> get_data_elapsed;
+    std::chrono::duration<double> read_data_elapsed;
+    std::chrono::duration<double> pub_io_elapsed;
+    std::chrono::duration<double> pub_tool_elapsed;
+    std::chrono::duration<double> pub_pose_elapsed;
+    std::chrono::duration<double> pub_robot_elapsed;
+    std::chrono::duration<double> pub_temp_elapsed;
+  };
+
+  readStats get_read_stats();
+
 protected:
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame.
@@ -292,6 +304,8 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   std::string robot_ip_;
   std::string tf_prefix_;
+
+  readStats read_stats_;
 };
 
 }  // namespace ur_driver

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -438,10 +438,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<double>::zero();
+  read_stats_.read_data_elapsed = std::chrono::duration<double>::zero();
+  read_stats_.pub_io_elapsed = std::chrono::duration<double>::zero();
+  read_stats_.pub_tool_elapsed = std::chrono::duration<double>::zero();
+  read_stats_.pub_pose_elapsed = std::chrono::duration<double>::zero();
+  read_stats_.pub_robot_elapsed = std::chrono::duration<double>::zero();
+  read_stats_.pub_temp_elapsed = std::chrono::duration<double>::zero();
+
+  const std::chrono::steady_clock::time_point get_data_start = std::chrono::steady_clock::now();
   std::unique_ptr<rtde_interface::DataPackage> 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_);
@@ -472,16 +483,27 @@ 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;
+    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
     if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))
@@ -687,6 +709,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));

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -47,24 +47,43 @@ void signalHandler(int signum)
   exit(signum);
 }
 
-void update_msg_stats(diagnostic_msgs::DiagnosticStatus &stat, 
-                      std::string stat_name, 
-                      std::chrono::duration<double> min_stat, 
-                      std::chrono::duration<double> max_stat, 
-                      std::chrono::duration<double> total_stat,
-                      long num_loops)
+struct msgStats {
+  std::chrono::duration<double> min_stat;
+  std::chrono::duration<double> max_stat;
+  std::chrono::duration<double> total_stat;
+};
+
+void reset_msg_stats(msgStats &stats)
+{
+  stats.min_stat = std::chrono::duration<double>::zero();
+  stats.max_stat = std::chrono::duration<double>::zero();
+  stats.total_stat = std::chrono::duration<double>::zero();
+}
+
+void update_msg_stats(std::chrono::duration<double> stat_elapsed,
+                      msgStats &stats)
+{
+  if (stats.min_stat == std::chrono::duration<double>::zero() || stat_elapsed < stats.min_stat) stats.min_stat = stat_elapsed;
+  if (stats.max_stat == std::chrono::duration<double>::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(min_stat.count());
+  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(max_stat.count());
+  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(total_stat.count() / num_loops);
+  avg_stat_kv.value = std::to_string(msg_stats.total_stat.count() / num_loops);
   stat.values.push_back(avg_stat_kv);
 }
 
@@ -160,15 +179,29 @@ int main(int argc, char** argv)
   ros::Duration max_period = ros::Duration(0);
   ros::Duration period_totals = ros::Duration(0);
 
-  std::chrono::duration<double> min_read = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> max_read = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> read_totals = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> min_cm_update = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> max_cm_update = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> cm_update_totals = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> min_write = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> max_write = std::chrono::duration<double>::zero();
-  std::chrono::duration<double> write_totals = std::chrono::duration<double>::zero();
+  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);
+
   long debug_loops = 0;
 
   // Debug timing printout every 5 seconds
@@ -182,9 +215,16 @@ int main(int argc, char** argv)
     const std::chrono::steady_clock::time_point read_start = std::chrono::steady_clock::now();
     g_hw_interface->read(timestamp, period);
     const std::chrono::duration<double> read_elapsed = std::chrono::steady_clock::now() - read_start;
-    if (min_read == std::chrono::duration<double>::zero() || read_elapsed < min_read) min_read = read_elapsed;
-    if (max_read == std::chrono::duration<double>::zero() || read_elapsed > max_read) max_read = read_elapsed;
-    read_totals += read_elapsed;
+    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();
@@ -195,19 +235,12 @@ int main(int argc, char** argv)
     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<double> cm_update_elapsed = std::chrono::steady_clock::now() - cm_update_start;
-    if (min_cm_update == std::chrono::duration<double>::zero() || cm_update_elapsed < min_cm_update) min_cm_update = cm_update_elapsed;
-    if (max_cm_update == std::chrono::duration<double>::zero() || cm_update_elapsed > max_cm_update) max_cm_update = cm_update_elapsed;
-    cm_update_totals += cm_update_elapsed;
+    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<double> write_elapsed = std::chrono::steady_clock::now() - write_start;
-    if (min_write == std::chrono::duration<double>::zero() || write_elapsed < min_write) min_write = write_elapsed;
-    if (max_write == std::chrono::duration<double>::zero() || write_elapsed > max_write) max_write = write_elapsed;
-    write_totals += write_elapsed;
-
-
-
+    update_msg_stats(write_elapsed, write_stats);
 
     if (min_period.isZero() || period < min_period) min_period = period;
     if (max_period.isZero() || period > max_period) max_period = period;
@@ -246,74 +279,33 @@ int main(int argc, char** argv)
       avg_period_kv.value = std::to_string(period_totals.toSec() / debug_loops);
       robot_status.values.push_back(avg_period_kv);
 
-
-
-      update_msg_stats(robot_status, "read", min_read, max_read, read_totals, debug_loops);
-      update_msg_stats(robot_status, "cm_update", min_cm_update, max_cm_update, cm_update_totals, debug_loops);
-      update_msg_stats(robot_status, "write", min_write, max_write, write_totals, debug_loops);
-
-      // diagnostic_msgs::KeyValue min_read_kv;
-      // min_read_kv.key = "min read (s)";
-      // min_read_kv.value = std::to_string(min_read.count());
-      // robot_status.values.push_back(min_read_kv);
-      // diagnostic_msgs::KeyValue max_read_kv;
-      // max_read_kv.key = "max read (s)";
-      // max_read_kv.value = std::to_string(max_read.count());
-      // robot_status.values.push_back(max_read_kv);
-      // diagnostic_msgs::KeyValue avg_read_kv;
-      // avg_read_kv.key = "avg read (s)";
-      // avg_read_kv.value = std::to_string(read_totals.count() / debug_loops);
-      // robot_status.values.push_back(avg_read_kv);
-
-      // diagnostic_msgs::KeyValue min_cm_update_kv;
-      // min_cm_update_kv.key = "min cm_update (s)";
-      // min_cm_update_kv.value = std::to_string(min_cm_update.count());
-      // robot_status.values.push_back(min_cm_update_kv);
-      // diagnostic_msgs::KeyValue max_cm_update_kv;
-      // max_cm_update_kv.key = "max cm_update (s)";
-      // max_cm_update_kv.value = std::to_string(max_cm_update.count());
-      // robot_status.values.push_back(max_cm_update_kv);
-      // diagnostic_msgs::KeyValue avg_cm_update_kv;
-      // avg_cm_update_kv.key = "avg cm_update (s)";
-      // avg_cm_update_kv.value = std::to_string(cm_update_totals.count() / debug_loops);
-      // robot_status.values.push_back(avg_cm_update_kv);
-
-      // diagnostic_msgs::KeyValue min_write_kv;
-      // min_write_kv.key = "min write (s)";
-      // min_write_kv.value = std::to_string(min_write.count());
-      // robot_status.values.push_back(min_write_kv);
-      // diagnostic_msgs::KeyValue max_write_kv;
-      // max_write_kv.key = "max write (s)";
-      // max_write_kv.value = std::to_string(max_write.count());
-      // robot_status.values.push_back(max_write_kv);
-      // diagnostic_msgs::KeyValue avg_write_kv;
-      // avg_write_kv.key = "avg write (s)";
-      // avg_write_kv.value = std::to_string(write_totals.count() / debug_loops);
-      // robot_status.values.push_back(avg_write_kv);
+      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);
 
       dia_array.status.push_back(robot_status);
       diagnostic_pub.publish(dia_array);
 
-
-      // ROS_INFO_STREAM(
-      //     "Loop durations last " << total_elapsed.count() << "s: "
-      //     "min=" << min_period.toSec() * 1000.0 << "ms, "
-      //     "max=" << max_period.toSec() * 1000.0 << "ms, "
-      //     "avg=" << period_totals.toSec() * 1000.0 / debug_loops << "ms, "
-      //     "avgchrono=" << total_elapsed.count() * 1000.0 / debug_loops << "ms"
-      // );
       min_period = ros::Duration(0);
       max_period = ros::Duration(0);
       period_totals = ros::Duration(0);
-      min_read = std::chrono::duration<double>::zero();
-      max_read = std::chrono::duration<double>::zero();
-      read_totals = std::chrono::duration<double>::zero();
-      min_cm_update = std::chrono::duration<double>::zero();
-      max_cm_update = std::chrono::duration<double>::zero();
-      cm_update_totals = std::chrono::duration<double>::zero();
-      min_write = std::chrono::duration<double>::zero();
-      max_write = std::chrono::duration<double>::zero();
-      write_totals = std::chrono::duration<double>::zero();
+      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;
       debug_timing_start = debug_timing_now;
     }