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
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 GpsConversion::GpsConversion() :
00044 nh_(""), gps_sub_sync_(nh_, "fcu/gps", 1), imu_sub_sync_(nh_, "fcu/imu_custom", 1),
00045 gps_imu_sync_(GpsImuSyncPolicy(10), gps_sub_sync_, imu_sub_sync_), have_reference_(false),
00046 height_offset_(0), set_height_zero_(false), Q_90_DEG(sqrt(2.0) / 2.0, 0, 0, sqrt(2.0) / 2.0)
00047 {
00048 ros::NodeHandle nh;
00049 ros::NodeHandle pnh("~");
00050
00051 imu_sub_ = nh.subscribe("fcu/imu_custom", 1, &GpsConversion::imuCallback, this);
00052 gps_sub_ = nh.subscribe("fcu/gps", 1, &GpsConversion::gpsCallback, this);
00053 gps_custom_sub_ = nh.subscribe("fcu/gps_custom", 1, &GpsConversion::gpsCustomCallback, this);
00054
00055 gps_pose_pub_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped> ("fcu/gps_pose", 1);
00056 gps_position_pub_ = nh_.advertise<asctec_hl_comm::PositionWithCovarianceStamped> ("fcu/gps_position", 1);
00057 gps_custom_pub_ = nh_.advertise<asctec_hl_comm::GpsCustomCartesian> ("fcu/gps_position_custom", 1);
00058 zero_height_srv_ = nh.advertiseService("set_height_zero", &GpsConversion::zeroHeightCb, this);
00059
00060
00061 gps_to_enu_srv_ = nh.advertiseService("gps_to_local_enu", &GpsConversion::wgs84ToEnuSrv, this);
00062
00063 pnh.param("use_pressure_height", use_pressure_height_, false);
00064 ROS_INFO_STREAM("using height measurement from "<< (use_pressure_height_?"pressure sensor":"GPS"));
00065
00066 if (use_pressure_height_)
00067 {
00068 gps_imu_sync_.registerCallback(boost::bind(&GpsConversion::syncCallback, this, _1, _2));
00069 gps_imu_sync_.setInterMessageLowerBound(0, ros::Duration(0.180));
00070 }
00071
00072 if (nh.getParam("/gps_ref_latitude", ref_latitude_))
00073 {
00074 if (nh.getParam("/gps_ref_longitude", ref_longitude_))
00075 {
00076 if (nh.getParam("/gps_ref_altitude", ref_altitude_))
00077 {
00078 initReference(ref_latitude_, ref_longitude_, ref_altitude_);
00079 have_reference_ = true;
00080 }
00081 }
00082 }
00083 }
00084
00085 bool GpsConversion::zeroHeightCb(std_srvs::EmptyRequest & req, std_srvs::EmptyResponse & resp)
00086 {
00087 set_height_zero_ = true;
00088 return true;
00089 }
00090
00091 void GpsConversion::syncCallback(const sensor_msgs::NavSatFixConstPtr & gps,
00092 const asctec_hl_comm::mav_imuConstPtr & imu)
00093 {
00094 if (gps->status.status != sensor_msgs::NavSatStatus::STATUS_FIX)
00095 {
00096 ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
00097 return;
00098 }
00099
00100 if (!have_reference_)
00101 {
00102 ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00103 return;
00104 }
00105
00106 if (set_height_zero_)
00107 {
00108 set_height_zero_ = false;
00109 height_offset_ = imu->height;
00110 }
00111
00112 if (use_pressure_height_)
00113 {
00114 asctec_hl_comm::PositionWithCovarianceStampedPtr msg(new asctec_hl_comm::PositionWithCovarianceStamped);
00115 msg->header = gps->header;
00116 msg->position = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00117 msg->position.z = imu->height - height_offset_;
00118
00119 gps_position_pub_.publish(msg);
00120 }
00121 }
00122
00123 void GpsConversion::gpsCallback(const sensor_msgs::NavSatFixConstPtr & gps)
00124 {
00125 if (!have_reference_)
00126 {
00127 ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00128 return;
00129 }
00130
00131 if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_FIX)
00132 {
00133 gps_position_ = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00134 if (!use_pressure_height_)
00135 {
00136 asctec_hl_comm::PositionWithCovarianceStampedPtr msg(new asctec_hl_comm::PositionWithCovarianceStamped);
00137 msg->header = gps->header;
00138 msg->position = gps_position_;
00139 msg->covariance = gps->position_covariance;
00140 gps_position_pub_.publish(msg);
00141 }
00142 }
00143 else
00144 {
00145 gps_position_.x = gps_position_.y = gps_position_.z = 0;
00146 }
00147 }
00148
00149 void GpsConversion::gpsCustomCallback(const asctec_hl_comm::GpsCustomConstPtr & gps)
00150 {
00151 if (!have_reference_)
00152 {
00153 ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00154 return;
00155 }
00156
00157 if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_FIX)
00158 {
00159 geometry_msgs::Point pos = wgs84ToEnu(gps->latitude, gps->longitude, gps->altitude);
00160
00161 asctec_hl_comm::GpsCustomCartesianPtr msg(new asctec_hl_comm::GpsCustomCartesian);
00162 msg->header = gps->header;
00163 msg->position = pos;
00164 msg->position_covariance = gps->position_covariance;
00165
00166
00167 msg->velocity_x = gps->velocity_x;
00168 msg->velocity_y = gps->velocity_y;
00169
00170 msg->velocity_covariance = gps->velocity_covariance;
00171
00172 if (use_pressure_height_)
00173 {
00174 msg->position.z = gps->pressure_height - height_offset_;
00175 }
00176 gps_custom_pub_.publish(msg);
00177 }
00178
00179 }
00180
00181 void GpsConversion::imuCallback(const asctec_hl_comm::mav_imuConstPtr & imu)
00182 {
00183 if (gps_position_.x == 0 && gps_position_.y == 0 && gps_position_.z == 0)
00184 {
00185 ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
00186 return;
00187 }
00188
00189 if (!have_reference_)
00190 {
00191 ROS_WARN_STREAM_THROTTLE(1, "No GPS reference point set, not publishing");
00192 return;
00193 }
00194
00195 if (set_height_zero_)
00196 {
00197 set_height_zero_ = false;
00198 height_offset_ = imu->height;
00199 }
00200
00201 if (gps_pose_pub_.getNumSubscribers() > 0)
00202 {
00203 geometry_msgs::PoseWithCovarianceStampedPtr msg(new geometry_msgs::PoseWithCovarianceStamped);
00204
00205
00206
00207 Eigen::Quaterniond orientation(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
00208 orientation = Q_90_DEG * orientation;
00209 msg->header = imu->header;
00210 msg->pose.pose.orientation.w = orientation.w();
00211 msg->pose.pose.orientation.x = orientation.x();
00212 msg->pose.pose.orientation.y = orientation.y();
00213 msg->pose.pose.orientation.z = orientation.z();
00214 msg->pose.pose.position = gps_position_;
00215 msg->pose.pose.position.z = imu->height - height_offset_;
00216
00217 gps_pose_pub_.publish(msg);
00218 }
00219 }
00220
00221 void GpsConversion::initReference(const double & latitude, const double & longitude, const double & altitude)
00222 {
00223 Eigen::Matrix3d R;
00224 double s_long, s_lat, c_long, c_lat;
00225 sincos(latitude * DEG2RAD, &s_lat, &c_lat);
00226 sincos(longitude * DEG2RAD, &s_long, &c_long);
00227
00228 R(0, 0) = -s_long;
00229 R(0, 1) = c_long;
00230 R(0, 2) = 0;
00231
00232 R(1, 0) = -s_lat * c_long;
00233 R(1, 1) = -s_lat * s_long;
00234 R(1, 2) = c_lat;
00235
00236 R(2, 0) = c_lat * c_long;
00237 R(2, 1) = c_lat * s_long;
00238 R(2, 2) = s_lat;
00239
00240 ecef_ref_orientation_ = Eigen::Quaterniond(R);
00241
00242 ecef_ref_point_ = wgs84ToEcef(latitude, longitude, altitude);
00243 }
00244
00245 Eigen::Vector3d GpsConversion::wgs84ToEcef(const double & latitude, const double & longitude, const double & altitude)
00246 {
00247 const double a = 6378137.0;
00248 const double e_sq = 6.69437999014e-3;
00249
00250 double s_long, s_lat, c_long, c_lat;
00251 sincos(latitude * DEG2RAD, &s_lat, &c_lat);
00252 sincos(longitude * DEG2RAD, &s_long, &c_long);
00253
00254 const double N = a / sqrt(1 - e_sq * s_lat * s_lat);
00255
00256 Eigen::Vector3d ecef;
00257
00258 ecef[0] = (N + altitude) * c_lat * c_long;
00259 ecef[1] = (N + altitude) * c_lat * s_long;
00260 ecef[2] = (N * (1 - e_sq) + altitude) * s_lat;
00261
00262 return ecef;
00263 }
00264
00265 Eigen::Vector3d GpsConversion::ecefToEnu(const Eigen::Vector3d & ecef)
00266 {
00267 return ecef_ref_orientation_ * (ecef - ecef_ref_point_);
00268 }
00269
00270 bool GpsConversion::wgs84ToEnuSrv(asctec_hl_comm::Wgs84ToEnuRequest & wgs84Pt,
00271 asctec_hl_comm::Wgs84ToEnuResponse & enuPt)
00272 {
00273 geometry_msgs::Point tmp = wgs84ToEnu(wgs84Pt.lat, wgs84Pt.lon, wgs84Pt.alt);
00274 enuPt.x = tmp.x;
00275 enuPt.y = tmp.y;
00276 enuPt.z = tmp.z;
00277 return true;
00278 }
00279
00280 geometry_msgs::Point GpsConversion::wgs84ToEnu(const double & latitude, const double & longitude,
00281 const double & altitude)
00282 {
00283 geometry_msgs::Point ret;
00284 Eigen::Vector3d tmp;
00285 tmp = ecefToEnu(wgs84ToEcef(latitude, longitude, altitude));
00286 ret.x = tmp[0];
00287 ret.y = tmp[1];
00288 ret.z = tmp[2];
00289
00290
00291
00292
00293
00294
00295 return ret;
00296 }
00297
00298 geometry_msgs::Point GpsConversion::wgs84ToNwu(const double & latitude, const double & longitude,
00299 const double & altitude)
00300 {
00301 geometry_msgs::Point ret;
00302 Eigen::Vector3d tmp;
00303 tmp = ecefToEnu(wgs84ToEcef(latitude, longitude, altitude));
00304 ret.x = tmp[1];
00305 ret.y = -tmp[0];
00306 ret.z = tmp[2];
00307
00308
00309
00310
00311
00312
00313 return ret;
00314 }
00315
00316 }
00317