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 #ifndef HECTOR_POSE_ESTIMATION_H
00030 #define HECTOR_POSE_ESTIMATION_H
00031
00032 #include <hector_pose_estimation/types.h>
00033 #include <hector_pose_estimation/state.h>
00034 #include <hector_pose_estimation/system.h>
00035 #include <hector_pose_estimation/measurement.h>
00036 #include <hector_pose_estimation/parameters.h>
00037
00038 #include <boost/shared_ptr.hpp>
00039 #include <vector>
00040
00041 #include <hector_pose_estimation/measurements/rate.h>
00042 #include <hector_pose_estimation/measurements/gravity.h>
00043 #include <hector_pose_estimation/measurements/zerorate.h>
00044
00045 namespace hector_pose_estimation {
00046
00047 class Filter;
00048 class GlobalReference;
00049
00050 class PoseEstimation
00051 {
00052 public:
00053 PoseEstimation(const SystemPtr& system = SystemPtr());
00054 template <typename ConcreteSystemModel> PoseEstimation(ConcreteSystemModel *system_model);
00055 virtual ~PoseEstimation();
00056
00057 static PoseEstimation *Instance();
00058
00059 bool init();
00060 void cleanup();
00061 void reset();
00062
00063 void update(ros::Time timestamp);
00064 void update(double dt);
00065
00066 const SystemPtr& addSystem(const SystemPtr& system, const std::string& name = "system");
00067 const SystemPtr& addSystem(System *system) { return addSystem(SystemPtr(system)); }
00068 template <typename ConcreteSystemModel> const SystemPtr& addSystem(ConcreteSystemModel *model, const std::string& name = "system");
00069
00070 SystemPtr getSystem(const std::string& name) const { return systems_.get(name); }
00071
00072
00073
00074
00075
00076
00077
00078 const SystemModel *getSystemModel(const std::string& name) const {
00079 SystemPtr system = systems_.get(name);
00080 if (!system) return 0;
00081 return system->getModel();
00082 }
00083
00084 const MeasurementPtr& addMeasurement(const MeasurementPtr& measurement, const std::string& name = std::string());
00085 const MeasurementPtr& addMeasurement(Measurement *measurement) { return addMeasurement(MeasurementPtr(measurement)); }
00086 template <class ConcreteMeasurementModel> const MeasurementPtr& addMeasurement(ConcreteMeasurementModel *model, const std::string& name);
00087
00088 MeasurementPtr getMeasurement(const std::string &name) const { return measurements_.get(name); }
00089
00090 template <class InputType> boost::shared_ptr<InputType> registerInput(const std::string& name = std::string());
00091 template <class InputType> boost::shared_ptr<InputType> getInputType(const std::string& name) const { return inputs_.getType<InputType>(name); }
00092
00093 InputPtr addInput(const InputPtr& input, const std::string& name = std::string());
00094 InputPtr addInput(Input *input) { return addInput(InputPtr(input)); }
00095 InputPtr setInput(const Input& input, std::string name = std::string());
00096
00097 InputPtr getInput(const std::string& name) const { return inputs_.get(name); }
00098
00099 virtual const State& state() const { return filter_->state(); }
00100 virtual State& state() { return filter_->state(); }
00101
00102 virtual const State::Vector& getStateVector();
00103 virtual const State::Covariance& getCovariance();
00104
00105 virtual SystemStatus getSystemStatus() const;
00106 virtual SystemStatus getMeasurementStatus() const;
00107 virtual bool inSystemStatus(SystemStatus test_status) const;
00108 virtual bool setSystemStatus(SystemStatus new_status);
00109 virtual bool setMeasurementStatus(SystemStatus new_status);
00110 virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear);
00111 virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear);
00112
00113 virtual const GlobalReferencePtr &globalReference();
00114
00115 virtual const ros::Time& getTimestamp() const;
00116 virtual void setTimestamp(const ros::Time& timestamp);
00117
00118 virtual void getHeader(std_msgs::Header& header);
00119 virtual void getState(nav_msgs::Odometry& state, bool with_covariances = true);
00120 virtual void getPose(tf::Pose& pose);
00121 virtual void getPose(tf::Stamped<tf::Pose>& pose);
00122 virtual void getPose(geometry_msgs::Pose& pose);
00123 virtual void getPose(geometry_msgs::PoseStamped& pose);
00124 virtual void getPosition(tf::Point& point);
00125 virtual void getPosition(tf::Stamped<tf::Point>& point);
00126 virtual void getPosition(geometry_msgs::Point& pose);
00127 virtual void getPosition(geometry_msgs::PointStamped& pose);
00128 virtual void getGlobalPosition(double& latitude, double& longitude, double& altitude);
00129 virtual void getGlobalPosition(sensor_msgs::NavSatFix& global);
00130 virtual void getOrientation(tf::Quaternion& quaternion);
00131 virtual void getOrientation(tf::Stamped<tf::Quaternion>& quaternion);
00132 virtual void getOrientation(geometry_msgs::Quaternion& pose);
00133 virtual void getOrientation(geometry_msgs::QuaternionStamped& pose);
00134 virtual void getOrientation(double &yaw, double &pitch, double &roll);
00135 virtual void getImuWithBiases(geometry_msgs::Vector3& linear_acceleration, geometry_msgs::Vector3& angular_velocity);
00136 virtual void getVelocity(tf::Vector3& vector);
00137 virtual void getVelocity(tf::Stamped<tf::Vector3>& vector);
00138 virtual void getVelocity(geometry_msgs::Vector3& vector);
00139 virtual void getVelocity(geometry_msgs::Vector3Stamped& vector);
00140 virtual void getRate(tf::Vector3& vector);
00141 virtual void getRate(tf::Stamped<tf::Vector3>& vector);
00142 virtual void getRate(geometry_msgs::Vector3& vector);
00143 virtual void getRate(geometry_msgs::Vector3Stamped& vector);
00144
00145
00146 virtual void getBias(geometry_msgs::Vector3& angular_velocity, geometry_msgs::Vector3& linear_acceleration);
00147 virtual void getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration);
00148 virtual void getTransforms(std::vector<tf::StampedTransform>& transforms);
00149 virtual void updateWorldToOtherTransform(tf::StampedTransform& world_to_other_transform);
00150
00151 virtual ParameterList& parameters() { return parameters_; }
00152 virtual const ParameterList& parameters() const { return parameters_; }
00153
00154 virtual boost::shared_ptr<Filter> filter() { return filter_; }
00155 virtual boost::shared_ptr<const Filter> filter() const { return filter_; }
00156
00157 virtual void updated();
00158
00159 protected:
00160 Systems systems_;
00161 Measurements measurements_;
00162 Inputs inputs_;
00163
00164 private:
00165 boost::shared_ptr<Filter> filter_;
00166
00167 ParameterList parameters_;
00168
00169 ros::Time timestamp_;
00170 std::string world_frame_;
00171 std::string nav_frame_;
00172 std::string base_frame_;
00173 std::string stabilized_frame_;
00174 std::string footprint_frame_;
00175 std::string position_frame_;
00176
00177 ros::Time alignment_start_;
00178 double alignment_time_;
00179
00180 double gravity_;
00181
00182 boost::shared_ptr<Rate> rate_update_;
00183 boost::shared_ptr<Gravity> gravity_update_;
00184 boost::shared_ptr<ZeroRate> zerorate_update_;
00185
00186 };
00187
00188 template <typename ConcreteSystemModel>
00189 PoseEstimation::PoseEstimation(ConcreteSystemModel *system_model) {
00190 *this = PoseEstimation(System::create(system_model));
00191 }
00192
00193 template <typename ConcreteSystemModel>
00194 const SystemPtr& PoseEstimation::addSystem(ConcreteSystemModel *model, const std::string& name)
00195 {
00196 return addSystem(System::create(model, name));
00197 }
00198
00199 template <class ConcreteMeasurementModel>
00200 const MeasurementPtr& PoseEstimation::addMeasurement(ConcreteMeasurementModel *model, const std::string& name) {
00201 return addMeasurement(Measurement::create(model, name));
00202 }
00203
00204 template <class InputType>
00205 boost::shared_ptr<InputType> PoseEstimation::registerInput(const std::string& name) {
00206 boost::shared_ptr<InputType> input = getInputType<InputType>(name);
00207 if (input) return input;
00208
00209 input.reset(new InputType());
00210 if (!addInput(boost::shared_static_cast<Input>(input), name)) input.reset();
00211 return input;
00212 }
00213
00214 }
00215
00216 #endif // HECTOR_POSE_ESTIMATION_H