[Personalrobots-commit] SF.net SVN: personalrobots:[10426] pkg/trunk

[<rob...@us...>]

Revision: 10426
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10426&view=rev
Author:   rob_wheeler
Date:     2009-02-02 18:39:21 +0000 (Mon, 02 Feb 2009)

Log Message:
-----------
Add per-controller timing statistics

Modified Paths:
--------------
    pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
    pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
    pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp

Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h	2009-02-02 18:39:21 UTC (rev 10426)
@@ -1,6 +1,6 @@
 
 ///////////////////////////////////////////////////////////////////////////////
-// Copyright (C) 2008, Eric Berger
+// Copyright (C) 2008-2009, Willow Garage, Inc.
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
@@ -41,7 +41,13 @@
 #include <tinyxml/tinyxml.h>
 #include <hardware_interface/hardware_interface.h>
 #include <mechanism_model/robot.h>
+#include <boost/circular_buffer.hpp>
 #include <boost/thread/mutex.hpp>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/max.hpp>
+#include <boost/accumulators/statistics/mean.hpp>
+#include <boost/accumulators/statistics/variance.hpp>
 #include <mechanism_model/controller.h>
 #include <realtime_tools/realtime_publisher.h>
 #include <misc_utils/advertised_service_guard.h>
@@ -51,9 +57,9 @@
 #include <robot_srvs/SpawnController.h>
 #include <robot_srvs/KillController.h>
 #include <robot_msgs/MechanismState.h>
+#include <robot_msgs/DiagnosticMessage.h>
 #include "tf/tfMessage.h"
 
-
 typedef controller::Controller* (*ControllerAllocator)();
 
 class MechanismControl {
@@ -84,6 +90,20 @@
   controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
   std::string controller_names_[MAX_NUM_CONTROLLERS];
 
+  typedef boost::accumulators::accumulator_set<double, boost::accumulators::stats<boost::accumulators::tag::max, boost::accumulators::tag::mean, boost::accumulators::tag::variance> > TimeStatistics;
+  struct {
+    TimeStatistics acc_;
+    double max_;
+    boost::circular_buffer<double> max1_;
+    struct {
+      TimeStatistics acc_;
+      double max_;
+      boost::circular_buffer<double> max1_;
+    } controllers_[MAX_NUM_CONTROLLERS];
+  } diagnostics_;
+  realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
+  void publishDiagnostics();
+
   // Killing a controller:
   // 1. Non-realtime thread places the index of the controller into please_remove_
   // 2. Realtime thread moves the controller out of the array and into removed_

Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp	2009-02-02 18:39:21 UTC (rev 10426)
@@ -33,12 +33,17 @@
 #include "ros/console.h"
 
 using namespace mechanism;
+using namespace boost::accumulators;
 
 MechanismControl::MechanismControl(HardwareInterface *hw) :
-  state_(NULL), hw_(hw), initialized_(0), please_remove_(-1), removed_(NULL)
+  state_(NULL), hw_(hw), initialized_(0), publisher_("/diagnostics", 1), please_remove_(-1), removed_(NULL)
 {
   memset(controllers_, 0, MAX_NUM_CONTROLLERS * sizeof(void*));
   model_.hw_ = hw;
+
+  diagnostics_.max1_.resize(60);
+  for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
+    diagnostics_.controllers_[i].max1_.resize(60);
 }
 
 MechanismControl::~MechanismControl()
@@ -72,23 +77,95 @@
   return NULL;
 }
 
+#define ADD_STRING_FMT(lab, fmt, ...) \
+  s.label = (lab); \
+  { char buf[1024]; \
+    snprintf(buf, sizeof(buf), fmt, ##__VA_ARGS__); \
+    s.value = buf; \
+  } \
+  strings.push_back(s)
 
+void MechanismControl::publishDiagnostics()
+{
+  if (publisher_.trylock())
+  {
+    int active = 0;
+    TimeStatistics a;
+
+    vector<robot_msgs::DiagnosticStatus> statuses;
+    vector<robot_msgs::DiagnosticValue> values;
+    vector<robot_msgs::DiagnosticString> strings;
+    robot_msgs::DiagnosticStatus status;
+    robot_msgs::DiagnosticValue v;
+    robot_msgs::DiagnosticString s;
+
+    status.name = "Mechanism Control";
+    status.level = 0;
+    status.message = "OK";
+
+    for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
+    {
+      if (controllers_[i] != NULL)
+      {
+        ++active;
+        double m = extract_result<tag::max>(diagnostics_.controllers_[i].acc_);
+        diagnostics_.controllers_[i].max1_.push_back(m);
+        diagnostics_.controllers_[i].max_ = std::max(m, diagnostics_.controllers_[i].max_);
+        ADD_STRING_FMT(controller_names_[i], "%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max)", mean(diagnostics_.controllers_[i].acc_)*1e6, sqrt(variance(diagnostics_.controllers_[i].acc_))*1e6, m*1e6, *std::max_element(diagnostics_.controllers_[i].max1_.begin(), diagnostics_.controllers_[i].max1_.end())*1e6, diagnostics_.controllers_[i].max_*1e6);
+        /* Clear accumulator */
+        diagnostics_.controllers_[i].acc_ = a;
+      }
+    }
+
+    double m = extract_result<tag::max>(diagnostics_.acc_);
+    diagnostics_.max1_.push_back(m);
+    diagnostics_.max_ = std::max(m, diagnostics_.max_);
+    ADD_STRING_FMT("Overall", "%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max)", mean(diagnostics_.acc_)*1e6, sqrt(variance(diagnostics_.acc_))*1e6, m*1e6, *std::max_element(diagnostics_.max1_.begin(), diagnostics_.max1_.end())*1e6, diagnostics_.max_*1e6);
+    ADD_STRING_FMT("Active controllers", "%d", active);
+
+    status.set_values_vec(values);
+    status.set_strings_vec(strings);
+    statuses.push_back(status);
+    publisher_.msg_.set_status_vec(statuses);
+    publisher_.unlockAndPublish();
+
+    /* Clear accumulator */
+    diagnostics_.acc_ = a;
+  }
+}
+
 // Must be realtime safe.
 void MechanismControl::update()
 {
+  static int count = 0;
+
   state_->propagateState();
   state_->zeroCommands();
 
   // Update all controllers
+  double start_update = realtime_gettime();
   for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
   {
     if (controllers_[i] != NULL)
+    {
+      double start = realtime_gettime();
       controllers_[i]->update();
+      double end = realtime_gettime();
+      diagnostics_.controllers_[i].acc_(end - start);
+    }
   }
+  double end_update = realtime_gettime();
+  diagnostics_.acc_(end_update - start_update);
 
   state_->enforceSafety();
   state_->propagateEffort();
 
+  if (++count == 1000)
+  {
+    count = 0;
+    publishDiagnostics();
+  }
+
   // If there's a controller to remove, we take it out of the controllers array.
   if (please_remove_ >= 0)
   {

Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2009-02-02 17:40:18 UTC (rev 10425)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp	2009-02-02 18:39:21 UTC (rev 10426)
@@ -479,7 +479,7 @@
   if (!g_options.xml_)
     Usage("You must specify a robot description XML file");
 
-  ros::Node *node = new ros::Node("mechanism_control",
+  ros::Node *node = new ros::Node("pr2_etherCAT",
                                   ros::Node::DONT_HANDLE_SIGINT);
 
   // Catch attempts to quit


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.