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/global_reference.h>
00030 #include <hector_pose_estimation/state.h>
00031 #include <cmath>
00032
00033 using namespace std;
00034
00035 namespace hector_pose_estimation {
00036
00037 GlobalReference::GlobalReference()
00038 {
00039 parameters().add("reference_latitude", position_.latitude);
00040 parameters().add("reference_longitude", position_.longitude);
00041 parameters().add("reference_altitude", position_.altitude);
00042 parameters().add("reference_heading", heading_.value);
00043
00044 reset();
00045 }
00046
00047 const GlobalReferencePtr &GlobalReference::Instance()
00048 {
00049 static GlobalReferencePtr instance;
00050 if (!instance) { instance.reset(new GlobalReference); }
00051 return instance;
00052 }
00053
00054 void GlobalReference::reset()
00055 {
00056 position_ = Position();
00057 heading_ = Heading();
00058
00059 has_position_ = false;
00060 has_heading_ = false;
00061 has_altitude_ = false;
00062
00063 updated();
00064 }
00065
00066 ParameterList& GlobalReference::parameters() {
00067 return parameters_;
00068 }
00069
00070 void GlobalReference::updated() {
00071
00072 if (position_.latitude != Position().latitude) has_position_ = true;
00073 if (position_.longitude != Position().longitude) has_position_ = true;
00074 if (position_.altitude != Position().altitude) has_altitude_ = true;
00075 if (heading_.value != Heading().value) has_heading_ = true;
00076
00077
00078 static const double equatorial_radius = 6378137.0;
00079 static const double flattening = 1.0/298.257223563;
00080 static const double excentrity2 = 2*flattening - flattening*flattening;
00081
00082
00083 double temp = 1.0 / (1.0 - excentrity2 * sin(position_.latitude) * sin(position_.latitude));
00084 double prime_vertical_radius = equatorial_radius * sqrt(temp);
00085 radius_.north = prime_vertical_radius * (1 - excentrity2) * temp;
00086 radius_.east = prime_vertical_radius * cos(position_.latitude);
00087
00088
00089 sincos(heading_.value, &heading_.sin, &heading_.cos);
00090 }
00091
00092 void GlobalReference::fromWGS84(double latitude, double longitude, double &x, double &y) {
00093 double north = radius_.north * (latitude - position_.latitude);
00094 double east = radius_.east * (longitude - position_.longitude);
00095 fromNorthEast(north, east, x, y);
00096 }
00097
00098 void GlobalReference::toWGS84(double x, double y, double &latitude, double &longitude) {
00099 if (radius_.north == 0.0 || radius_.east == 0.0) {
00100 latitude = 0.0;
00101 longitude = 0.0;
00102 return;
00103 }
00104
00105 double north, east;
00106 toNorthEast(x, y, north, east);
00107 latitude = position_.latitude + north / radius_.north;
00108 longitude = position_.longitude + east / radius_.east;
00109 }
00110
00111 void GlobalReference::fromNorthEast(double north, double east, double &x, double &y) {
00112 x = north * heading_.cos + east * heading_.sin;
00113 y = north * heading_.sin - east * heading_.cos;
00114 }
00115
00116 void GlobalReference::toNorthEast(double x, double y, double &north, double &east) {
00117 north = x * heading_.cos + y * heading_.sin;
00118 east = x * heading_.sin - y * heading_.cos;
00119 }
00120
00121 GlobalReference& GlobalReference::setPosition(double latitude, double longitude, bool quiet ) {
00122 position_.latitude = latitude;
00123 position_.longitude = longitude;
00124 has_position_ = true;
00125 updated();
00126 if (!quiet) ROS_INFO("Set new reference position to %f deg N / %f deg E", this->position().latitude * 180.0/M_PI, this->position().longitude * 180.0/M_PI);
00127 return *this;
00128 }
00129
00130 GlobalReference& GlobalReference::setHeading(double heading, bool quiet ) {
00131 heading_.value = heading;
00132 has_heading_ = true;
00133 updated();
00134 if (!quiet) ROS_INFO("Set new reference heading to %.1f degress", this->heading() * 180.0 / M_PI);
00135 return *this;
00136 }
00137
00138 GlobalReference& GlobalReference::setAltitude(double altitude, bool quiet ) {
00139 position_.altitude = altitude;
00140 has_altitude_ = true;
00141 updated();
00142 if (!quiet) ROS_INFO("Set new reference altitude to %.2f m", this->position().altitude);
00143 return *this;
00144 }
00145
00146 GlobalReference& GlobalReference::setCurrentPosition(const State& state, double new_latitude, double new_longitude) {
00147 State::ConstPositionType position = state.getPosition();
00148
00149
00150 setPosition(new_latitude, new_longitude, true);
00151
00152
00153
00154
00155 toWGS84(-position.x(), -position.y(), new_latitude, new_longitude);
00156 setPosition(new_latitude, new_longitude);
00157
00158 return *this;
00159 }
00160
00161 GlobalReference& GlobalReference::setCurrentHeading(const State& state, double new_heading) {
00162
00163 double current_yaw = state.getYaw();
00164 State::ConstPositionType position = state.getPosition();
00165
00166
00167 double current_latitude, current_longitude;
00168 if (hasPosition()) {
00169 toWGS84(position.x(), position.y(), current_latitude, current_longitude);
00170 }
00171
00172
00173 setHeading(new_heading - (-current_yaw));
00174
00175
00176 if (hasPosition()) {
00177 setCurrentPosition(state, current_latitude, current_longitude);
00178 }
00179
00180 return *this;
00181 }
00182
00183 GlobalReference& GlobalReference::setCurrentAltitude(const State& state, double new_altitude) {
00184 State::ConstPositionType position = state.getPosition();
00185 setAltitude(new_altitude - position.z());
00186 return *this;
00187 }
00188
00189 }