Go to the documentation of this file.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
00026
00027
00028
00029 #include <hector_pose_estimation/measurement.h>
00030
00031 namespace hector_pose_estimation {
00032
00033 Measurement::Measurement(const std::string& name)
00034 : name_(name)
00035 , status_flags_(0)
00036 , enabled_(true)
00037 , min_interval_(0.0)
00038 , timeout_(1.0)
00039 , timer_(0.0)
00040 {
00041 parameters().add("enabled", enabled_);
00042 parameters().add("timeout", timeout_);
00043 parameters().add("min_interval", min_interval_);
00044 parameters().add("timeout", timeout_);
00045 }
00046
00047 Measurement::~Measurement()
00048 {
00049 }
00050
00051 bool Measurement::init(PoseEstimation& estimator, State& state)
00052 {
00053 if (getModel() && !getModel()->init(estimator, state)) return false;
00054 if (!onInit(estimator)) return false;
00055 return true;
00056 }
00057
00058 void Measurement::cleanup()
00059 {
00060 if (getModel()) getModel()->cleanup();
00061 onCleanup();
00062 }
00063
00064 void Measurement::reset(State& state)
00065 {
00066 queue().clear();
00067 timer_ = 0;
00068
00069 if (getModel()) getModel()->reset(state);
00070 onReset();
00071 }
00072
00073 bool Measurement::active(const State& state) {
00074 bool active = enabled() && (getModel() ? getModel()->active(state) : !(state.getSystemStatus() & STATUS_ALIGNMENT));
00075 if (!active) status_flags_ = 0;
00076 if (min_interval_ > 0.0 && timer_ < min_interval_) return false;
00077 return active;
00078 }
00079
00080 void Measurement::increase_timer(double dt) {
00081 timer_ += dt;
00082 }
00083
00084 bool Measurement::timedout() const {
00085 return timeout_ > 0.0 && timer_ > timeout_;
00086 }
00087
00088 void Measurement::add(const MeasurementUpdate& update) {
00089 queue().push(update);
00090 }
00091
00092 bool Measurement::process() {
00093 bool result = true;
00094
00095 while(!(queue().empty())) {
00096 result &= update(queue().pop());
00097 }
00098
00099
00100 if (timedout()) {
00101 if (status_flags_ > 0) ROS_WARN("Measurement %s timed out.", getName().c_str());
00102 status_flags_ = 0;
00103 }
00104 return result;
00105 }
00106
00107 bool Measurement::update(const MeasurementUpdate &update)
00108 {
00109 if (!filter() || !active(filter()->state())) return false;
00110
00111 if (!updateImpl(update)) return false;
00112
00113 timer_ = 0.0;
00114 if (getModel()) status_flags_ = getModel()->getStatusFlags();
00115 return true;
00116 }
00117
00118 }