diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp index 70d16ec59..c50612ffd 100644 --- a/mavros/src/plugins/global_position.cpp +++ b/mavros/src/plugins/global_position.cpp @@ -9,27 +9,30 @@ */ /* * Copyright 2014 Nuno Marques. + * Copyright 2015 Vladimir Ermakov. * * This file is part of the mavros package and subject to the license terms * in the top-level LICENSE file of the mavros repository. * https://github.com/mavlink/mavros/tree/master/LICENSE.md */ +#include #include #include #include +#include +#include -#include -#include -#include +#include #include #include -#include -#include -#include -#include +#include +#include +#include namespace mavplugin { + + /** * @brief Global position plugin. * @@ -42,7 +45,7 @@ class GlobalPositionPlugin : public MavRosPlugin { GlobalPositionPlugin() : gp_nh("~global_position"), uas(nullptr), - send_tf(false), + tf_send(false), rot_cov(99999.0) { }; @@ -50,21 +53,33 @@ class GlobalPositionPlugin : public MavRosPlugin { { uas = &uas_; - gp_nh.param("send_tf", send_tf, true); - gp_nh.param("frame_id", frame_id, "local_origin"); - gp_nh.param("child_frame_id", child_frame_id, "fcu"); - gp_nh.param("rot_covariance", rot_cov, 99999.0); - - fix_pub = gp_nh.advertise("global", 10); - pos_pub = gp_nh.advertise("local", 10); - vel_pub = gp_nh.advertise("gp_vel", 10); - rel_alt_pub = gp_nh.advertise("rel_alt", 10); - hdg_pub = gp_nh.advertise("compass_hdg", 10); + // general params + gp_nh.param("frame_id", frame_id, "fcu"); + gp_nh.param("rot_covariance", rot_cov, 99999.0); + // tf subsection + gp_nh.param("tf/send", tf_send, true); + gp_nh.param("tf/frame_id", tf_frame_id, "local_origin"); + gp_nh.param("tf/child_frame_id", tf_child_frame_id, "fcu"); + + UAS_DIAG(uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run); + + // gps data + raw_fix_pub = gp_nh.advertise("raw/fix", 10); + raw_vel_pub = gp_nh.advertise("raw/gps_vel", 10); + + // fused global position + gp_fix_pub = gp_nh.advertise("global", 10); + //gp_pos_pub = gp_nh.advertise("local", 10); + gp_vel_pub = gp_nh.advertise("gp_vel", 10); + gp_rel_alt_pub = gp_nh.advertise("rel_alt", 10); + gp_hdg_pub = gp_nh.advertise("compass_hdg", 10); } const message_map get_rx_handlers() { return { - MESSAGE_HANDLER(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &GlobalPositionPlugin::handle_global_position_int) + MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_RAW_INT, &GlobalPositionPlugin::handle_gps_raw_int), + // MAVLINK_MSG_ID_GPS_STATUS: there no corresponding ROS message, and it is not supported by APM + MESSAGE_HANDLER(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &GlobalPositionPlugin::handle_global_position_int) }; } @@ -72,84 +87,160 @@ class GlobalPositionPlugin : public MavRosPlugin { ros::NodeHandle gp_nh; UAS *uas; - ros::Publisher fix_pub; - ros::Publisher pos_pub; - ros::Publisher vel_pub; - ros::Publisher hdg_pub; - ros::Publisher rel_alt_pub; + ros::Publisher raw_fix_pub; + ros::Publisher raw_vel_pub; + ros::Publisher gp_fix_pub; + ros::Publisher gp_pos_pub; + ros::Publisher gp_vel_pub; + ros::Publisher gp_hdg_pub; + ros::Publisher gp_rel_alt_pub; - tf::TransformBroadcaster tf_broadcaster; + //tf::TransformBroadcaster tf_broadcaster; - std::string frame_id; //!< origin for TF - std::string child_frame_id; //!< frame for TF and Pose - bool send_tf; + std::string frame_id; //!< frame for topic headers + std::string tf_frame_id; //!< origin for TF + std::string tf_child_frame_id; //!< frame for TF and Pose + bool tf_send; double rot_cov; + template + inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) { + fix->latitude = msg.lat / 1E7; // deg + fix->longitude = msg.lon / 1E7; // deg + fix->altitude = msg.alt / 1E3; // m + } + + inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) { + fix->position_covariance.fill(0.0); + fix->position_covariance[0] = -1.0; + fix->position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + } + + /* -*- message handlers -*- */ + + void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { + mavlink_gps_raw_int_t raw_gps; + mavlink_msg_gps_raw_int_decode(msg, &raw_gps); + + auto fix = boost::make_shared(); + + fix->header.frame_id = frame_id; + fix->header.stamp = uas->synchronise_stamp(raw_gps.time_usec); + + fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; + if (raw_gps.fix_type > 2) + fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + else { + ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix"); + fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + } + + fill_lla(raw_gps, fix); + + float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN; + float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN; + + if (!isnan(eph)) { + const double hdop = eph; + + // From nmea_navsat_driver + fix->position_covariance[0 + 0] = \ + fix->position_covariance[3 + 1] = std::pow(hdop, 2); + fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2); + fix->position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; + } + else { + fill_unknown_cov(fix); + } + + // store & publish + uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible); + raw_fix_pub.publish(fix); + + if (raw_gps.vel != UINT16_MAX && + raw_gps.cog != UINT16_MAX) { + double speed = raw_gps.vel / 1E2; // m/s + double course = angles::from_degrees(raw_gps.cog / 1E2); // rad + + auto vel = boost::make_shared(); + + vel->header.frame_id = frame_id; + vel->header.stamp = fix->header.stamp; + + // From nmea_navsat_driver + vel->twist.linear.x = speed * std::sin(course); + vel->twist.linear.y = speed * std::cos(course); + + raw_vel_pub.publish(vel); + } + } + /** @todo Handler for GLOBAL_POSITION_INT_COV */ void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) { - mavlink_global_position_int_t gp_pos; - mavlink_msg_global_position_int_decode(msg, &gp_pos); - - ROS_DEBUG_THROTTLE_NAMED(10, "global_position", "Global position: boot_ms:%06d " - "lat/lon/alt:(%d %d %d) relative_alt: (%d) gp_vel:(%d %d %d) compass_heading: (%d)", - gp_pos.time_boot_ms, - gp_pos.lat, gp_pos.lon, gp_pos.alt, gp_pos.relative_alt, - gp_pos.vx, gp_pos.vy, gp_pos.vz, gp_pos.hdg); + mavlink_global_position_int_t gpos; + mavlink_msg_global_position_int_decode(msg, &gpos); auto pose_cov = boost::make_shared(); - auto gp_fix = boost::make_shared(); - auto gp_vel = boost::make_shared(); + auto fix = boost::make_shared(); + auto vel = boost::make_shared(); auto relative_alt = boost::make_shared(); auto compass_heading = boost::make_shared(); std_msgs::Header header; header.frame_id = frame_id; - header.stamp = uas->synchronise_stamp(gp_pos.time_boot_ms); + header.stamp = uas->synchronise_stamp(gpos.time_boot_ms); // Global position fix - gp_fix->header = header; - gp_fix->latitude = gp_pos.lat / 1E7; - gp_fix->longitude = gp_pos.lon / 1E7; - gp_fix->altitude = gp_pos.alt / 1E3; // in meters + fix->header = header; + + fill_lla(gpos, fix); // fill GPS status fields using GPS_RAW data auto raw_fix = uas->get_gps_fix(); if (raw_fix) { - gp_fix->status.service = raw_fix->status.service; - gp_fix->status.status = raw_fix->status.status; - gp_fix->position_covariance = raw_fix->position_covariance; - gp_fix->position_covariance_type = raw_fix->position_covariance_type; + fix->status.service = raw_fix->status.service; + fix->status.status = raw_fix->status.status; + fix->position_covariance = raw_fix->position_covariance; + fix->position_covariance_type = raw_fix->position_covariance_type; } else { // no GPS_RAW_INT -> fix status unknown - gp_fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; - gp_fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; + fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; // we don't know covariance - std::fill(gp_fix->position_covariance.begin(), - gp_fix->position_covariance.end(), 0.0); - gp_fix->position_covariance[0] = -1.0; - gp_fix->position_covariance_type = - sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + fill_unknown_cov(fix); } - // Global position velocity - gp_vel->header = header; - gp_vel->vector.x = gp_pos.vx / 1E2; // in m/s - gp_vel->vector.y = gp_pos.vy / 1E2; - gp_vel->vector.z = gp_pos.vz / 1E2; + /* Global position velocity + * + * No transform needed: + * X: latitude m/s + * Y: longitude m/s + * Z: altitude m/s + */ + vel->header = header; + tf::vectorEigenToMsg( + Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2, + vel->twist.linear); - relative_alt->data = gp_pos.relative_alt / 1E3; // in meters - compass_heading->data = (gp_pos.hdg != UINT16_MAX) ? gp_pos.hdg / 1E2 : NAN; // in degrees + relative_alt->data = gpos.relative_alt / 1E3; // in meters + compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees + // local position (UTM) calculation double northing, easting; std::string zone; +#if 0 + // temporary comment out + /** @note Adapted from gps_umd ROS package @http://wiki.ros.org/gps_umd * Author: Ken Tossell */ - UTM::LLtoUTM(gp_fix->latitude, gp_fix->longitude, northing, easting, zone); + UTM::LLtoUTM(gfix->latitude, gfix->longitude, northing, easting, zone); pose_cov->header = header; pose_cov->pose.pose.position.x = easting; @@ -178,13 +269,17 @@ class GlobalPositionPlugin : public MavRosPlugin { }; pose_cov->pose.covariance = covariance; +#endif - fix_pub.publish(gp_fix); - pos_pub.publish(pose_cov); - vel_pub.publish(gp_vel); - rel_alt_pub.publish(relative_alt); - hdg_pub.publish(compass_heading); + // publish + gp_fix_pub.publish(fix); + //gp_pos_pub.publish(pose_cov); + gp_vel_pub.publish(vel); + gp_rel_alt_pub.publish(relative_alt); + gp_hdg_pub.publish(compass_heading); +#if 0 + // need tf2 if (send_tf) { tf::Transform transform; @@ -202,6 +297,37 @@ class GlobalPositionPlugin : public MavRosPlugin { pose_cov->header.stamp, frame_id, child_frame_id)); } +#endif + } + + /* -*- diagnostics -*- */ + void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) { + int fix_type, satellites_visible; + float eph, epv; + + uas->get_gps_epts(eph, epv, fix_type, satellites_visible); + + if (satellites_visible <= 0) + stat.summary(2, "No satellites"); + else if (fix_type < 2) + stat.summary(1, "No fix"); + else if (fix_type == 2) + stat.summary(0, "2D fix"); + else if (fix_type >= 3) + stat.summary(0, "3D fix"); + + stat.addf("Satellites visible", "%zd", satellites_visible); + stat.addf("Fix type", "%d", fix_type); + + if (!isnan(eph)) + stat.addf("EPH (m)", "%.2f", eph); + else + stat.add("EPH (m)", "Unknown"); + + if (!isnan(epv)) + stat.addf("EPV (m)", "%.2f", epv); + else + stat.add("EPV (m)", "Unknown"); } }; }; // namespace mavplugin