Program Listing for File mavros_uas.hpp

↰ Return to documentation for file (/tmp/ws/src/mavros/mavros/include/mavros/mavros_uas.hpp)

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

#ifndef MAVROS__MAVROS_UAS_HPP_
#define MAVROS__MAVROS_UAS_HPP_

#include <tf2_ros/buffer.h>                         // NOLINT
#include <tf2_ros/transform_listener.h>             // NOLINT
#include <tf2_ros/transform_broadcaster.h>          // NOLINT
#include <tf2_ros/static_transform_broadcaster.h>   // NOLINT

#include <array>
#include <atomic>
#include <memory>
#include <type_traits>
#include <string>
#include <vector>
#include <unordered_map>

#include "diagnostic_updater/diagnostic_updater.hpp"
#include "mavconn/interface.hpp"
#include "pluginlib/class_loader.hpp"
#include "rclcpp/rclcpp.hpp"
#include "GeographicLib/Geoid.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

#include "mavros/utils.hpp"
#include "mavros/plugin.hpp"
#include "mavros/frame_tf.hpp"
#include "mavros/uas_executor.hpp"

namespace mavros
{
namespace uas
{

static constexpr auto MAV_COMP_ID_ONBOARD_COMPUTER = 191;

using s_unique_lock = std::unique_lock<std::shared_timed_mutex>;
using s_shared_lock = std::shared_lock<std::shared_timed_mutex>;

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


class Data
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Data();
  ~Data() = default;

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

  void update_attitude_imu_enu(const sensor_msgs::msg::Imu & imu);

  void update_attitude_imu_ned(const sensor_msgs::msg::Imu & imu);

  sensor_msgs::msg::Imu get_attitude_imu_enu();

  sensor_msgs::msg::Imu get_attitude_imu_ned();

  geometry_msgs::msg::Quaternion get_attitude_orientation_enu();

  geometry_msgs::msg::Quaternion get_attitude_orientation_ned();

  geometry_msgs::msg::Vector3 get_attitude_angular_velocity_enu();

  geometry_msgs::msg::Vector3 get_attitude_angular_velocity_ned();


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

  void update_gps_fix_epts(
    const sensor_msgs::msg::NavSatFix & 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::msg::NavSatFix get_gps_fix();

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

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

  template<class T, std::enable_if_t<std::is_pointer<T>::value, bool> = true>
  inline double geoid_to_ellipsoid_height(const T lla)
  {
    if (egm96_5) {
      return GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(lla->latitude, lla->longitude);
    } else {
      return 0.0;
    }
  }

  template<class T, std::enable_if_t<std::is_class<T>::value, bool> = true>
  inline double geoid_to_ellipsoid_height(const T & lla)
  {
    return geoid_to_ellipsoid_height(&lla);
  }

  template<class T, std::enable_if_t<std::is_pointer<T>::value, bool> = true>
  inline double ellipsoid_to_geoid_height(const T lla)
  {
    if (egm96_5) {
      return GeographicLib::Geoid::ELLIPSOIDTOGEOID * (*egm96_5)(lla->latitude, lla->longitude);
    } else {
      return 0.0;
    }
  }

  template<class T, std::enable_if_t<std::is_class<T>::value, bool> = true>
  inline double ellipsoid_to_geoid_height(const T & lla)
  {
    return ellipsoid_to_geoid_height(&lla);
  }

private:
  std::shared_timed_mutex mu;

  sensor_msgs::msg::Imu imu_enu_data;
  sensor_msgs::msg::Imu imu_ned_data;

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

  static std::once_flag init_flag;

  static void init_geographiclib();
};

class UAS : public rclcpp::Node
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS(UAS)
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

  using StrV = std::vector<std::string>;

  explicit UAS(const std::string & name_ = "mavros")
  : UAS(rclcpp::NodeOptions(), name_) {}

  explicit UAS(
    const rclcpp::NodeOptions & options_ = rclcpp::NodeOptions(),
    const std::string & name_ = "mavros",
    const std::string & uas_url_ = "/uas1", uint8_t target_system_ = 1,
    uint8_t target_component_ = 1);

  ~UAS() = default;

  diagnostic_updater::Updater diagnostic_updater;

  Data data;

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

  inline uint8_t get_tgt_component()
  {
    return target_component;
  }

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

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

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

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

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

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

  inline void send_message(const mavlink::Message & msg)
  {
    send_message(msg, source_component);
  }

  void send_message(const mavlink::Message & msg, const uint8_t src_compid);

  void set_protocol_version(mavconn::Protocol ver);

private:
  friend class TestUAS;

  // params
  uint8_t source_system;
  uint8_t source_component;
  uint8_t target_system;
  uint8_t target_component;

  std::string uas_url;

  std::string base_link_frame_id;
  std::string odom_frame_id;
  std::string map_frame_id;

  StrV plugin_allowlist;
  StrV plugin_denylist;

  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr set_parameters_handle_ptr;
  rclcpp::TimerBase::SharedPtr startup_delay_timer;

  // XXX(vooon): we have to use own executor because Node::create_sub_node() doesn't work for us.
  using thread_ptr = std::unique_ptr<std::thread, std::function<void (std::thread *)>>;
  thread_ptr exec_spin_thd;
  // rclcpp::executors::MultiThreadedExecutor exec;
  UASExecutor exec;

  // plugins
  pluginlib::ClassLoader<plugin::PluginFactory> plugin_factory_loader;
  std::vector<plugin::Plugin::SharedPtr> loaded_plugins;

  std::unordered_map<mavlink::msgid_t, plugin::Plugin::Subscriptions> plugin_subscriptions;

  std::shared_timed_mutex mu;

  // essential data
  std::atomic<uint8_t> type;
  std::atomic<uint8_t> autopilot;
  std::atomic<uint8_t> base_mode;
  std::atomic<bool> fcu_caps_known;
  std::atomic<uint64_t> fcu_capabilities;

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

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

  // UAS -> Router connection
  mavlink::mavlink_status_t mavlink_status;
  rclcpp::Subscription<mavros_msgs::msg::Mavlink>::SharedPtr source;    // FCU -> UAS
  rclcpp::Publisher<mavros_msgs::msg::Mavlink>::SharedPtr sink;         // UAS -> FCU

  void connect_to_router();

  void recv_message(const mavros_msgs::msg::Mavlink::SharedPtr rmsg);

  void plugin_route(const mavlink::mavlink_message_t * mmsg, const mavconn::Framing framing);

  bool is_plugin_allowed(const std::string & pl_name);

  virtual plugin::Plugin::SharedPtr create_plugin_instance(const std::string & pl_name);

  void add_plugin(const std::string & pl_name);

  rcl_interfaces::msg::SetParametersResult on_set_parameters_cb(
    const std::vector<rclcpp::Parameter> & parameters);

  void log_connect_change(bool connected);

  void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat);
};
}              // namespace uas
}       // namespace mavros

#endif  // MAVROS__MAVROS_UAS_HPP_