mavros_uas.h
Go to the documentation of this file.
00001 
00010 /*
00011  * Copyright 2014,2015 Vladimir Ermakov.
00012  *
00013  * This file is part of the mavros package and subject to the license terms
00014  * in the top-level LICENSE file of the mavros repository.
00015  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00016  */
00017 
00018 #pragma once
00019 
00020 #include <array>
00021 #include <mutex>
00022 #include <atomic>
00023 #include <Eigen/Eigen>
00024 #include <Eigen/Geometry>
00025 #include <tf2_ros/transform_listener.h>
00026 #include <tf2_ros/transform_broadcaster.h>
00027 #include <diagnostic_updater/diagnostic_updater.h>
00028 #include <mavconn/interface.h>
00029 
00030 #include <sensor_msgs/Imu.h>
00031 #include <sensor_msgs/NavSatFix.h>
00032 
00033 namespace mavros {
00037 #define UAS_FCU(uasobjptr)                              \
00038         ((uasobjptr)->fcu_link)
00039 
00043 #define UAS_DIAG(uasobjptr)                             \
00044         ((uasobjptr)->diag_updater)
00045 
00051 #define UAS_PACK_CHAN(uasobjptr)                        \
00052         UAS_FCU(uasobjptr)->get_system_id(),            \
00053         UAS_FCU(uasobjptr)->get_component_id(),         \
00054         UAS_FCU(uasobjptr)->get_channel()
00055 
00061 #define UAS_PACK_TGT(uasobjptr)                         \
00062         (uasobjptr)->get_tgt_system(),                  \
00063         (uasobjptr)->get_tgt_component()
00064 
00080 class UAS {
00081 public:
00082         typedef std::lock_guard<std::recursive_mutex> lock_guard;
00083         typedef std::unique_lock<std::recursive_mutex> unique_lock;
00084 
00086         typedef boost::array<double, 9> Covariance3d;
00088         typedef boost::array<double, 36> Covariance6d;
00089 
00091         typedef Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > EigenMapCovariance3d;
00092         typedef Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > EigenMapConstCovariance3d;
00094         typedef Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > EigenMapCovariance6d;
00095         typedef Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > EigenMapConstCovariance6d;
00096 
00097         UAS();
00098         ~UAS() {};
00099 
00103         void stop(void);
00104 
00108         mavconn::MAVConnInterface::Ptr fcu_link;
00109 
00113         diagnostic_updater::Updater diag_updater;
00114 
00120         boost::signals2::signal<void(bool)> sig_connection_changed;
00121 
00125         inline bool is_connected() {
00126                 return connected;
00127         }
00128 
00129         /* -*- HEARTBEAT data -*- */
00130 
00134         void update_heartbeat(uint8_t type_, uint8_t autopilot_, uint8_t base_mode_);
00135 
00139         void update_connection_status(bool conn_);
00140 
00144         inline enum MAV_TYPE get_type() {
00145                 uint8_t type_ = type;
00146                 return static_cast<enum MAV_TYPE>(type_);
00147         }
00148 
00152         inline enum MAV_AUTOPILOT get_autopilot() {
00153                 uint8_t autopilot_ = autopilot;
00154                 return static_cast<enum MAV_AUTOPILOT>(autopilot_);
00155         }
00156 
00162         inline bool get_armed() {
00163                 uint8_t base_mode_ = base_mode;
00164                 return base_mode_ & MAV_MODE_FLAG_SAFETY_ARMED;
00165         }
00166 
00170         inline bool get_hil_state() {
00171                 uint8_t base_mode_ = base_mode;
00172                 return base_mode_ & MAV_MODE_FLAG_HIL_ENABLED;
00173         }
00174 
00175         /* -*- FCU target id pair -*- */
00176 
00180         inline uint8_t get_tgt_system() {
00181                 return target_system;   // not changed after configuration
00182         }
00183 
00187         inline uint8_t get_tgt_component() {
00188                 return target_component;// not changed after configuration
00189         }
00190 
00191         inline void set_tgt(uint8_t sys, uint8_t comp) {
00192                 target_system = sys;
00193                 target_component = comp;
00194         }
00195 
00196 
00197         /* -*- IMU data -*- */
00198 
00200         void update_attitude_imu(sensor_msgs::Imu::Ptr &imu);
00201 
00203         sensor_msgs::Imu::Ptr get_attitude_imu();
00204 
00209         geometry_msgs::Quaternion get_attitude_orientation();
00210 
00215         geometry_msgs::Vector3 get_attitude_angular_velocity();
00216 
00217 
00218         /* -*- GPS data -*- */
00219 
00221         void update_gps_fix_epts(sensor_msgs::NavSatFix::Ptr &fix,
00222                         float eph, float epv,
00223                         int fix_type, int satellites_visible);
00224 
00226         void get_gps_epts(float &eph, float &epv, int &fix_type, int &satellites_visible);
00227 
00229         sensor_msgs::NavSatFix::Ptr get_gps_fix();
00230 
00231 
00232         /* -*- transform -*- */
00233 
00234         tf2_ros::Buffer tf2_buffer;
00235         tf2_ros::TransformListener tf2_listener;
00236         tf2_ros::TransformBroadcaster tf2_broadcaster;
00237 
00238         /* -*- time sync -*- */
00239 
00240         inline void set_time_offset(uint64_t offset_ns) {
00241                 time_offset = offset_ns;
00242         }
00243 
00244         inline uint64_t get_time_offset(void) {
00245                 return time_offset;
00246         }
00247 
00248         /* -*- autopilot version -*- */
00249         uint64_t get_capabilities();
00250         void update_capabilities(bool known, uint64_t caps = 0);
00251 
00259         ros::Time synchronise_stamp(uint32_t time_boot_ms);
00260         ros::Time synchronise_stamp(uint64_t time_usec);
00261 
00271         template<typename T>
00272         inline std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp) {
00273                 std_msgs::Header out;
00274                 out.frame_id = frame_id;
00275                 out.stamp = synchronise_stamp(time_stamp);
00276                 return out;
00277         }
00278 
00279         /* -*- utils -*- */
00280 
00284         inline bool is_my_target(uint8_t sysid, uint8_t compid) {
00285                 return sysid == get_tgt_system() && compid == get_tgt_component();
00286         }
00287 
00291         inline bool is_my_target(uint8_t sysid) {
00292                 return sysid == get_tgt_system();
00293         }
00294 
00298         inline bool is_ardupilotmega() {
00299                 return MAV_AUTOPILOT_ARDUPILOTMEGA == get_autopilot();
00300         }
00301 
00305         inline bool is_px4() {
00306                 return MAV_AUTOPILOT_PX4 == get_autopilot();
00307         }
00308 
00322         std::string str_mode_v10(uint8_t base_mode, uint32_t custom_mode);
00323 
00333         bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode);
00334 
00338         static std::string str_autopilot(enum MAV_AUTOPILOT ap);
00339 
00343         static std::string str_type(enum MAV_TYPE type);
00344 
00348         static std::string str_system_status(enum MAV_STATE st);
00349 
00354         static Eigen::Quaterniond sensor_orientation_matching(MAV_SENSOR_ORIENTATION orientation);
00355 
00359         static std::string str_sensor_orientation(MAV_SENSOR_ORIENTATION orientation);
00360 
00364         static int orientation_from_str(const std::string &sensor_orientation);
00365 
00366         /* -*- frame conversion utilities -*- */
00367 
00371         static Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy);
00372 
00378         static inline Eigen::Quaterniond quaternion_from_rpy(const double roll, const double pitch, const double yaw) {
00379                 return quaternion_from_rpy(Eigen::Vector3d(roll, pitch, yaw));
00380         }
00381 
00387         static Eigen::Vector3d quaternion_to_rpy(const Eigen::Quaterniond &q);
00388 
00392         static inline void quaternion_to_rpy(const Eigen::Quaterniond &q, double &roll, double &pitch, double &yaw) {
00393                 const auto rpy = quaternion_to_rpy(q);
00394                 roll = rpy.x();
00395                 pitch = rpy.y();
00396                 yaw = rpy.z();
00397         }
00398 
00404         static double quaternion_get_yaw(const Eigen::Quaterniond &q);
00405 
00412         static inline void quaternion_to_mavlink(const Eigen::Quaterniond &q, float qmsg[4]) {
00413                 qmsg[0] = q.w();
00414                 qmsg[1] = q.x();
00415                 qmsg[2] = q.y();
00416                 qmsg[3] = q.z();
00417         }
00418 
00422         enum class STATIC_TRANSFORM : uint8_t {
00423                 NED_TO_ENU,     
00424                 ENU_TO_NED,     
00425                 AIRCRAFT_TO_BASELINK,   
00426                 BASELINK_TO_AIRCRAFT    
00427         };
00428 
00436         static Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond &q, const STATIC_TRANSFORM transform);
00437 
00443         static Eigen::Vector3d transform_frame(const Eigen::Vector3d &vec, const Eigen::Quaterniond &q);
00444 
00450         static Covariance3d transform_frame(const Covariance3d &cov, const Eigen::Quaterniond &q);
00451 
00452         // XXX TODO implement that function
00453         static Covariance6d transform_frame(const Covariance6d &cov, const Eigen::Quaterniond &q);
00454 
00460         static Eigen::Vector3d transform_static_frame(const Eigen::Vector3d &vec, const STATIC_TRANSFORM transform);
00461 
00467         static Covariance3d transform_static_frame(const Covariance3d &cov, const STATIC_TRANSFORM transform);
00468 
00469         // XXX TODO implement that function
00470         static Covariance6d transform_static_frame(const Covariance6d &cov, const STATIC_TRANSFORM transform);
00471 
00476         template<class T>
00477         static inline T transform_orientation_ned_enu(const T &in) {
00478                 return transform_orientation(in, STATIC_TRANSFORM::NED_TO_ENU);
00479         }
00480 
00485         template<class T>
00486         static inline T transform_orientation_enu_ned(const T &in) {
00487                 return transform_orientation(in, STATIC_TRANSFORM::ENU_TO_NED);
00488         }
00489 
00494         template<class T>
00495         static inline T transform_orientation_aircraft_baselink(const T &in) {
00496                 return transform_orientation(in, STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK);
00497         }
00498 
00503         template<class T>
00504         static inline T transform_orientation_baselink_aircraft(const T &in) {
00505                 return transform_orientation(in, STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT);
00506         }
00507 
00512         template<class T>
00513         static inline T transform_frame_ned_enu(const T &in) {
00514                 return transform_static_frame(in, STATIC_TRANSFORM::NED_TO_ENU);
00515         }
00516 
00521         template<class T>
00522         static inline T transform_frame_enu_ned(const T &in) {
00523                 return transform_static_frame(in, STATIC_TRANSFORM::ENU_TO_NED);
00524         }
00525 
00530         template<class T>
00531         static inline T transform_frame_aircraft_baselink(const T &in) {
00532                 return transform_static_frame(in, STATIC_TRANSFORM::AIRCRAFT_TO_BASELINK);
00533         }
00534 
00539         template<class T>
00540         static inline T transform_frame_baselink_aircraft(const T &in) {
00541                 return transform_static_frame(in, STATIC_TRANSFORM::BASELINK_TO_AIRCRAFT);
00542         }
00543 
00548         template<class T>
00549         static inline T transform_frame_aircraft_ned(const T &in,const Eigen::Quaterniond &q) {
00550                 return transform_frame(in, q);
00551         }
00552 
00557         template<class T>
00558         static inline T transform_frame_ned_aircraft(const T &in,const Eigen::Quaterniond &q) {
00559                 return transform_frame(in, q);
00560         }
00561 
00566         template<class T>
00567         static inline T transform_frame_aircraft_enu(const T &in,const Eigen::Quaterniond &q) {
00568                 return transform_frame(in, q);
00569         }
00570 
00575         template<class T>
00576         static inline T transform_frame_enu_aircraft(const T &in,const Eigen::Quaterniond &q) {
00577                 return transform_frame(in, q);
00578         }
00579 
00584         template<class T>
00585         static inline T transform_frame_enu_baselink(const T &in,const Eigen::Quaterniond &q) {
00586                 return transform_frame(in, q);
00587         }
00588 
00593         template<class T>
00594         static inline T transform_frame_baselink_enu(const T &in,const Eigen::Quaterniond &q) {
00595                 return transform_frame(in, q);
00596         }
00597 
00601         static inline double transform_frame_yaw_enu_ned(double yaw) {
00602                 return transform_frame_yaw(yaw);
00603         }
00604 
00608         static inline double transform_frame_yaw_ned_enu(double yaw) {
00609                 return transform_frame_yaw(yaw);
00610         }
00611 
00612 private:
00613         std::recursive_mutex mutex;
00614 
00615         std::atomic<uint8_t> type;
00616         std::atomic<uint8_t> autopilot;
00617         std::atomic<uint8_t> base_mode;
00618 
00619         uint8_t target_system;
00620         uint8_t target_component;
00621 
00622         std::atomic<bool> connected;
00623 
00624         sensor_msgs::Imu::Ptr imu_data;
00625 
00626         sensor_msgs::NavSatFix::Ptr gps_fix;
00627         float gps_eph;
00628         float gps_epv;
00629         int gps_fix_type;
00630         int gps_satellites_visible;
00631 
00632         std::atomic<uint64_t> time_offset;
00633 
00634         std::atomic<bool> fcu_caps_known;
00635         std::atomic<uint64_t> fcu_capabilities;
00636 
00637         static inline double transform_frame_yaw(double yaw) {
00638                 return -yaw;
00639         }
00640 };
00641 };      // namespace mavros


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