gps_conversion.cpp
Go to the documentation of this file.
00001 /*
00002 
00003  Copyright (c) 2012, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004  You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006  All rights reserved.
00007 
00008  Redistribution and use in source and binary forms, with or without
00009  modification, are permitted provided that the following conditions are met:
00010  * Redistributions of source code must retain the above copyright
00011  notice, this list of conditions and the following disclaimer.
00012  * Redistributions in binary form must reproduce the above copyright
00013  notice, this list of conditions and the following disclaimer in the
00014  documentation and/or other materials provided with the distribution.
00015  * Neither the name of ETHZ-ASL nor the
00016  names of its contributors may be used to endorse or promote products
00017  derived from this software without specific prior written permission.
00018 
00019  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030  */
00031 
00032 #include "gps_conversion.h"
00033 
00034 #ifdef __APPLE__
00035 template<typename T> void sincos(T ang, T* s, T* c){
00036   __sincos(ang, s, c);
00037 }
00038 #endif
00039 
00040 namespace asctec_hl_gps
00041 {
00042 
00043 const double GpsConversion::DEG2RAD = M_PI/180.0;
00044 
00045 GpsConversion::GpsConversion() :
00046       nh_(""), gps_sub_sync_(nh_, "fcu/gps", 1), imu_sub_sync_(nh_, "fcu/imu_custom", 1),
00047       gps_imu_sync_(GpsImuSyncPolicy(10), gps_sub_sync_, imu_sub_sync_), have_reference_(false),
00048       height_offset_(0), set_height_zero_(false), Q_90_DEG(sqrt(2.0) / 2.0, 0, 0, sqrt(2.0) / 2.0)
00049 {
00050   ros::NodeHandle nh;
00051   ros::NodeHandle pnh("~");
00052 
00053   // Wait until GPS reference parameters are initialized.
00054   // Note: this loop probably does not belong to a constructor, it'd be better placed in some sort
00055   // of "init()" function
00056   do {
00057     ROS_INFO("Waiting for GPS reference parameters...");
00058     if (nh.getParam("/gps_ref_latitude", ref_latitude_) &&
00059         nh.getParam("/gps_ref_longitude", ref_longitude_) &&
00060         nh.getParam("/gps_ref_altitude", ref_altitude_)) {
00061       initReference(ref_latitude_, ref_longitude_, ref_altitude_);
00062       have_reference_ = true;
00063     }
00064     else {
00065       ROS_INFO("GPS reference not ready yet, use set_gps_reference_node to set it");
00066       ros::Duration(0.5).sleep(); // sleep for half a second
00067     }
00068   } while (!have_reference_);
00069   ROS_INFO("GPS reference initialized correctly %f, %f, %f", ref_latitude_, ref_longitude_, ref_altitude_);
00070 
00071   imu_sub_ = nh.subscribe("fcu/imu_custom", 1, &GpsConversion::imuCallback, this);
00072   gps_sub_ = nh.subscribe("fcu/gps", 1, &GpsConversion::gpsCallback, this);
00073   gps_custom_sub_ = nh.subscribe("fcu/gps_custom", 1, &GpsConversion::gpsCustomCallback, this);
00074   filtered_odometry_sub_ = nh.subscribe("fcu/filtered_odometry", 1, &GpsConversion::filteredOdometryCallback, this);
00075 
00076   gps_pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped> ("fcu/gps_pose", 1);
00077   gps_position_pub_ = nh_.advertise<asctec_hl_comm::PositionWithCovarianceStamped> ("fcu/gps_position", 1);
00078   gps_position_nocov_pub_ = nh_.advertise<geometry_msgs::PointStamped> ("fcu/gps_position_nocov", 1);
00079   gps_custom_pub_ = nh_.advertise<asctec_hl_comm::GpsCustomCartesian> ("fcu/gps_position_custom", 1);
00080 
00081   gps_filtered_pub_ = nh_.advertise<sensor_msgs::NavSatFix> ("fcu/gps_position_filtered", 1);
00082 
00083   zero_height_srv_ = nh.advertiseService("set_height_zero", &GpsConversion::zeroHeightCb, this);
00084 
00085   // Andrew Holliday's modification
00086   gps_to_enu_srv_ = nh.advertiseService("gps_to_local_enu", &GpsConversion::wgs84ToEnuSrv, this);
00087 
00088   pnh.param("use_pressure_height", use_pressure_height_, false);
00089   ROS_INFO_STREAM("using height measurement from " << (use_pressure_height_?"pressure sensor":"GPS"));
00090 
00091   if (use_pressure_height_)
00092   {
00093     gps_imu_sync_.registerCallback(boost::bind(&GpsConversion::syncCallback, this, _1, _2));
00094     gps_imu_sync_.setInterMessageLowerBound(0, ros::Duration(0.180)); // gps arrives at max with 5 Hz
00095   }
00096 }
00097 
00098 bool GpsConversion::zeroHeightCb(std_srvs::EmptyRequest & req, std_srvs::EmptyResponse & resp)
00099 {
00100   set_height_zero_ = true;
00101   return true;
00102 }
00103 
00104 void GpsConversion::syncCallback(const sensor_msgs::NavSatFixConstPtr & gps,
00105                                  const asctec_hl_comm::mav_imuConstPtr & imu)
00106 {
00107   if (gps->status.status != sensor_msgs::NavSatStatus::STATUS_FIX)
00108   {
00109     ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
00110     return;
00111   }
00112 
00113   if (!have_reference_)
00114   {
00115     ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00116     return;
00117   }
00118 
00119   if (set_height_zero_)
00120   {
00121     set_height_zero_ = false;
00122     height_offset_ = imu->height;
00123   }
00124 
00125   if (use_pressure_height_)
00126   {
00127     asctec_hl_comm::PositionWithCovarianceStampedPtr msg(new asctec_hl_comm::PositionWithCovarianceStamped);
00128     msg->header = gps->header;
00129     msg->position = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00130     msg->position.z = imu->height - height_offset_;
00131     gps_position_pub_.publish(msg);
00132 
00133     geometry_msgs::PointStampedPtr msg_nocov(new geometry_msgs::PointStamped);
00134     msg_nocov->header = msg->header;
00135     msg_nocov->point = msg->position;
00136     gps_position_nocov_pub_.publish(msg_nocov);
00137   }
00138 }
00139 
00140 void GpsConversion::gpsCallback(const sensor_msgs::NavSatFixConstPtr & gps)
00141 {
00142   if (!have_reference_)
00143   {
00144     ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00145     return;
00146   }
00147 
00148   if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_FIX)
00149   {
00150     gps_position_ = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00151     if (!use_pressure_height_)
00152     {
00153       asctec_hl_comm::PositionWithCovarianceStampedPtr msg(new asctec_hl_comm::PositionWithCovarianceStamped);
00154       msg->header = gps->header;
00155       msg->position = gps_position_;
00156       msg->covariance = gps->position_covariance;
00157       gps_position_pub_.publish(msg);
00158 
00159       geometry_msgs::PointStampedPtr msg_nocov(new geometry_msgs::PointStamped);
00160       msg_nocov->header = gps->header;
00161       msg_nocov->point = gps_position_;
00162       gps_position_nocov_pub_.publish(msg_nocov);
00163     }
00164   }
00165   else
00166   {
00167     gps_position_.x = gps_position_.y = gps_position_.z = 0;
00168   }
00169 }
00170 
00171 void GpsConversion::gpsCustomCallback(const asctec_hl_comm::GpsCustomConstPtr & gps)
00172 {
00173   if (!have_reference_)
00174   {
00175     ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00176     return;
00177   }
00178 
00179   if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_FIX)
00180   {
00181     geometry_msgs::Point pos = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00182 
00183     asctec_hl_comm::GpsCustomCartesianPtr msg(new asctec_hl_comm::GpsCustomCartesian);
00184     msg->header = gps->header;
00185     msg->position = pos;
00186     msg->position_covariance = gps->position_covariance;
00187 
00188     // rotate velocity to ENU !
00189     msg->velocity_x = gps->velocity_x;
00190     msg->velocity_y = gps->velocity_y;
00191 
00192     msg->velocity_covariance = gps->velocity_covariance;
00193 
00194     if (use_pressure_height_)
00195     {
00196       msg->position.z = gps->pressure_height - height_offset_;
00197     }
00198     gps_custom_pub_.publish(msg);
00199   }
00200 
00201 }
00202 
00203 void GpsConversion::imuCallback(const asctec_hl_comm::mav_imuConstPtr & imu)
00204 {
00205   if (gps_position_.x == 0 && gps_position_.y == 0 && gps_position_.z == 0)
00206   {
00207     ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
00208     return;
00209   }
00210 
00211   if (!have_reference_)
00212   {
00213     ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00214     return;
00215   }
00216 
00217   if (set_height_zero_)
00218   {
00219     set_height_zero_ = false;
00220     height_offset_ = imu->height;
00221   }
00222 
00223   if (gps_pose_pub_.getNumSubscribers() > 0)
00224   {
00225     geometry_msgs::PoseWithCovarianceStampedPtr msg(new geometry_msgs::PoseWithCovarianceStamped);
00226 
00227     // magnetic compass is zero when pointing north, need to rotate measurement 90 deg towards east to be consistent with ENU
00228 
00229     Eigen::Quaterniond orientation(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
00230     orientation = Q_90_DEG * orientation;
00231     msg->header = imu->header;
00232     msg->pose.pose.orientation.w = orientation.w();
00233     msg->pose.pose.orientation.x = orientation.x();
00234     msg->pose.pose.orientation.y = orientation.y();
00235     msg->pose.pose.orientation.z = orientation.z();
00236     msg->pose.pose.position = gps_position_;
00237     msg->pose.pose.position.z = imu->height - height_offset_;
00238 
00239     gps_pose_pub_.publish(msg);
00240   }
00241 }
00242 
00243 void GpsConversion::filteredOdometryCallback(const nav_msgs::Odometry & filtered_odometry)
00244 {
00245   // Fill NavSatFix message using filtered odometry and publish
00246   // Point being we obtain the filtered position of the vehicle in global
00247   // lat/lon/altitude coordinates
00248 
00249   sensor_msgs::NavSatFixPtr global_position(new sensor_msgs::NavSatFix);
00250 
00251   global_position->header = filtered_odometry.header;
00252 
00253   // filtered_odometry is in ENU
00254   const double north =  filtered_odometry.pose.pose.position.y;
00255   const double east  =  filtered_odometry.pose.pose.position.x;
00256   const double depth = -filtered_odometry.pose.pose.position.z;
00257 
00258   ned_->ned2Geodetic(north, east, depth,
00259                      global_position->latitude,
00260                      global_position->longitude,
00261                      global_position->altitude);
00262 
00263   // TODO: grab covariance from odometry
00264   global_position->position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00265   gps_filtered_pub_.publish(global_position);
00266 }
00267 
00268 void GpsConversion::initReference(const double & latitude, const double & longitude, const double & altitude)
00269 {
00270   Eigen::Matrix3d R;
00271   double s_long, s_lat, c_long, c_lat;
00272   sincos(latitude * DEG2RAD, &s_lat, &c_lat);
00273   sincos(longitude * DEG2RAD, &s_long, &c_long);
00274 
00275   R(0, 0) = -s_long;
00276   R(0, 1) = c_long;
00277   R(0, 2) = 0;
00278 
00279   R(1, 0) = -s_lat * c_long;
00280   R(1, 1) = -s_lat * s_long;
00281   R(1, 2) = c_lat;
00282 
00283   R(2, 0) = c_lat * c_long;
00284   R(2, 1) = c_lat * s_long;
00285   R(2, 2) = s_lat;
00286 
00287   ecef_ref_orientation_ = Eigen::Quaterniond(R);
00288 
00289   ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
00290 
00291   ned_.reset(new geodesy_ned::Ned(latitude, longitude, altitude));
00292 }
00293 
00294 Eigen::Vector3d GpsConversion::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude)
00295 {
00296   const double a = 6378137.0; // semi-major axis
00297   const double e_sq = 6.69437999014e-3; // first eccentricity squared
00298 
00299   double s_long, s_lat, c_long, c_lat;
00300   sincos(latitude * DEG2RAD, &s_lat, &c_lat);
00301   sincos(longitude * DEG2RAD, &s_long, &c_long);
00302 
00303   const double N = a / sqrt(1 - e_sq * s_lat * s_lat);
00304 
00305   Eigen::Vector3d ecef;
00306 
00307   ecef[0] = (N + altitude) * c_lat * c_long;
00308   ecef[1] = (N + altitude) * c_lat * s_long;
00309   ecef[2] = (N * (1 - e_sq) + altitude) * s_lat;
00310 
00311   return ecef;
00312 }
00313 
00314 Eigen::Vector3d GpsConversion::ecefToEnu(const Eigen::Vector3d & ecef)
00315 {
00316   return ecef_ref_orientation_ * (ecef - ecef_ref_point_);
00317 }
00318 
00319 geometry_msgs::Point GpsConversion::wgs84ToEnu(const double & latitude, const double & longitude,
00320                                                const double & altitude)
00321 {
00322   geometry_msgs::Point ret;
00323   Eigen::Vector3d tmp;
00324   tmp = ecefToEnu(wgs84ToEcef(latitude, longitude, altitude));
00325   ret.x = tmp[0];
00326   ret.y = tmp[1];
00327   ret.z = tmp[2];
00328 
00329   // debug ...
00330   //  double dbg_x, dbg_y;
00331   //  latlon2xy_dbg(ref_latitude_, ref_longitude_, latitude, longitude, &dbg_x, &dbg_y);
00332   //  ROS_INFO("local metric coordinates: x: %f/%f y: %f/%f", ret.x, dbg_x, ret.y, dbg_y);
00333 
00334   return ret;
00335 }
00336 
00337 geometry_msgs::Point GpsConversion::wgs84ToNwu(const double & latitude, const double & longitude,
00338                                                const double & altitude)
00339 {
00340   geometry_msgs::Point ret;
00341   Eigen::Vector3d tmp;
00342   tmp = ecefToEnu(wgs84ToEcef(latitude, longitude, altitude));
00343   ret.x = tmp[1];
00344   ret.y = -tmp[0];
00345   ret.z = tmp[2];
00346 
00347   // debug ...
00348   //  double dbg_x, dbg_y;
00349   //  latlon2xy_dbg(ref_latitude_, ref_longitude_, latitude, longitude, &dbg_x, &dbg_y);
00350   //  ROS_INFO("local metric coordinates: x: %f/%f y: %f/%f", ret.x, dbg_y, ret.y, -dbg_x);
00351 
00352   return ret;
00353 }
00354 
00355 bool GpsConversion::wgs84ToEnuSrv(asctec_hl_comm::Wgs84ToEnuRequest & wgs84Pt,
00356                                   asctec_hl_comm::Wgs84ToEnuResponse & enuPt)
00357 {
00358     geometry_msgs::Point tmp = wgs84ToEnu(wgs84Pt.lat, wgs84Pt.lon, wgs84Pt.alt);
00359     enuPt.x = tmp.x;
00360     enuPt.y = tmp.y;
00361     enuPt.z = tmp.z;
00362     return true;
00363 }
00364 
00365 } // end namespace


asctec_hl_gps
Author(s): Markus Achtelik
autogenerated on Thu Jun 6 2019 20:57:13