Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[15595] pkg/trunk
|
From: <rob...@us...> - 2009-05-18 16:44:33
|
Revision: 15595
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15595&view=rev
Author: rob_wheeler
Date: 2009-05-18 16:42:53 +0000 (Mon, 18 May 2009)
Log Message:
-----------
- use boost accumulators to gather max and mean stats.
- format diagnostic messages
- report pre-update, update, and post-update times in mechanism_control diags
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt 2009-05-18 16:32:29 UTC (rev 15594)
+++ pkg/trunk/drivers/motor/ethercat_hardware/CMakeLists.txt 2009-05-18 16:42:53 UTC (rev 15595)
@@ -8,5 +8,8 @@
add_definitions(-Wall)
rospack_add_library(ethercat_hardware src/ethercat_hardware.cpp src/wg0x.cpp src/ek1122.cpp src/wg014.cpp)
+rospack_remove_compile_flags(ethercat_hardware -W)
+
rospack_add_executable(motorconf src/motorconf.cpp src/ethercat_hardware.cpp src/wg0x.cpp src/ek1122.cpp src/wg014.cpp)
target_link_libraries(motorconf loki)
+rospack_remove_compile_flags(motorconf -W)
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2009-05-18 16:32:29 UTC (rev 15594)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2009-05-18 16:42:53 UTC (rev 15595)
@@ -47,6 +47,12 @@
#include <realtime_tools/realtime_publisher.h>
+#include <boost/accumulators/accumulators.hpp>
+#include <boost/accumulators/statistics/stats.hpp>
+#include <boost/accumulators/statistics/max.hpp>
+#include <boost/accumulators/statistics/mean.hpp>
+using namespace boost::accumulators;
+
class EthercatHardware
{
public:
@@ -103,12 +109,11 @@
realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
struct {
- struct {
- double roundtrip_;
- } iteration_[1000];
+ accumulator_set<double, stats<tag::max, tag::mean> > acc_;
double max_roundtrip_;
int txandrx_errors_;
} diagnostics_;
+ double last_published_;
vector<robot_msgs::DiagnosticStatus> statuses_;
vector<robot_msgs::DiagnosticValue> values_;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-05-18 16:32:29 UTC (rev 15594)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2009-05-18 16:42:53 UTC (rev 15595)
@@ -40,10 +40,6 @@
EthercatHardware::EthercatHardware() :
hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), halt_motors_(true), reset_state_(0), publisher_("/diagnostics", 1)
{
- for (uint32_t i = 0; i < sizeof(diagnostics_.iteration_)/sizeof(diagnostics_.iteration_[0]); ++i)
- {
- diagnostics_.iteration_[i].roundtrip_ = 0;
- }
diagnostics_.max_roundtrip_ = 0;
diagnostics_.txandrx_errors_ = 0;
}
@@ -168,6 +164,7 @@
// Create HardwareInterface
hw_ = new HardwareInterface(num_actuators);
hw_->current_time_ = realtime_gettime();
+ last_published_ = hw_->current_time_;
// Initialize slaves
set<string> actuator_names;
@@ -238,6 +235,18 @@
publishDiagnostics();
}
+#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)
+#define ADD_STRING(lab, val) \
+ s.label = (lab); \
+ s.value = (val); \
+ strings_.push_back(s)
+
void EthercatHardware::publishDiagnostics()
{
// Publish status of EtherCAT master
@@ -259,44 +268,21 @@
status.message = "OK";
}
- // Motors halted?
- s.value = halt_motors_ ? "true" : "false";
- s.label = "Motors halted";
- strings_.push_back(s);
+ ADD_STRING("Motors halted", halt_motors_ ? "true" : "false");
+ ADD_STRING_FMT("EtherCAT devices", "%d", num_slaves_);
+ ADD_STRING("Interface", interface_);
+ ADD_STRING_FMT("Reset state", "%d", reset_state_);
- // Num devices
- v.value = num_slaves_;
- v.label = "EtherCAT devices";
- values_.push_back(v);
-
- // Interface
- s.value = interface_;
- s.label = "Interface";
- strings_.push_back(s);
-
- // Interface
- v.value = reset_state_;
- v.label = "Reset state";
- values_.push_back(v);
-
// Roundtrip
- double total = 0;
- for (int i = 0; i < 1000; ++i)
- {
- total += diagnostics_.iteration_[i].roundtrip_;
- diagnostics_.max_roundtrip_ = max(diagnostics_.max_roundtrip_, diagnostics_.iteration_[i].roundtrip_);
- }
- v.value = total / 1000.0 * 1e6;
- v.label = "Average roundtrip time";
- values_.push_back(v);
+ diagnostics_.max_roundtrip_ = std::max(diagnostics_.max_roundtrip_,
+ extract_result<tag::max>(diagnostics_.acc_));
+ ADD_STRING_FMT("Average roundtrip time (us)", "%.4f", extract_result<tag::mean>(diagnostics_.acc_) * 1e6);
- v.value = diagnostics_.max_roundtrip_ * 1e6;
- v.label = "Maximum roundtrip time";
- values_.push_back(v);
+ accumulator_set<double, stats<tag::max, tag::mean> > blank;
+ diagnostics_.acc_ = blank;
- v.value = diagnostics_.txandrx_errors_;
- v.label = "EtherCAT txandrx errors";
- values_.push_back(v);
+ ADD_STRING_FMT("Maximum roundtrip time (us)", "%.4f", diagnostics_.max_roundtrip_ * 1e6);
+ ADD_STRING_FMT("EtherCAT txandrx errors", "%d", diagnostics_.txandrx_errors_);
status.set_values_vec(values_);
status.set_strings_vec(strings_);
@@ -322,8 +308,6 @@
{
unsigned char *current, *last;
- static int count = 0;
-
// Convert HW Interface commands to MCB-specific buffers
current = current_buffer_;
@@ -367,7 +351,7 @@
if (!em_->txandrx_PD(buffer_size_, current_buffer_)) {
++diagnostics_.txandrx_errors_;
}
- diagnostics_.iteration_[count].roundtrip_ = realtime_gettime() - start;
+ diagnostics_.acc_(realtime_gettime() - start);
// Convert status back to HW Interface
current = current_buffer_;
@@ -397,10 +381,10 @@
current_buffer_ = last_buffer_;
last_buffer_ = tmp;
- if (++count == 1000)
+ if ((hw_->current_time_ - last_published_) > 1.0)
{
+ last_published_ = hw_->current_time_;
publishDiagnostics();
- count = 0;
}
}
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-05-18 16:32:29 UTC (rev 15594)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2009-05-18 16:42:53 UTC (rev 15595)
@@ -130,13 +130,11 @@
double, boost::accumulators::stats<boost::accumulators::tag::max,
boost::accumulators::tag::mean,
boost::accumulators::tag::variance> > TimeStatistics;
- struct Diagnostics {
+ struct Statistics {
TimeStatistics acc;
double max;
boost::circular_buffer<double> max1;
-
-
- Diagnostics() : max(0), max1(60) {}
+ Statistics() : max(0), max1(60) {}
};
enum {EMPTY, INITIALIZED = 1, RUNNING};
@@ -144,24 +142,26 @@
std::string name;
boost::shared_ptr<controller::Controller> c;
int state;
- boost::shared_ptr<Diagnostics> diagnostics;
+ boost::shared_ptr<Statistics> stats;
- ControllerSpec() : state(EMPTY), diagnostics(new Diagnostics) {}
+ ControllerSpec() : state(EMPTY), stats(new Statistics) {}
ControllerSpec(const ControllerSpec &spec)
- : name(spec.name), c(spec.c), state(spec.state), diagnostics(spec.diagnostics) {}
+ : name(spec.name), c(spec.c), state(spec.state), stats(spec.stats) {}
};
boost::mutex controllers_lock_;
std::vector<ControllerSpec> controllers_lists_[2];
int current_controllers_list_, used_by_realtime_;
- Diagnostics diagnostics_;
+ Statistics pre_update_stats_;
+ Statistics update_stats_;
+ Statistics post_update_stats_;
realtime_tools::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
void publishDiagnostics();
std::vector<controller::Controller*> start_request_, stop_request_;
bool please_switch_;
- int loop_count_;
+ double last_published_;
};
/*
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-05-18 16:32:29 UTC (rev 15594)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2009-05-18 16:42:53 UTC (rev 15595)
@@ -47,7 +47,7 @@
start_request_(0),
stop_request_(0),
please_switch_(false),
- loop_count_(0)
+ last_published_(realtime_gettime())
{
model_.hw_ = hw;
mechanism_control_ = this;
@@ -108,9 +108,9 @@
if (controllers[i].state != EMPTY)
{
++active;
- double m = extract_result<tag::max>(controllers[i].diagnostics->acc);
- controllers[i].diagnostics->max1.push_back(m);
- controllers[i].diagnostics->max = std::max(m, controllers[i].diagnostics->max);
+ double m = extract_result<tag::max>(controllers[i].stats->acc);
+ controllers[i].stats->max1.push_back(m);
+ controllers[i].stats->max = std::max(m, controllers[i].stats->max);
std::string state;
if (controllers[i].c->state_ == RUNNING)
state = "Running";
@@ -120,26 +120,35 @@
state = "Unknown";
ADD_STRING_FMT(controllers[i].name,
"%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max) %s (state)",
- mean(controllers[i].diagnostics->acc)*1e6,
- sqrt(variance(controllers[i].diagnostics->acc))*1e6,
+ mean(controllers[i].stats->acc)*1e6,
+ sqrt(variance(controllers[i].stats->acc))*1e6,
m*1e6,
- *std::max_element(controllers[i].diagnostics->max1.begin(), controllers[i].diagnostics->max1.end())*1e6,
- controllers[i].diagnostics->max*1e6,
+ *std::max_element(controllers[i].stats->max1.begin(), controllers[i].stats->max1.end())*1e6,
+ controllers[i].stats->max*1e6,
state.c_str());
- controllers[i].diagnostics->acc = blank_statistics; // clear
+ controllers[i].stats->acc = blank_statistics; // clear
}
}
- 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);
+#define REPORT_STATS(stats_, label) \
+ { \
+ double m = extract_result<tag::max>(stats_.acc); \
+ stats_.max1.push_back(m); \
+ stats_.max = std::max(m, stats_.max); \
+ ADD_STRING_FMT(label, "%.4f (avg) %.4f (stdev) %.4f (max) %.4f (1-min max) %.4f (life max)", \
+ mean(stats_.acc)*1e6, \
+ sqrt(variance(stats_.acc))*1e6, \
+ m*1e6, \
+ *std::max_element(stats_.max1.begin(), stats_.max1.end())*1e6, \
+ stats_.max*1e6); \
+ stats_.acc = blank_statistics; \
+ }
+
+ REPORT_STATS(pre_update_stats_, "Before Update");
+ REPORT_STATS(update_stats_, "Update");
+ REPORT_STATS(post_update_stats_, "After Update");
+
ADD_STRING_FMT("Active controllers", "%d", active);
status.set_values_vec(values);
@@ -148,7 +157,6 @@
publisher_.msg_.set_status_vec(statuses);
publisher_.unlockAndPublish();
- diagnostics_.acc = blank_statistics; // clear
}
}
@@ -158,12 +166,14 @@
used_by_realtime_ = current_controllers_list_;
std::vector<ControllerSpec> &controllers = controllers_lists_[used_by_realtime_];
+ double start = realtime_gettime();
state_->propagateState();
state_->zeroCommands();
+ double start_update = realtime_gettime();
+ pre_update_stats_.acc(start_update - start);
// Update all controllers
// Start with controller that was last added
- double start_update = realtime_gettime();
for (int i=controllers.size()-1; i>=0; i--)
{
if (controllers[i].state != EMPTY)
@@ -176,19 +186,21 @@
double start = realtime_gettime();
controllers[i].c->updateRequest();
double end = realtime_gettime();
- controllers[i].diagnostics->acc(end - start);
+ controllers[i].stats->acc(end - start);
}
}
double end_update = realtime_gettime();
- diagnostics_.acc(end_update - start_update);
+ update_stats_.acc(end_update - start_update);
state_->enforceSafety();
state_->propagateEffort();
+ double end = realtime_gettime();
+ post_update_stats_.acc(end - end_update);
- if (++loop_count_ >= 1000)
+ if ((end - last_published_) > 1.0)
{
- loop_count_ = 0;
publishDiagnostics();
+ last_published_ = end;
}
// there are controllers to start/stop
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
