global_position.cpp
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014 Nuno Marques.
00012  * Copyright 2015 Vladimir Ermakov.
00013  *
00014  * This file is part of the mavros package and subject to the license terms
00015  * in the top-level LICENSE file of the mavros repository.
00016  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
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                 // general params
00056                 gp_nh.param<std::string>("frame_id", frame_id, "map");
00057                 gp_nh.param("rot_covariance", rot_cov, 99999.0);
00058                 // tf subsection
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                 // gps data
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                 // fused global position
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                                 // MAVLINK_MSG_ID_GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
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;          // deg
00104                 fix->longitude = msg.lon / 1E7;         // deg
00105                 fix->altitude = msg.alt / 1E3;          // m
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         /* -*- message handlers -*- */
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                         // From nmea_navsat_driver
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                 // store & publish
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;                               // m/s
00159                         double course = angles::from_degrees(raw_gps.cog / 1E2);        // rad
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                         // From nmea_navsat_driver
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                 // Global position fix
00188                 fix->header = header;
00189 
00190                 fill_lla(gpos, fix);
00191 
00192                 // fill GPS status fields using GPS_RAW data
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                         // no GPS_RAW_INT -> fix status unknown
00202                         fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00203                         fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00204 
00205                         // we don't know covariance
00206                         fill_unknown_cov(fix);
00207                 }
00208 
00209                 relative_alt->data = gpos.relative_alt / 1E3;   // in meters
00210                 compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN;        // in degrees
00211 
00212                 /* Global position odometry
00213                  *
00214                  * Assuming no transform is needed:
00215                  * X: latitude m
00216                  * Y: longitude m
00217                  * Z: altitude m
00218                  * VX: latitude vel m/s
00219                  * VY: longitude vel m/s
00220                  * VZ: altitude vel m/s
00221                  */
00222                 odom->header = header;
00223                 
00224                 // Velocity
00225                 tf::vectorEigenToMsg(
00226                                 Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
00227                                 odom->twist.twist.linear);
00228 
00229                 // Velocity covariance unknown
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                 // Local position (UTM) calculation
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                 // Use ENU covariance to build XYZRPY covariance
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                 // publish
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                 // TF
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                         // setRotation()
00274                         transform.transform.rotation = odom->pose.pose.orientation;
00275 
00276                         // setOrigin()
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         /* -*- diagnostics -*- */
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 };      // namespace mavplugin
00316 
00317 PLUGINLIB_EXPORT_CLASS(mavplugin::GlobalPositionPlugin, mavplugin::MavRosPlugin)


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19