00001
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include <angles/angles.h>
00020 #include <mavros/mavros_plugin.h>
00021 #include <mavros/gps_conversions.h>
00022 #include <pluginlib/class_list_macros.h>
00023 #include <eigen_conversions/eigen_msg.h>
00024
00025 #include <std_msgs/Float64.h>
00026 #include <nav_msgs/Odometry.h>
00027 #include <sensor_msgs/NavSatFix.h>
00028 #include <sensor_msgs/NavSatStatus.h>
00029 #include <geometry_msgs/TwistStamped.h>
00030 #include <geometry_msgs/TransformStamped.h>
00031
00032 namespace mavplugin {
00033
00034
00042 class GlobalPositionPlugin : public MavRosPlugin {
00043 public:
00044 GlobalPositionPlugin() :
00045 gp_nh("~global_position"),
00046 uas(nullptr),
00047 tf_send(false),
00048 rot_cov(99999.0)
00049 { };
00050
00051 void initialize(UAS &uas_)
00052 {
00053 uas = &uas_;
00054
00055
00056 gp_nh.param<std::string>("frame_id", frame_id, "map");
00057 gp_nh.param("rot_covariance", rot_cov, 99999.0);
00058
00059 gp_nh.param("tf/send", tf_send, true);
00060 gp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
00061 gp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");
00062
00063 UAS_DIAG(uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);
00064
00065
00066 raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);
00067 raw_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("raw/gps_vel", 10);
00068
00069
00070 gp_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
00071 gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 10);
00072 gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
00073 gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
00074 }
00075
00076 const message_map get_rx_handlers() {
00077 return {
00078 MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_RAW_INT, &GlobalPositionPlugin::handle_gps_raw_int),
00079
00080 MESSAGE_HANDLER(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &GlobalPositionPlugin::handle_global_position_int)
00081 };
00082 }
00083
00084 private:
00085 ros::NodeHandle gp_nh;
00086 UAS *uas;
00087
00088 ros::Publisher raw_fix_pub;
00089 ros::Publisher raw_vel_pub;
00090 ros::Publisher gp_odom_pub;
00091 ros::Publisher gp_fix_pub;
00092 ros::Publisher gp_hdg_pub;
00093 ros::Publisher gp_rel_alt_pub;
00094
00095 std::string frame_id;
00096 std::string tf_frame_id;
00097 std::string tf_child_frame_id;
00098 bool tf_send;
00099 double rot_cov;
00100
00101 template<typename MsgT>
00102 inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
00103 fix->latitude = msg.lat / 1E7;
00104 fix->longitude = msg.lon / 1E7;
00105 fix->altitude = msg.alt / 1E3;
00106 }
00107
00108 inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) {
00109 fix->position_covariance.fill(0.0);
00110 fix->position_covariance[0] = -1.0;
00111 fix->position_covariance_type =
00112 sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00113 }
00114
00115
00116
00117 void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00118 mavlink_gps_raw_int_t raw_gps;
00119 mavlink_msg_gps_raw_int_decode(msg, &raw_gps);
00120
00121 auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
00122
00123 fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);
00124
00125 fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00126 if (raw_gps.fix_type > 2)
00127 fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
00128 else {
00129 ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
00130 fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00131 }
00132
00133 fill_lla(raw_gps, fix);
00134
00135 float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
00136 float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;
00137
00138 if (!std::isnan(eph)) {
00139 const double hdop = eph;
00140
00141
00142 fix->position_covariance[0 + 0] = \
00143 fix->position_covariance[3 + 1] = std::pow(hdop, 2);
00144 fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
00145 fix->position_covariance_type =
00146 sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
00147 }
00148 else {
00149 fill_unknown_cov(fix);
00150 }
00151
00152
00153 uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
00154 raw_fix_pub.publish(fix);
00155
00156 if (raw_gps.vel != UINT16_MAX &&
00157 raw_gps.cog != UINT16_MAX) {
00158 double speed = raw_gps.vel / 1E2;
00159 double course = angles::from_degrees(raw_gps.cog / 1E2);
00160
00161 auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
00162
00163 vel->header.frame_id = frame_id;
00164 vel->header.stamp = fix->header.stamp;
00165
00166
00167 vel->twist.linear.x = speed * std::sin(course);
00168 vel->twist.linear.y = speed * std::cos(course);
00169
00170 raw_vel_pub.publish(vel);
00171 }
00172 }
00173
00176 void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
00177 mavlink_global_position_int_t gpos;
00178 mavlink_msg_global_position_int_decode(msg, &gpos);
00179
00180 auto odom = boost::make_shared<nav_msgs::Odometry>();
00181 auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
00182 auto relative_alt = boost::make_shared<std_msgs::Float64>();
00183 auto compass_heading = boost::make_shared<std_msgs::Float64>();
00184
00185 auto header = uas->synchronized_header(frame_id, gpos.time_boot_ms);
00186
00187
00188 fix->header = header;
00189
00190 fill_lla(gpos, fix);
00191
00192
00193 auto raw_fix = uas->get_gps_fix();
00194 if (raw_fix) {
00195 fix->status.service = raw_fix->status.service;
00196 fix->status.status = raw_fix->status.status;
00197 fix->position_covariance = raw_fix->position_covariance;
00198 fix->position_covariance_type = raw_fix->position_covariance_type;
00199 }
00200 else {
00201
00202 fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00203 fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00204
00205
00206 fill_unknown_cov(fix);
00207 }
00208
00209 relative_alt->data = gpos.relative_alt / 1E3;
00210 compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN;
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222 odom->header = header;
00223
00224
00225 tf::vectorEigenToMsg(
00226 Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
00227 odom->twist.twist.linear);
00228
00229
00230 UAS::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
00231 vel_cov_out.fill(0.0);
00232 vel_cov_out(0) = -1.0;
00233
00234
00235 double northing, easting;
00236 std::string zone;
00237
00241 UTM::LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);
00242
00243 odom->pose.pose.position.x = easting;
00244 odom->pose.pose.position.y = northing;
00245 odom->pose.pose.position.z = relative_alt->data;
00246 odom->pose.pose.orientation = uas->get_attitude_orientation();
00247
00248
00249 UAS::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
00250 UAS::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());
00251 pos_cov_out <<
00252 gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0 , 0.0 , 0.0 ,
00253 gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0 , 0.0 , 0.0 ,
00254 gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0 , 0.0 , 0.0 ,
00255 0.0 , 0.0 , 0.0 , rot_cov , 0.0 , 0.0 ,
00256 0.0 , 0.0 , 0.0 , 0.0 , rot_cov , 0.0 ,
00257 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , rot_cov ;
00258
00259
00260 gp_fix_pub.publish(fix);
00261 gp_odom_pub.publish(odom);
00262 gp_rel_alt_pub.publish(relative_alt);
00263 gp_hdg_pub.publish(compass_heading);
00264
00265
00266 if (tf_send) {
00267 geometry_msgs::TransformStamped transform;
00268
00269 transform.header.stamp = odom->header.stamp;
00270 transform.header.frame_id = tf_frame_id;
00271 transform.child_frame_id = tf_child_frame_id;
00272
00273
00274 transform.transform.rotation = odom->pose.pose.orientation;
00275
00276
00277 transform.transform.translation.x = odom->pose.pose.position.x;
00278 transform.transform.translation.y = odom->pose.pose.position.y;
00279 transform.transform.translation.z = odom->pose.pose.position.z;
00280
00281 uas->tf2_broadcaster.sendTransform(transform);
00282 }
00283 }
00284
00285
00286 void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
00287 int fix_type, satellites_visible;
00288 float eph, epv;
00289
00290 uas->get_gps_epts(eph, epv, fix_type, satellites_visible);
00291
00292 if (satellites_visible <= 0)
00293 stat.summary(2, "No satellites");
00294 else if (fix_type < 2)
00295 stat.summary(1, "No fix");
00296 else if (fix_type == 2)
00297 stat.summary(0, "2D fix");
00298 else if (fix_type >= 3)
00299 stat.summary(0, "3D fix");
00300
00301 stat.addf("Satellites visible", "%zd", satellites_visible);
00302 stat.addf("Fix type", "%d", fix_type);
00303
00304 if (!std::isnan(eph))
00305 stat.addf("EPH (m)", "%.2f", eph);
00306 else
00307 stat.add("EPH (m)", "Unknown");
00308
00309 if (!std::isnan(epv))
00310 stat.addf("EPV (m)", "%.2f", epv);
00311 else
00312 stat.add("EPV (m)", "Unknown");
00313 }
00314 };
00315 };
00316
00317 PLUGINLIB_EXPORT_CLASS(mavplugin::GlobalPositionPlugin, mavplugin::MavRosPlugin)