fake_gps.cpp

Go to the documentation of this file.

/*
 * Copyright 2015 Christoph Tobler.
 * Copyright 2017 Nuno Marques.
 * Copyright 2019 Amilcar Lucas.
 *
 * 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 <mavros/mavros_plugin.h>
#include <mavros/setpoint_mixin.h>
#include <eigen_conversions/eigen_msg.h>
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/Geoid.hpp>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>

// the number of GPS leap seconds
#define GPS_LEAPSECONDS_MILLIS 18000ULL

#define MSEC_PER_WEEK (7ULL * 86400ULL * 1000ULL)
#define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS)

namespace mavros {
namespace extra_plugins {
using mavlink::common::GPS_FIX_TYPE;
using mavlink::common::GPS_INPUT_IGNORE_FLAGS;

class FakeGPSPlugin : public plugin::PluginBase,
        private plugin::TF2ListenerMixin<FakeGPSPlugin> {
public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        FakeGPSPlugin() : PluginBase(),
                fp_nh("~fake_gps"),
                gps_rate(5.0),
                // WGS-84 ellipsoid (a - equatorial radius, f - flattening of ellipsoid)
                earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()),
                use_mocap(true),
                use_vision(false),
                use_hil_gps(true),
                mocap_transform(true),
                mocap_withcovariance(false),
                tf_listen(false),
                eph(2.0),
                epv(2.0),
                horiz_accuracy(0.0f),
                vert_accuracy(0.0f),
                speed_accuracy(0.0f),
                gps_id(0),
                satellites_visible(5),
                fix_type(GPS_FIX_TYPE::NO_GPS),
                tf_rate(10.0),
                map_origin(0.0, 0.0, 0.0)
        { }

        void initialize(UAS &uas_) override
        {
                PluginBase::initialize(uas_);

                double _gps_rate;
                double origin_lat, origin_lon, origin_alt;

                last_pos_time = ros::Time(0.0);

                // general params
                fp_nh.param<int>("gps_id", gps_id, 0);
                int ft_i;
                fp_nh.param<int>("fix_type", ft_i, utils::enum_value(GPS_FIX_TYPE::NO_GPS));
                fix_type = static_cast<GPS_FIX_TYPE>(ft_i);
                fp_nh.param("gps_rate", _gps_rate, 5.0);                // GPS data rate of 5hz
                gps_rate = _gps_rate;
                fp_nh.param("eph", eph, 2.0);
                fp_nh.param("epv", epv, 2.0);
                fp_nh.param<float>("horiz_accuracy", horiz_accuracy, 0.0f);
                fp_nh.param<float>("vert_accuracy", vert_accuracy, 0.0f);
                fp_nh.param<float>("speed_accuracy", speed_accuracy, 0.0f);
                fp_nh.param<int>("satellites_visible", satellites_visible, 5);

                // default origin/starting point: Zürich geodetic coordinates
                fp_nh.param("geo_origin/lat", origin_lat, 47.3667);     // [degrees]
                fp_nh.param("geo_origin/lon", origin_lon, 8.5500);      // [degrees]
                fp_nh.param("geo_origin/alt", origin_alt, 408.0);       // [meters - height over the WGS-84 ellipsoid]

                // init map origin with geodetic coordinates
                map_origin = {origin_lat, origin_lon, origin_alt};

                try {
                        earth.Forward(map_origin.x(), map_origin.y(), map_origin.z(),
                                ecef_origin.x(), ecef_origin.y(), ecef_origin.z());
                }
                catch (const std::exception& e) {
                        ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl);
                }

                // source set params
                fp_nh.param("use_mocap", use_mocap, true);              // listen to MoCap source
                fp_nh.param("mocap_transform", mocap_transform, true);  // listen to MoCap source (TransformStamped if true; PoseStamped if false)
                fp_nh.param("mocap_withcovariance", mocap_withcovariance, false);       // ~mocap/pose uses PoseWithCovarianceStamped Message

                fp_nh.param("tf/listen", tf_listen, false);             // listen to TF source
                fp_nh.param("use_vision", use_vision, false);           // listen to Vision source
                fp_nh.param("use_hil_gps", use_hil_gps, true);          // send HIL_GPS MAVLink messages if true,
                                                                        // send GPS_INPUT mavlink messages if false

                // tf params
                fp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
                fp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "fix");
                fp_nh.param("tf/rate_limit", tf_rate, 10.0);

                if (use_mocap) {
                        if (mocap_transform) {  // MoCap data in TransformStamped msg
                                mocap_tf_sub = fp_nh.subscribe("mocap/tf", 10, &FakeGPSPlugin::mocap_tf_cb, this);
                        }
                        else if (mocap_withcovariance) {// MoCap data in PoseWithCovarianceStamped msg
                                mocap_pose_cov_sub = fp_nh.subscribe("mocap/pose_cov", 10, &FakeGPSPlugin::mocap_pose_cov_cb, this);
                        }
                        else {  // MoCap data in PoseStamped msg
                                mocap_pose_sub = fp_nh.subscribe("mocap/pose", 10, &FakeGPSPlugin::mocap_pose_cb, this);
                        }
                }
                else if (use_vision) {  // Vision data in PoseStamped msg
                        vision_pose_sub = fp_nh.subscribe("vision", 10, &FakeGPSPlugin::vision_cb, this);
                }
                else if (tf_listen) {   // Pose aquired from TF Listener
                        ROS_INFO_STREAM_NAMED("fake_gps", "Listen to transform " << tf_frame_id
                                                                                 << " -> " << tf_child_frame_id);
                        tf2_start("FakeGPSVisionTF", &FakeGPSPlugin::transform_cb);
                }
                else {
                        ROS_ERROR_NAMED("fake_gps", "No pose source!");
                }
        }

        Subscriptions get_subscriptions() override
        {
                return { /* Rx disabled */ };
        }

