pr2 ethercat should report last time halt occurred in diagnostics (ros ticket #4332)

[ahendrix]

I will add a diagnostic key-value pair in the Ethercat Master diagnostics that contains that time that a motor halt last occurred.

trac data:

• Owner: dking
• Reporter: blaise
• Reported at: Fri May 14 16:31:41 2010
• URL: https://code.ros.org/trac/wg-ros-pkg/ticket/4332

[ahendrix]

[blaise] Actually, this might be a rob ticket...

[ahendrix]

[blaise] Just curious why the won't fix? It is still something that strikes me as useful, and that we'll wish we had next time we run into one of these scheduling/motors halting problems.

[ahendrix]

[dking] Patch for pr2_ethercat_drivers 1.5

{{{

--- include/ethercat_hardware/ethercat_hardware.h (revision 48057)
+++ include/ethercat_hardware/ethercat_hardware.h (working copy)
@@ -84,6 +84,7 @@
struct netif_counters counters_;
bool input_thread_is_stopped_;
bool motors_halted_; //!< True if motors are halted

• ros::Time last_time_motors_halted_; //!< Last time motors halted

static const bool collect_extra_timing_ = true;
};

Index: src/ethercat_hardware.cpp

--- src/ethercat_hardware.cpp (revision 48057)
+++ src/ethercat_hardware.cpp (working copy)
@@ -53,6 +53,7 @@
halt_motors_error_count_(0)
{
resetMaxTiming();

• last_time_motors_halted_ = ros::Time::now(); // Assume motors start in halted state
  }

void EthercatHardwareDiagnostics::resetMaxTiming()
@@ -374,7 +375,7 @@
boost::unique_lockboost::mutex lock(diagnostics_mutex_, try_lock);
if (lock.owns_lock())
{

• // Make copies of diagnostic data for dianostic thread
• // Make copies of diagnostic data for diagnostic thread
  memcpy(diagnostics_buffer_, buffer, buffer_size_);
  diagnostics_ = diagnostics;
  // Trigger diagnostics publish thread
  @@ -446,6 +447,8 @@
  }

status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");

• status_.addf("Last time motors halted", "%f", diagnostics_.last_time_motors_halted_.toSec());
• status_.addf("EtherCAT devices (expected)", "%d", num_slaves_);
  status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
  ethernet_interface_info_.publishDiagnostics(status_);
  @@ -641,12 +644,16 @@
  motor_publisher_.unlockAndPublish();
  }
• if ((halt_motors_ && !old_halt_motors) && (halt_motors_error))
• if (halt_motors_ && !old_halt_motors)
  {
• ++diagnostics_.halt_motors_error_count_;
• if ((update_start_time - last_reset_) < ros::Duration(0.5))
• diagnostics_.last_time_motors_halted_ = update_start_time;
• if (halt_motors_error)
  {

•  diagnostics_.halt_after_reset_ = true;
  

•  ++diagnostics_.halt_motors_error_count_;
  

•  if ((update_start_time - last_reset_) < ros::Duration(0.5))
  

•  {
  

•    diagnostics_.halt_after_reset_ = true;
  

•  }
  

  }
  }

dking@bli:/wg/stor1a/dking/dros/pr2_ethercat_drivers/ethercat_hardware$ svn di

Index: include/ethercat_hardware/ethercat_hardware.h

--- include/ethercat_hardware/ethercat_hardware.h (revision 48057)
+++ include/ethercat_hardware/ethercat_hardware.h (working copy)
@@ -84,6 +84,7 @@
struct netif_counters counters_;
bool input_thread_is_stopped_;
bool motors_halted_; //!< True if motors are halted

• ros::Time last_time_motors_halted_; //!< Last time motors halted

static const bool collect_extra_timing_ = true;
};

Index: src/ethercat_hardware.cpp

--- src/ethercat_hardware.cpp (revision 48057)
+++ src/ethercat_hardware.cpp (working copy)
@@ -53,6 +53,7 @@
halt_motors_error_count_(0)
{
resetMaxTiming();

• last_time_motors_halted_ = ros::Time::now(); // Assume motors start in halted state
  }

void EthercatHardwareDiagnostics::resetMaxTiming()
@@ -374,7 +375,7 @@
boost::unique_lockboost::mutex lock(diagnostics_mutex_, try_lock);
if (lock.owns_lock())
{

• // Make copies of diagnostic data for dianostic thread
• // Make copies of diagnostic data for diagnostic thread
  memcpy(diagnostics_buffer_, buffer, buffer_size_);
  diagnostics_ = diagnostics;
  // Trigger diagnostics publish thread
  @@ -446,6 +447,8 @@
  }

status_.add("Motors halted", diagnostics_.motors_halted_ ? "true" : "false");

• status_.addf("Last time motors halted", "%f", diagnostics_.last_time_motors_halted_.toSec());
• status_.addf("EtherCAT devices (expected)", "%d", num_slaves_);
  status_.addf("EtherCAT devices (current)", "%d", diagnostics_.device_count_);
  ethernet_interface_info_.publishDiagnostics(status_);
  @@ -641,12 +644,16 @@
  motor_publisher_.unlockAndPublish();
  }
• if ((halt_motors_ && !old_halt_motors) && (halt_motors_error))
• if (halt_motors_ && !old_halt_motors)
  {
• ++diagnostics_.halt_motors_error_count_;
• if ((update_start_time - last_reset_) < ros::Duration(0.5))
• diagnostics_.last_time_motors_halted_ = update_start_time;
• if (halt_motors_error)
  {

•  diagnostics_.halt_after_reset_ = true;
  

•  ++diagnostics_.halt_motors_error_count_;
  

•  if ((update_start_time - last_reset_) < ros::Duration(0.5))
  

•  {
  

•    diagnostics_.halt_after_reset_ = true;
  

•  }
  

  }
  }
  }}}