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_GLOBAL_REFERENCE_H
00030 #define HECTOR_POSE_ESTIMATION_GLOBAL_REFERENCE_H
00031
00032 #include <hector_pose_estimation/types.h>
00033 #include <hector_pose_estimation/parameters.h>
00034
00035 #include <geographic_msgs/GeoPose.h>
00036 #include <geometry_msgs/TransformStamped.h>
00037
00038 #include <limits>
00039 #include <boost/function.hpp>
00040
00041 namespace hector_pose_estimation {
00042
00043 class PoseEstimation;
00044
00045 class GlobalReference {
00046 public:
00047 struct Position {
00048 Position() : latitude(std::numeric_limits<double>::quiet_NaN()), longitude(std::numeric_limits<double>::quiet_NaN()), altitude(std::numeric_limits<double>::quiet_NaN()) {}
00049 double latitude;
00050 double longitude;
00051 double altitude;
00052 };
00053
00054 struct Heading {
00055 Heading() : value(std::numeric_limits<double>::quiet_NaN()), cos(1.0), sin(0.0) {}
00056 Heading(double heading);
00057 double value;
00058 double cos;
00059 double sin;
00060 operator double() const { return value; }
00061 Quaternion quaternion() const;
00062 };
00063
00064 struct Radius {
00065 Radius() : north(std::numeric_limits<double>::quiet_NaN()), east(std::numeric_limits<double>::quiet_NaN()) {}
00066 Radius(double latitude);
00067 double north;
00068 double east;
00069 };
00070
00071 const Position& position() const { return position_; }
00072 const Heading& heading() const { return heading_; }
00073 const Radius& radius() const { return radius_; }
00074
00075 void getGeoPose(geographic_msgs::GeoPose& geopose) const;
00076 bool getWorldToNavTransform(geometry_msgs::TransformStamped& transform, const std::string &world_frame, const std::string &nav_frame, const ros::Time& stamp = ros::Time()) const;
00077
00078 GlobalReference& setPosition(double latitude, double longitude, bool intermediate = false);
00079 GlobalReference& setHeading(double heading, bool intermediate = false);
00080 GlobalReference& setAltitude(double altitude, bool intermediate = false);
00081
00082 GlobalReference& setCurrentPosition(const State& state, double latitude, double longitude);
00083 GlobalReference& setCurrentHeading(const State& state, double heading);
00084 GlobalReference& setCurrentAltitude(const State& state, double altitude);
00085
00086 bool hasPosition() const { return !std::isnan(position_.latitude) && !std::isnan(position_.longitude); }
00087 bool hasHeading() const { return !std::isnan(heading_.value); }
00088 bool hasAltitude() const { return !std::isnan(position_.altitude); }
00089
00090 void fromWGS84(double latitude, double longitude, double &x, double &y);
00091 void toWGS84(double x, double y, double &latitude, double &longitude);
00092 void fromNorthEast(double north, double east, double &x, double &y);
00093 void toNorthEast(double x, double y, double &north, double &east);
00094 void fromAltitude(double altitude, double &z);
00095 void toAltitude(double z, double &altitude);
00096
00097 ParameterList& parameters();
00098
00099 static const GlobalReferencePtr &Instance();
00100
00101 void reset();
00102 void updated(bool intermediate = false);
00103
00104 typedef boost::function<void()> UpdateCallback;
00105 void addUpdateCallback(const UpdateCallback &);
00106
00107 private:
00108 GlobalReference();
00109
00110 Position position_;
00111 Heading heading_;
00112 Radius radius_;
00113
00114 ParameterList parameters_;
00115 double reference_latitude_;
00116 double reference_longitude_;
00117 double reference_altitude_;
00118 double reference_heading_;
00119
00120 std::list<UpdateCallback> update_callbacks_;
00121 };
00122
00123 }
00124
00125 #endif // HECTOR_POSE_ESTIMATION_GLOBAL_REFERENCE_H