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