mavros_uas.h

Go to the documentation of this file.

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

#pragma once

#include <array>
#include <mutex>
#include <atomic>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <mavconn/interface.h>

#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)

#define UAS_PACK_CHAN(uasobjptr)                        \
        UAS_FCU(uasobjptr)->get_system_id(),            \
        UAS_FCU(uasobjptr)->get_component_id(),         \
        UAS_FCU(uasobjptr)->get_channel()

#define UAS_PACK_TGT(uasobjptr)                         \
        (uasobjptr)->get_tgt_system(),                  \
        (uasobjptr)->get_tgt_component()

class UAS {
public:
        typedef std::lock_guard<std::recursive_mutex> lock_guard;
        typedef std::unique_lock<std::recursive_mutex> unique_lock;

        typedef boost::array<double, 9> Covariance3d;
        typedef boost::array<double, 36> Covariance6d;

        typedef Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > EigenMapCovariance3d;
        typedef Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > EigenMapConstCovariance3d;
        typedef Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > EigenMapCovariance6d;
        typedef Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > EigenMapConstCovariance6d;

        UAS();
        ~UAS() {};

        void stop(void);

        mavconn::MAVConnInterface::Ptr fcu_link;

        diagnostic_updater::Updater diag_updater;

        boost::signals2::signal<void(bool)> sig_connection_changed;

        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_);

        inline enum MAV_TYPE get_type() {
                uint8_t type_ = type;
                return static_cast<enum MAV_TYPE>(type_);
        }

        inline enum MAV_AUTOPILOT get_autopilot() {
                uint8_t autopilot_ = autopilot;
                return static_cast<enum MAV_AUTOPILOT>(autopilot_);
        }

        inline bool get_armed() {
                uint8_t base_mode_ = base_mode;
                return base_mode_ & MAV_MODE_FLAG_SAFETY_ARMED;
        }

        inline bool get_hil_state() {
                uint8_t base_mode_ = base_mode;
                return base_mode_ & 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(sensor_msgs::Imu::Ptr &imu);

        sensor_msgs::Imu::Ptr get_attitude_imu();

        geometry_msgs::Quaternion get_attitude_orientation();

        geometry_msgs::Vector3 get_attitude_angular_velocity();


        /* -*- 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();


        /* -*- transform -*- */

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

        /* -*- 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;
        }

        /* -*- autopilot version -*- */
        uint64_t get_capabilities();
        void update_capabilities(bool known, uint64_t caps = 0);

        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 -*- */

        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);

        static std::string str_autopilot(enum MAV_AUTOPILOT ap);

        static std::string str_type(enum MAV_TYPE type);

        static std::string str_system_status(enum MAV_STATE st);

        static Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);

        static std::string str_sensor_orientation(MAV_SENSOR_ORIENTATION orientation);

        static int orientation_from_str(const std::string &sensor_orientation);

        /* -*- frame conversion utilities -*- */

        static Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy);

        static inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw) {
                return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw));
        }

        static Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q);

        static inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw) {
                const auto rpy = quaternion_to_rpy(q);
                roll = rpy.x();
                pitch = rpy.y();
                yaw = rpy.z();
        }

        static double quaternion_get_yaw(const Eigen::Quaterniond &q);

        static inline void quaternion_to_mavlink(const Eigen::Quaterniond &q, float qmsg[4]) {
                qmsg[0] = q.w();
                qmsg[1] = q.x();
                qmsg[2] = q.y();
                qmsg[3] = q.z();
        }

        enum class STATIC_TRANSFORM : uint8_t {
                NED_TO_ENU,     
                ENU_TO_NED,     
                AIRCRAFT_TO_BASELINK,   
                BASELINK_TO_AIRCRAFT    
        };

        static Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const STATIC_TRANSFORM transform);

        static Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q);

        static Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q);

        // XXX TODO implement that function
        static Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q);

        static Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const STATIC_TRANSFORM transform);

        static Covariance3d transform_static_frame(const Covariance3d &cov, const STATIC_TRANSFORM transform);

        // XXX TODO implement that function
        static Covariance6d transform_static_frame(const Covariance6d &cov, const STATIC_TRANSFORM transform);

        template<class T>
        static inline T transform_orientation_ned_enu(const T &in) {
                return transform_orientation(in, STATIC_TRANSFORM::NED_TO_ENU);
        }

        template<class T>
        static inline T transform_orientation_enu_ned(const T &in) {
                return transform_orientation(in, STATIC_TRANSFORM::ENU_TO_NED);
        }

        template<class T>
        static inline T transform_orientation_aircraft_baselink(const T &in) {
                return transform_orientation(in, STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK);
        }

        template<class T>
        static inline T transform_orientation_baselink_aircraft(const T &in) {
                return transform_orientation(in, STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT);
        }

        template<class T>
        static inline T transform_frame_ned_enu(const T &in) {
                return transform_static_frame(in, STATIC_TRANSFORM::NED_TO_ENU);
        }

        template<class T>
        static inline T transform_frame_enu_ned(const T &in) {
                return transform_static_frame(in, STATIC_TRANSFORM::ENU_TO_NED);
        }

        template<class T>
        static inline T transform_frame_aircraft_baselink(const T &in) {
                return transform_static_frame(in, STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK);
        }

        template<class T>
        static inline T transform_frame_baselink_aircraft(const T &in) {
                return transform_static_frame(in, STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT);
        }

        template<class T>
        static inline T transform_frame_aircraft_ned(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        template<class T>
        static inline T transform_frame_ned_aircraft(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        template<class T>
        static inline T transform_frame_aircraft_enu(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        template<class T>
        static inline T transform_frame_enu_aircraft(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        template<class T>
        static inline T transform_frame_enu_baselink(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        template<class T>
        static inline T transform_frame_baselink_enu(const T &in,const Eigen::Quaterniond &q) {
                return transform_frame(in, q);
        }

        static inline double transform_frame_yaw_enu_ned(double yaw) {
                return transform_frame_yaw(yaw);
        }

        static inline double transform_frame_yaw_ned_enu(double yaw) {
                return transform_frame_yaw(yaw);
        }

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;

        sensor_msgs::Imu::Ptr imu_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;

        std::atomic<bool> fcu_caps_known;
        std::atomic<uint64_t> fcu_capabilities;

        static inline double transform_frame_yaw(double yaw) {
                return -yaw;
        }
};
};      // namespace mavros