mavros_uas.h

Go to the documentation of this file.

/*
 * Copyright 2014,2015,2016,2017 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
 */

#pragma once

#include <array>
#include <mutex>
#include <atomic>
#include <type_traits>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <mavconn/interface.h>
#include <mavros/utils.h>
#include <mavros/frame_tf.h>

#include <GeographicLib/Geoid.hpp>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

namespace mavros {
#define UAS_FCU(uasobjptr)                              \
        ((uasobjptr)->fcu_link)

#define UAS_DIAG(uasobjptr)                             \
        ((uasobjptr)->diag_updater)


class UAS {
public:
        // common enums used by UAS
        using MAV_TYPE = mavlink::minimal::MAV_TYPE;
        using MAV_AUTOPILOT = mavlink::minimal::MAV_AUTOPILOT;
        using MAV_MODE_FLAG = mavlink::minimal::MAV_MODE_FLAG;
        using MAV_STATE = mavlink::minimal::MAV_STATE;
        using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY;
        using timesync_mode = utils::timesync_mode;

        // other UAS aliases
        using ConnectionCb = std::function<void(bool)>;
        using CapabilitiesCb = std::function<void(MAV_CAP)>;
        using lock_guard = std::lock_guard<std::recursive_mutex>;
        using unique_lock = std::unique_lock<std::recursive_mutex>;

        UAS();
        ~UAS() {};

        mavconn::MAVConnInterface::Ptr fcu_link;

        diagnostic_updater::Updater diag_updater;

        inline bool is_connected() {
                return connected;
        }

        /* -*- HEARTBEAT data -*- */

        void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_);

        void update_connection_status(bool conn_);

        void add_connection_change_handler(ConnectionCb cb);

        inline MAV_TYPE get_type() {
                std::underlying_type<MAV_TYPE>::type type_ = type;
                return static_cast<MAV_TYPE>(type_);
        }

        inline MAV_AUTOPILOT get_autopilot() {
                std::underlying_type<MAV_AUTOPILOT>::type autopilot_ = autopilot;
                return static_cast<MAV_AUTOPILOT>(autopilot_);
        }

        inline bool get_armed() {
                uint8_t base_mode_ = base_mode;
                return base_mode_ & utils::enum_value(MAV_MODE_FLAG::SAFETY_ARMED);
        }

        inline bool get_hil_state() {
                uint8_t base_mode_ = base_mode;
                return base_mode_ & utils::enum_value(MAV_MODE_FLAG::HIL_ENABLED);
        }

        /* -*- FCU target id pair -*- */

        inline uint8_t get_tgt_system() {
                return target_system;   // not changed after configuration
        }

        inline uint8_t get_tgt_component() {
                return target_component;// not changed after configuration
        }

        inline void set_tgt(uint8_t sys, uint8_t comp) {
                target_system = sys;
                target_component = comp;
        }


        /* -*- IMU data -*- */

        void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu);

        void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu);

        sensor_msgs::Imu::Ptr get_attitude_imu_enu();

        sensor_msgs::Imu::Ptr get_attitude_imu_ned();

        geometry_msgs::Quaternion get_attitude_orientation_enu();

        geometry_msgs::Quaternion get_attitude_orientation_ned();

        geometry_msgs::Vector3 get_attitude_angular_velocity_enu();

        geometry_msgs::Vector3 get_attitude_angular_velocity_ned();


        /* -*- GPS data -*- */

        void update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix,
                float eph, float epv,
                int fix_type, int satellites_visible);

        void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible);

        sensor_msgs::NavSatFix::Ptr get_gps_fix();

        /* -*- GograpticLib utils -*- */

        std::shared_ptr<GeographicLib::Geoid> egm96_5;

        template <class T>
        inline double geoid_to_ellipsoid_height(T lla)
        {
                if (egm96_5)
                        return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
                else
                        return 0.0;
        }

        template <class T>
        inline double ellipsoid_to_geoid_height(T lla)
        {
                if (egm96_5)
                        return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude);
                else
                        return 0.0;
        }

        /* -*- transform -*- */

        tf2_ros::Buffer tf2_buffer;
        tf2_ros::TransformListener tf2_listener;
        tf2_ros::TransformBroadcaster tf2_broadcaster;
        tf2_ros::StaticTransformBroadcaster tf2_static_broadcaster;

        void add_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr, std::vector<geometry_msgs::TransformStamped>& vector);

        void publish_static_transform(const std::string &frame_id, const std::string &child_id, const Eigen::Affine3d &tr);

        /* -*- time sync -*- */

        inline void set_time_offset(uint64_t offset_ns) {
                time_offset = offset_ns;
        }

        inline uint64_t get_time_offset(void) {
                return time_offset;
        }

        inline void set_timesync_mode(timesync_mode mode) {
                tsync_mode = mode;
        }

        inline timesync_mode get_timesync_mode(void) {
                return tsync_mode;
        }

        /* -*- autopilot version -*- */
        uint64_t get_capabilities();

        template<typename T>
        bool has_capability(T capability){
                static_assert(std::is_enum<T>::value, "Only query capabilities using the UAS::MAV_CAP enum.");
                return get_capabilities() & utils::enum_value(capability);
        }

        template<typename ... Ts>
        bool has_capabilities(Ts ... capabilities){
                bool ret = true;
                std::initializer_list<bool> capabilities_list{has_capability<Ts>(capabilities) ...};
                for (auto has_cap : capabilities_list) ret &= has_cap;
                return ret;
        }

        void update_capabilities(bool known, uint64_t caps = 0);

        void add_capabilities_change_handler(CapabilitiesCb cb);

        ros::Time synchronise_stamp(uint32_t time_boot_ms);
        ros::Time synchronise_stamp(uint64_t time_usec);

        template<typename T>
        inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) {
                std_msgs::Header out;
                out.frame_id = frame_id;
                out.stamp = synchronise_stamp(time_stamp);
                return out;
        }

        /* -*- utils -*- */

        template<typename _T>
        inline void msg_set_target(_T &msg) {
                msg.target_system = get_tgt_system();
                msg.target_component = get_tgt_component();
        }

        inline bool is_my_target(uint8_t sysid, uint8_t compid) {
                return sysid == get_tgt_system() && compid == get_tgt_component();
        }

        inline bool is_my_target(uint8_t sysid) {
                return sysid == get_tgt_system();
        }

        inline bool is_ardupilotmega() {
                return MAV_AUTOPILOT::ARDUPILOTMEGA == get_autopilot();
        }

        inline bool is_px4() {
                return MAV_AUTOPILOT::PX4 == get_autopilot();
        }

        std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode);

        bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);

private:
        std::recursive_mutex mutex;

        std::atomic<uint8_t> type;
        std::atomic<uint8_t> autopilot;
        std::atomic<uint8_t> base_mode;

        uint8_t target_system;
        uint8_t target_component;

        std::atomic<bool> connected;
        std::vector<ConnectionCb> connection_cb_vec;
        std::vector<CapabilitiesCb> capabilities_cb_vec;

        sensor_msgs::Imu::Ptr imu_enu_data;
        sensor_msgs::Imu::Ptr imu_ned_data;

        sensor_msgs::NavSatFix::Ptr gps_fix;
        float gps_eph;
        float gps_epv;
        int gps_fix_type;
        int gps_satellites_visible;

        std::atomic<uint64_t> time_offset;
        timesync_mode tsync_mode;

        std::atomic<bool> fcu_caps_known;
        std::atomic<uint64_t> fcu_capabilities;
};
}       // namespace mavros