$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer and Martin Nowara, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 //================================================================================================= 00028 00029 #ifndef HECTOR_POSE_ESTIMATION_H 00030 #define HECTOR_POSE_ESTIMATION_H 00031 00032 #include "types.h" 00033 #include "system.h" 00034 #include "measurement.h" 00035 #include "parameters.h" 00036 #include "global_reference.h" 00037 00038 #include <bfl/filter/extendedkalmanfilter.h> 00039 00040 #include <boost/shared_ptr.hpp> 00041 #include <vector> 00042 00043 #include <boost/function.hpp> 00044 00045 #include <ros/time.h> 00046 #include <tf/transform_datatypes.h> 00047 #include <geometry_msgs/PoseStamped.h> 00048 #include <geometry_msgs/PointStamped.h> 00049 #include <geometry_msgs/QuaternionStamped.h> 00050 #include <geometry_msgs/Vector3Stamped.h> 00051 #include <nav_msgs/Odometry.h> 00052 #include <sensor_msgs/NavSatFix.h> 00053 00054 #include "measurements/rate.h" 00055 #include "measurements/gravity.h" 00056 #include "measurements/zerorate.h" 00057 //#include "measurements/heading.h" 00058 00059 namespace hector_pose_estimation { 00060 00061 class PoseEstimation 00062 { 00063 public: 00064 PoseEstimation(const SystemPtr& system = SystemPtr()); 00065 template <typename ConcreteSystemModel> PoseEstimation(ConcreteSystemModel *system_model); 00066 virtual ~PoseEstimation(); 00067 00068 static PoseEstimation *Instance(); 00069 00070 virtual bool init(); 00071 virtual void cleanup(); 00072 virtual void reset(); 00073 00074 virtual void update(const SystemInput& input, ros::Time timestamp); 00075 virtual void update(double dt); 00076 00077 template <typename ConcreteSystemModel> const SystemPtr& setSystemModel(ConcreteSystemModel *system_model, const std::string& name = "system"); 00078 virtual const SystemPtr& setSystem(const SystemPtr& system); 00079 virtual const SystemPtr& setSystem(System *system); 00080 virtual const SystemModel *getSystemModel() const; 00081 virtual const SystemPtr& getSystem() const; 00082 00083 virtual const MeasurementPtr& addMeasurement(const MeasurementPtr& measurement); 00084 virtual const MeasurementPtr& addMeasurement(Measurement *measurement); 00085 virtual const MeasurementPtr& addMeasurement(const std::string& name, const MeasurementPtr& measurement); 00086 virtual MeasurementPtr getMeasurement(const std::string &name) const; 00087 00088 template <class ConcreteMeasurementModel> 00089 const MeasurementPtr& addMeasurement(const std::string& name, ConcreteMeasurementModel *model) { 00090 return addMeasurement(new Measurement_<ConcreteMeasurementModel>(model, name)); 00091 } 00092 00093 virtual const StateVector& getState(); 00094 virtual const StateCovariance& getCovariance(); 00095 virtual void setState(const StateVector& state); 00096 virtual void setCovariance(const StateCovariance& covariance); 00097 00098 virtual SystemStatus getSystemStatus() const; 00099 virtual SystemStatus getMeasurementStatus() const; 00100 virtual bool inSystemStatus(SystemStatus test_status) const; 00101 virtual bool setSystemStatus(SystemStatus new_status); 00102 virtual bool setMeasurementStatus(SystemStatus new_status); 00103 virtual bool updateSystemStatus(SystemStatus set, SystemStatus clear); 00104 virtual bool updateMeasurementStatus(SystemStatus set, SystemStatus clear); 00105 00106 typedef boost::function<bool(SystemStatus&)> SystemStatusCallback; 00107 virtual void setSystemStatusCallback(SystemStatusCallback callback); 00108 00109 virtual ros::Time getTimestamp() const; 00110 virtual void setTimestamp(ros::Time timestamp); 00111 00112 virtual GlobalReference* globalReference(); 00113 00114 virtual void getHeader(std_msgs::Header& header); 00115 virtual void getState(nav_msgs::Odometry& state, bool with_covariances = true); 00116 virtual void getPose(tf::Pose& pose); 00117 virtual void getPose(tf::Stamped<tf::Pose>& pose); 00118 virtual void getPose(geometry_msgs::Pose& pose); 00119 virtual void getPose(geometry_msgs::PoseStamped& pose); 00120 virtual void getPosition(tf::Point& point); 00121 virtual void getPosition(tf::Stamped<tf::Point>& point); 00122 virtual void getPosition(geometry_msgs::Point& pose); 00123 virtual void getPosition(geometry_msgs::PointStamped& pose); 00124 virtual void getGlobalPosition(double& latitude, double& longitude, double& altitude); 00125 virtual void getGlobalPosition(sensor_msgs::NavSatFix& global); 00126 virtual void getOrientation(tf::Quaternion& quaternion); 00127 virtual void getOrientation(tf::Stamped<tf::Quaternion>& quaternion); 00128 virtual void getOrientation(geometry_msgs::Quaternion& pose); 00129 virtual void getOrientation(geometry_msgs::QuaternionStamped& pose); 00130 virtual void getOrientation(double &yaw, double &pitch, double &roll); 00131 virtual void getImuWithBiases(geometry_msgs::Vector3& linear_acceleration, geometry_msgs::Vector3& angular_velocity); 00132 virtual void getVelocity(tf::Vector3& vector); 00133 virtual void getVelocity(tf::Stamped<tf::Vector3>& vector); 00134 virtual void getVelocity(geometry_msgs::Vector3& vector); 00135 virtual void getVelocity(geometry_msgs::Vector3Stamped& vector); 00136 virtual void getRate(tf::Vector3& vector); 00137 virtual void getRate(tf::Stamped<tf::Vector3>& vector); 00138 virtual void getRate(geometry_msgs::Vector3& vector); 00139 virtual void getRate(geometry_msgs::Vector3Stamped& vector); 00140 virtual void getBias(tf::Vector3& angular_velocity, tf::Vector3& linear_acceleration); 00141 virtual void getBias(tf::Stamped<tf::Vector3>& angular_velocity, tf::Stamped<tf::Vector3>& linear_acceleration); 00142 virtual void getBias(geometry_msgs::Vector3& angular_velocity, geometry_msgs::Vector3& linear_acceleration); 00143 virtual void getBias(geometry_msgs::Vector3Stamped& angular_velocity, geometry_msgs::Vector3Stamped& linear_acceleration); 00144 virtual void getTransforms(std::vector<tf::StampedTransform>& transforms); 00145 virtual void updateWorldToOtherTransform(tf::StampedTransform& world_to_other_transform); 00146 00147 virtual ParameterList getParameters() const; 00148 00149 virtual ParameterList& parameters() { return parameters_; } 00150 virtual const ParameterList& parameters() const { return parameters_; } 00151 00152 virtual BFL::KalmanFilter *filter() { return filter_.get(); } 00153 virtual const BFL::KalmanFilter *filter() const { return filter_.get(); } 00154 00155 virtual void updated(); 00156 00157 protected: 00158 SystemPtr system_; 00159 typedef std::vector<MeasurementPtr> Measurements; 00160 Measurements measurements_; 00161 00162 private: 00163 boost::shared_ptr<BFL::ExtendedKalmanFilter> filter_; 00164 00165 StateVector state_; 00166 StateCovariance covariance_; 00167 bool state_is_dirty_; 00168 bool covariance_is_dirty_; 00169 00170 SystemStatus status_; 00171 SystemStatus measurement_status_; 00172 ParameterList parameters_; 00173 00174 GlobalReference global_reference_; 00175 00176 ros::Time timestamp_; 00177 std::string world_frame_; 00178 std::string nav_frame_; 00179 std::string base_frame_; 00180 std::string stabilized_frame_; 00181 std::string footprint_frame_; 00182 std::string position_frame_; 00183 00184 ros::Time alignment_start_; 00185 double alignment_time_; 00186 00187 SystemStatusCallback status_callback_; 00188 00189 boost::shared_ptr<Rate> rate_; 00190 boost::shared_ptr<Gravity> gravity_; 00191 boost::shared_ptr<ZeroRate> zerorate_; 00192 // boost::shared_ptr<Heading> heading_; 00193 }; 00194 00195 template <typename ConcreteSystemModel> 00196 const SystemPtr& PoseEstimation::setSystemModel(ConcreteSystemModel *new_system_model, const std::string& name) { 00197 setSystem(SystemPtr()); 00198 if (!new_system_model || new_system_model == getSystemModel()) return getSystem(); 00199 return setSystem(System::create(new_system_model, name)); 00200 } 00201 00202 template <typename ConcreteSystemModel> 00203 PoseEstimation::PoseEstimation(ConcreteSystemModel *system_model) { 00204 *this = PoseEstimation(System::create(system_model)); 00205 } 00206 00207 } // namespace hector_pose_estimation 00208 00209 #endif // HECTOR_POSE_ESTIMATION_H