00001
00010
00011
00012
00013
00014
00015
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
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
00176
00180 inline uint8_t get_tgt_system() {
00181 return target_system;
00182 }
00183
00187 inline uint8_t get_tgt_component() {
00188 return target_component;
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
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
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
00233
00234 tf2_ros::Buffer tf2_buffer;
00235 tf2_ros::TransformListener tf2_listener;
00236 tf2_ros::TransformBroadcaster tf2_broadcaster;
00237
00238
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
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
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
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
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
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 };