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 #include <ros/console.h>
00034
00035 using namespace std;
00036
00037 namespace hector_pose_estimation {
00038
00039 GlobalReference::GlobalReference()
00040 {
00041 parameters().add("reference_latitude", reference_latitude_ = std::numeric_limits<double>::quiet_NaN());
00042 parameters().add("reference_longitude", reference_longitude_ = std::numeric_limits<double>::quiet_NaN());
00043 parameters().add("reference_altitude", reference_altitude_ = std::numeric_limits<double>::quiet_NaN());
00044 parameters().add("reference_heading", heading_.value = std::numeric_limits<double>::quiet_NaN());
00045
00046 reset();
00047 }
00048
00049 const GlobalReferencePtr &GlobalReference::Instance()
00050 {
00051 static GlobalReferencePtr instance;
00052 if (!instance) { instance.reset(new GlobalReference); }
00053 return instance;
00054 }
00055
00056 void GlobalReference::reset()
00057 {
00058 position_ = Position();
00059 heading_ = Heading();
00060 radius_ = Radius();
00061
00062
00063 position_.latitude = reference_latitude_ * M_PI/180.0;
00064 position_.longitude = reference_longitude_ * M_PI/180.0;
00065 position_.altitude = reference_altitude_;
00066 heading_.value = reference_heading_;
00067
00068 updated();
00069 }
00070
00071 ParameterList& GlobalReference::parameters() {
00072 return parameters_;
00073 }
00074
00075 void GlobalReference::updated(bool intermediate) {
00076
00077 if (hasPosition()) {
00078 radius_ = Radius(position_.latitude);
00079 }
00080
00081
00082 if (hasHeading()) {
00083 sincos(heading_.value, &heading_.sin, &heading_.cos);
00084 }
00085
00086
00087 if (!intermediate) {
00088 for(std::list<UpdateCallback>::iterator cb = update_callbacks_.begin(); cb != update_callbacks_.end(); ++cb)
00089 (*cb)();
00090 }
00091 }
00092
00093 GlobalReference::Heading::Heading(double heading) : value(heading)
00094 {
00095 sincos(heading, &sin, &cos);
00096 }
00097
00098 Quaternion GlobalReference::Heading::quaternion() const
00099 {
00100 double sin_2, cos_2;
00101 sincos(0.5 * value, &sin_2, &cos_2);
00102 return Quaternion(cos_2, 0.0, 0.0, -sin_2);
00103 }
00104
00105 GlobalReference::Radius::Radius(double latitude) {
00106
00107 static const double equatorial_radius = 6378137.0;
00108 static const double flattening = 1.0/298.257223563;
00109 static const double excentrity2 = 2*flattening - flattening*flattening;
00110
00111 double temp = 1.0 / (1.0 - excentrity2 * sin(latitude) * sin(latitude));
00112 double prime_vertical_radius = equatorial_radius * sqrt(temp);
00113 north = prime_vertical_radius * (1 - excentrity2) * temp;
00114 east = prime_vertical_radius * cos(latitude);
00115 }
00116
00117 void GlobalReference::fromWGS84(double latitude, double longitude, double &x, double &y) {
00118 if (!hasPosition()) {
00119 x = 0.0;
00120 y = 0.0;
00121 return;
00122 }
00123 double north = radius_.north * (latitude - position_.latitude);
00124 double east = radius_.east * (longitude - position_.longitude);
00125 fromNorthEast(north, east, x, y);
00126 }
00127
00128 void GlobalReference::toWGS84(double x, double y, double &latitude, double &longitude) {
00129 if (!hasPosition()) {
00130 latitude = 0.0;
00131 longitude = 0.0;
00132 return;
00133 }
00134
00135 double north, east;
00136 toNorthEast(x, y, north, east);
00137 latitude = position_.latitude + north / radius_.north;
00138 longitude = position_.longitude + east / radius_.east;
00139 }
00140
00141 void GlobalReference::fromNorthEast(double north, double east, double &x, double &y) {
00142 if (!hasHeading()) {
00143 x = 0.0;
00144 y = 0.0;
00145 return;
00146 }
00147 x = north * heading_.cos + east * heading_.sin;
00148 y = north * heading_.sin - east * heading_.cos;
00149 }
00150
00151 void GlobalReference::toNorthEast(double x, double y, double &north, double &east) {
00152 if (!hasHeading()) {
00153 north = 0.0;
00154 east = 0.0;
00155 return;
00156 }
00157 north = x * heading_.cos + y * heading_.sin;
00158 east = x * heading_.sin - y * heading_.cos;
00159 }
00160
00161 GlobalReference& GlobalReference::setPosition(double latitude, double longitude, bool intermediate ) {
00162 position_.latitude = latitude;
00163 position_.longitude = longitude;
00164 if (!intermediate) 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);
00165 updated(intermediate);
00166 return *this;
00167 }
00168
00169 GlobalReference& GlobalReference::setHeading(double heading, bool intermediate ) {
00170 heading_.value = heading;
00171 if (!intermediate) ROS_INFO("Set new reference heading to %.1f degress", this->heading() * 180.0 / M_PI);
00172 updated(intermediate);
00173 return *this;
00174 }
00175
00176 GlobalReference& GlobalReference::setAltitude(double altitude, bool intermediate ) {
00177 position_.altitude = altitude;
00178 if (!intermediate) ROS_INFO("Set new reference altitude to %.2f m", this->position().altitude);
00179 updated(intermediate);
00180 return *this;
00181 }
00182
00183 GlobalReference& GlobalReference::setCurrentPosition(const State& state, double new_latitude, double new_longitude) {
00184 State::ConstPositionType position = state.getPosition();
00185
00186
00187 setPosition(new_latitude, new_longitude, true);
00188
00189
00190
00191
00192 toWGS84(-position.x(), -position.y(), new_latitude, new_longitude);
00193 setPosition(new_latitude, new_longitude);
00194
00195 return *this;
00196 }
00197
00198 GlobalReference& GlobalReference::setCurrentHeading(const State& state, double new_heading) {
00199
00200 double current_yaw = state.getYaw();
00201 State::ConstPositionType position = state.getPosition();
00202
00203
00204 double current_latitude, current_longitude;
00205 if (hasPosition()) {
00206 toWGS84(position.x(), position.y(), current_latitude, current_longitude);
00207 }
00208
00209
00210 setHeading(new_heading - (-current_yaw));
00211
00212
00213 if (hasPosition()) {
00214 setCurrentPosition(state, current_latitude, current_longitude);
00215 }
00216
00217 return *this;
00218 }
00219
00220 GlobalReference& GlobalReference::setCurrentAltitude(const State& state, double new_altitude) {
00221 State::ConstPositionType position = state.getPosition();
00222 setAltitude(new_altitude - position.z());
00223 return *this;
00224 }
00225
00226 void GlobalReference::getGeoPose(geographic_msgs::GeoPose& geopose) const
00227 {
00228 Quaternion orientation(heading().quaternion());
00229 geopose.orientation.w = orientation.w();
00230 geopose.orientation.x = orientation.x();
00231 geopose.orientation.y = orientation.y();
00232 geopose.orientation.z = orientation.z();
00233 geopose.position.latitude = position().latitude * 180.0/M_PI;
00234 geopose.position.longitude = position().longitude * 180.0/M_PI;
00235 geopose.position.altitude = position().altitude;
00236 }
00237
00238 bool GlobalReference::getWorldToNavTransform(geometry_msgs::TransformStamped& transform, const std::string &world_frame, const std::string &nav_frame, const ros::Time& stamp) const
00239 {
00240
00241
00242 if (isnan(reference_latitude_) ||
00243 isnan(reference_longitude_) ||
00244 isnan(reference_altitude_) ||
00245 isnan(reference_heading_)) {
00246 return false;
00247 }
00248
00249 transform.header.stamp = stamp;
00250 transform.header.frame_id = world_frame;
00251 transform.child_frame_id = nav_frame;
00252
00253 Radius reference_radius(reference_latitude_ * M_PI/180.0);
00254 double north = reference_radius.north * (position_.latitude - reference_latitude_ * M_PI/180.0);
00255 double east = reference_radius.east * (position_.longitude - reference_longitude_ * M_PI/180.0);
00256 Heading reference_heading(reference_heading_ * M_PI/180.0);
00257 transform.transform.translation.x = north * reference_heading.cos + east * reference_heading.sin;
00258 transform.transform.translation.y = north * reference_heading.sin - east * reference_heading.cos;
00259 transform.transform.translation.z = position_.altitude - reference_altitude_;
00260 double heading_diff = heading().value - reference_heading;
00261 transform.transform.rotation.w = cos(heading_diff / 2.);
00262 transform.transform.rotation.x = 0.0;
00263 transform.transform.rotation.y = 0.0;
00264 transform.transform.rotation.z = -sin(heading_diff / 2.);
00265
00266 return true;
00267 }
00268
00269 void GlobalReference::addUpdateCallback(const UpdateCallback &cb)
00270 {
00271 update_callbacks_.push_back(cb);
00272 }
00273
00274 }