00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00027
00028
00029
00030
00031
00032 #ifndef MECHANISM_CONTROL_CONTROL_SPEC_H
00033 #define MECHANISM_CONTROL_CONTROL_SPEC_H
00034
00035 #pragma GCC diagnostic ignored "-Wextra"
00036
00037 #include <map>
00038 #include <string>
00039 #include <vector>
00040 #include <pr2_controller_interface/controller.h>
00041 #include <boost/circular_buffer.hpp>
00042 #include <boost/thread/mutex.hpp>
00043 #include <boost/accumulators/accumulators.hpp>
00044 #include <boost/accumulators/statistics/stats.hpp>
00045 #include <boost/accumulators/statistics/max.hpp>
00046 #include <boost/accumulators/statistics/mean.hpp>
00047 #include <boost/accumulators/statistics/variance.hpp>
00048
00049 typedef boost::accumulators::accumulator_set<
00050 double, boost::accumulators::stats<boost::accumulators::tag::max,
00051 boost::accumulators::tag::mean,
00052 boost::accumulators::tag::variance> > TimeStatistics;
00053
00054 struct Statistics {
00055 TimeStatistics acc;
00056 ros::Time time_last_control_loop_overrun;
00057 unsigned int num_control_loop_overruns;
00058 double max;
00059 boost::circular_buffer<double> max1;
00060 Statistics() : num_control_loop_overruns(0), max(0), max1(60) {}
00061 };
00062
00063 struct ControllerSpec {
00064 std::string name;
00065 boost::shared_ptr<pr2_controller_interface::Controller> c;
00066 boost::shared_ptr<Statistics> stats;
00067
00068 ControllerSpec() : stats(new Statistics) {}
00069 ControllerSpec(const ControllerSpec &spec)
00070 : name(spec.name), c(spec.c), stats(spec.stats) {}
00071 };
00072
00073 #endif
00074