Driver: reduce joint temperature spam (#3)

Author: clementchef

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -157,6 +157,14 @@ class HardwareInterface : public hardware_interface::RobotHW
    */
   bool shouldResetControllers();
 
+  /*!
+   * \brief Mark next tick as logging tick
+   *
+   * \returns void
+   */
+  void shouldLogTemperature(bool value);
+
+
 protected:
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame.
@@ -292,6 +300,8 @@ class HardwareInterface : public hardware_interface::RobotHW
 
   std::string robot_ip_;
   std::string tf_prefix_;
+
+  bool enable_temperature_log_;
 };
 
 }  // namespace ur_driver

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -428,6 +428,10 @@ void HardwareInterface::readBitsetData(const std::unique_ptr<rtde_interface::Dat
   }
 }
 
+void HardwareInterface::shouldLogTemperature(bool value) {
+  this->enable_temperature_log_ = value;
+}
+
 void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
 {
   // set defaults
@@ -481,7 +485,9 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
     transformForceTorque();
     publishPose();
     publishRobotAndSafetyMode();
-    publishJointTemperatures(time);
+    if (this->enable_temperature_log_) {
+      publishJointTemperatures(time);
+    }
 
     // pausing state follows runtime state when pausing
     if (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSED))

ur_robot_driver/src/ros/hardware_interface_node.cpp

@@ -129,9 +129,20 @@ int main(int argc, char** argv)
 
   double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
 
+  // 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();
+
   // Run as fast as possible
   while (ros::ok())
   {
+    const std::chrono::steady_clock::time_point debug_timing_now = std::chrono::steady_clock::now();
+    const std::chrono::duration<double> elapsed_since_debug = debug_timing_now - debug_timing_start;
+    // 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
     g_hw_interface->read(timestamp, period);