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