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