private:
        friend class TF2ListenerMixin;
        ros::NodeHandle fp_nh;

        ros::Rate gps_rate;
        ros::Time last_pos_time;

        // Constructor for a ellipsoid
        GeographicLib::Geocentric earth;

        ros::Subscriber mocap_tf_sub;
        ros::Subscriber mocap_pose_cov_sub;
        ros::Subscriber mocap_pose_sub;
        ros::Subscriber vision_pose_sub;

        bool use_mocap;                 
        bool use_vision;                
        bool use_hil_gps;               
        bool mocap_transform;           
        bool mocap_withcovariance;      
        bool tf_listen;                 

        double eph, epv;
        float horiz_accuracy;
        float vert_accuracy;
        float speed_accuracy;
        int gps_id;
        int satellites_visible;
        GPS_FIX_TYPE fix_type;

        double tf_rate;
        std::string tf_frame_id;
        std::string tf_child_frame_id;
        ros::Time last_transform_stamp;

        Eigen::Vector3d map_origin;     
        Eigen::Vector3d ecef_origin;    
        Eigen::Vector3d old_ecef;       
        double old_stamp;               

        /* -*- mid-level helpers and low-level send -*- */

        void send_fake_gps(const ros::Time &stamp, const Eigen::Vector3d &ecef_offset) {
                // Throttle incoming messages to 5hz
                if ((ros::Time::now() - last_pos_time) < ros::Duration(gps_rate)) {
                        return;
                }
                last_pos_time = ros::Time::now();

                Eigen::Vector3d geodetic;
                Eigen::Vector3d current_ecef(ecef_origin.x() + ecef_offset.x(),
                        ecef_origin.y() + ecef_offset.y(),
                        ecef_origin.z() + ecef_offset.z());

                try {
                        earth.Reverse(current_ecef.x(), current_ecef.y(), current_ecef.z(),
                                geodetic.x(), geodetic.y(), geodetic.z());
                }
                catch (const std::exception& e) {
                        ROS_INFO_STREAM("FGPS: Caught exception: " << e.what() << std::endl);
                }

                Eigen::Vector3d vel = (old_ecef - current_ecef) / (stamp.toSec() - old_stamp);  // [m/s]

                // store old values
                old_stamp = stamp.toSec();
                old_ecef = current_ecef;

                if (use_hil_gps) {
                        mavlink::common::msg::HIL_GPS hil_gps {};

                        vel *= 1e2; // [cm/s]

                        // compute course over ground
                        double cog;
                        if (vel.x() == 0 && vel.y() == 0) {
                                cog = 0;
                        }
                        else if (vel.x() >= 0 && vel.y() < 0) {
                                cog = M_PI * 5 / 2 - atan2(vel.x(), vel.y());
                        }
                        else {
                                cog = M_PI / 2 - atan2(vel.x(), vel.y());
                        }

                        // Fill in and send message
                        hil_gps.time_usec = stamp.toNSec() / 1000;      // [useconds]
                        hil_gps.lat = geodetic.x() * 1e7;       // [degrees * 1e7]
                        hil_gps.lon = geodetic.y() * 1e7;       // [degrees * 1e7]
                        hil_gps.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID *
                                (*m_uas->egm96_5)(geodetic.x(), geodetic.y())) * 1e3;   // [meters * 1e3]
                        hil_gps.vel = vel.block<2, 1>(0, 0).norm();     // [cm/s]
                        hil_gps.vn = vel.x();                   // [cm/s]
                        hil_gps.ve = vel.y();                   // [cm/s]
                        hil_gps.vd = vel.z();                   // [cm/s]
                        hil_gps.cog = cog * 1e2;                // [degrees * 1e2]
                        hil_gps.eph = eph * 1e2;                // [cm]
                        hil_gps.epv = epv * 1e2;                // [cm]
                        hil_gps.fix_type = utils::enum_value(fix_type);;
                        hil_gps.satellites_visible = satellites_visible;

                        UAS_FCU(m_uas)->send_message_ignore_drop(hil_gps);
                }
                else {
                        mavlink::common::msg::GPS_INPUT gps_input {};

                        // Fill in and send message
                        gps_input.time_usec = stamp.toNSec() / 1000;    // [useconds]
                        gps_input.gps_id = gps_id;              //
                        gps_input.ignore_flags = 0;
                        if (speed_accuracy == 0.0f)
                                gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_SPEED_ACCURACY);
                        if (eph == 0.0f)
                                gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_HDOP);
                        if (epv == 0.0f)
                                gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VDOP);
                        if (fabs(vel.x()) <= 0.01f && fabs(vel.y()) <= 0.01f)
                                gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_HORIZ);
                        if (fabs(vel.z()) <= 0.01f)
                                gps_input.ignore_flags |= utils::enum_value(GPS_INPUT_IGNORE_FLAGS::FLAG_VEL_VERT);
                        int64_t tdiff = (gps_input.time_usec / 1000) - UNIX_OFFSET_MSEC;
                        gps_input.time_week = tdiff / MSEC_PER_WEEK;
                        gps_input.time_week_ms = tdiff - (gps_input.time_week * MSEC_PER_WEEK);
                        gps_input.speed_accuracy = speed_accuracy;      // [m/s] TODO how can this be dynamicaly calculated ???
                        gps_input.horiz_accuracy = horiz_accuracy;      // [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow
                        gps_input.vert_accuracy = vert_accuracy;// [m] will either use the static parameter value, or the dynamic covariance from function mocap_pose_cov_cb() bellow
                        gps_input.lat = geodetic.x() * 1e7;     // [degrees * 1e7]
                        gps_input.lon = geodetic.y() * 1e7;     // [degrees * 1e7]
                        gps_input.alt = (geodetic.z() + GeographicLib::Geoid::ELLIPSOIDTOGEOID *
                                (*m_uas->egm96_5)(geodetic.x(), geodetic.y())); // [meters]
                        gps_input.vn = vel.x();                 // [m/s]
                        gps_input.ve = vel.y();                 // [m/s]
                        gps_input.vd = vel.z();                 // [m/s]
                        gps_input.hdop = eph;                   // [m]
                        gps_input.vdop = epv;                   // [m]
                        gps_input.fix_type = utils::enum_value(fix_type);;
                        gps_input.satellites_visible = satellites_visible;

                        UAS_FCU(m_uas)->send_message_ignore_drop(gps_input);
                }
        }

        /* -*- callbacks -*- */
        void mocap_tf_cb(const geometry_msgs::TransformStamped::ConstPtr &trans)
        {
                Eigen::Affine3d pos_enu;
                tf::transformMsgToEigen(trans->transform, pos_enu);

                send_fake_gps(trans->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
        }

        void mocap_pose_cov_cb(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &req)
        {
                Eigen::Affine3d pos_enu;
                tf::poseMsgToEigen(req->pose.pose, pos_enu);
                horiz_accuracy = (req->pose.covariance[0] + req->pose.covariance[7]) / 2.0f;
                vert_accuracy = req->pose.covariance[14];

                send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
        }

        void mocap_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
        {
                Eigen::Affine3d pos_enu;
                tf::poseMsgToEigen(req->pose, pos_enu);

                send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
        }

        void vision_cb(const geometry_msgs::PoseStamped::ConstPtr &req)
        {
                Eigen::Affine3d pos_enu;
                tf::poseMsgToEigen(req->pose, pos_enu);

                send_fake_gps(req->header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
        }

        void transform_cb(const geometry_msgs::TransformStamped &trans)
        {
                Eigen::Affine3d pos_enu;

                tf::transformMsgToEigen(trans.transform, pos_enu);

                send_fake_gps(trans.header.stamp, ftf::transform_frame_enu_ecef(Eigen::Vector3d(pos_enu.translation()), map_origin));
        }
};
}       // namespace extra_plugins
}       // namespace mavros

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::FakeGPSPlugin, mavros::plugin::PluginBase)