Program Listing for File plugin.hpp

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

/*
 * Copyright 2013,2014,2015,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__PLUGIN_HPP_
#define MAVROS__PLUGIN_HPP_

#include <tuple>
#include <vector>
#include <functional>
#include <mutex>
#include <shared_mutex>     // NOLINT
#include <memory>
#include <string>
#include <unordered_map>

#include "mavconn/interface.hpp"
#include "mavros/mavros_uas.hpp"

namespace mavros
{
namespace uas
{
class UAS;
using MAV_CAP = mavlink::common::MAV_PROTOCOL_CAPABILITY;
}     // namespace uas

namespace plugin
{

using mavros::uas::UAS;
using UASPtr = std::shared_ptr<UAS>;
using r_unique_lock = std::unique_lock<std::recursive_mutex>;
using s_unique_lock = std::unique_lock<std::shared_timed_mutex>;
using s_shared_lock = std::shared_lock<std::shared_timed_mutex>;

class Filter
{
  virtual bool operator()(
    UASPtr uas, const mavlink::mavlink_message_t * cmsg,
    const mavconn::Framing framing) = 0;
};

class Plugin : public std::enable_shared_from_this<Plugin>
{
private:
  explicit Plugin(const Plugin &) = delete;

public:
  RCLCPP_SMART_PTR_DEFINITIONS(Plugin)


  using HandlerCb = mavconn::MAVConnInterface::ReceivedCb;
  using HandlerInfo = std::tuple<mavlink::msgid_t, const char *, size_t, HandlerCb>;
  using Subscriptions = std::vector<HandlerInfo>;

  explicit Plugin(UASPtr uas_)
  : uas(uas_), node(std::dynamic_pointer_cast<rclcpp::Node>(uas_))
  {}

  explicit Plugin(
    UASPtr uas_, const std::string & subnode,
    const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : uas(uas_),
    // node(std::dynamic_pointer_cast<rclcpp::Node>(uas_)->create_sub_node(subnode))  // https://github.com/ros2/rclcpp/issues/731
    node(rclcpp::Node::make_shared(subnode,
      std::dynamic_pointer_cast<rclcpp::Node>(uas_)->get_fully_qualified_name(), options))
  {}

  virtual ~Plugin() = default;

  virtual Subscriptions get_subscriptions() = 0;

  virtual rclcpp::Node::SharedPtr get_node() const
  {
    return node;
  }

  virtual rclcpp::Logger get_logger() const
  {
    return node->get_logger();
  }

  virtual rclcpp::Clock::SharedPtr get_clock() const
  {
    return node->get_clock();
  }

protected:
  UASPtr uas;                       // uas back link
  rclcpp::Node::SharedPtr node;     // most of plugins uses sub-node

  using SetParametersResult = rcl_interfaces::msg::SetParametersResult;
  using ParameterFunctorT = std::function<void (const rclcpp::Parameter & p)>;

  std::unordered_map<std::string, ParameterFunctorT> node_watch_parameters;
  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr node_set_parameters_handle_ptr;

  template<class _C>
  HandlerInfo make_handler(
    const mavlink::msgid_t id, void (_C::* fn)(
      const mavlink::mavlink_message_t * msg, const mavconn::Framing framing))
  {
    auto bfn = std::bind(
      fn, std::static_pointer_cast<_C>(shared_from_this()), std::placeholders::_1,
      std::placeholders::_2);
    const auto type_hash_ = typeid(mavlink::mavlink_message_t).hash_code();

    return HandlerInfo {id, nullptr, type_hash_, bfn};
  }

  template<class _C, class _T, class _F>
  HandlerInfo make_handler(void (_C::* fn)(const mavlink::mavlink_message_t *, _T &, _F))
  {
    static_assert(
      std::is_base_of<Filter, _F>::value,
      "Filter class should be derived from mavros::plugin::Filter");

    auto bfn = std::bind(
      fn, std::static_pointer_cast<_C>(shared_from_this()), std::placeholders::_1,
      std::placeholders::_2, std::placeholders::_3);
    const auto id = _T::MSG_ID;
    const auto name = _T::NAME;
    const auto type_hash_ = typeid(_T).hash_code();
    auto uas_ = this->uas;

    return HandlerInfo {
      id, name, type_hash_,
      [bfn, uas_](const mavlink::mavlink_message_t * msg, const mavconn::Framing framing) {
        auto filter = _F();
        if (!filter(uas_, msg, framing)) {
          return;
        }

        mavlink::MsgMap map(msg);
        _T obj;
        obj.deserialize(map);

        bfn(msg, obj, filter);
      }
    };
  }

  virtual void connection_cb(bool connected)
  {
    (void)connected;
    assert(false);
  }

  void enable_connection_cb();

  virtual void capabilities_cb(uas::MAV_CAP capabilities)
  {
    (void)capabilities;
    assert(false);
  }

  void enable_capabilities_cb();

  virtual SetParametersResult node_on_set_parameters_cb(
    const std::vector<rclcpp::Parameter> & parameters);

  void enable_node_watch_parameters();

  template<typename ParameterT>
  auto node_declare_and_watch_parameter(
    const std::string & name, ParameterFunctorT cb,
    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
    rcl_interfaces::msg::ParameterDescriptor(),
    bool ignore_override = false
  )
  {
    node_watch_parameters[name] = cb;

#ifdef USE_OLD_DECLARE_PARAMETER
    // NOTE(vooon): for Foxy:
    return node->declare_parameter(
      name,
      rclcpp::ParameterValue(), parameter_descriptor, ignore_override);
#else
    // NOTE(vooon): for Galactic:
    return node->declare_parameter<ParameterT>(name, parameter_descriptor, ignore_override);
#endif
  }

  template<typename ParameterT>
  auto node_declare_and_watch_parameter(
    const std::string & name, const ParameterT & default_value,
    ParameterFunctorT cb,
    const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor =
    rcl_interfaces::msg::ParameterDescriptor(),
    bool ignore_override = false
  )
  {
    node_watch_parameters[name] = cb;
    return node->declare_parameter(name, default_value, parameter_descriptor, ignore_override);
  }

  inline uint64_t get_time_usec(const builtin_interfaces::msg::Time & t)
  {
    return rclcpp::Time(t).nanoseconds() / 1000;
  }

  inline uint64_t get_time_usec()
  {
    return get_time_usec(node->now());
  }

  inline uint32_t get_time_boot_ms(const builtin_interfaces::msg::Time & t)
  {
    return rclcpp::Time(t).nanoseconds() / 1000000;
  }

  inline uint32_t get_time_boot_ms()
  {
    return get_time_boot_ms(node->now());
  }
};

class PluginFactory
{
public:
  PluginFactory() = default;
  virtual ~PluginFactory() = default;

  virtual Plugin::SharedPtr create_plugin_instance(UASPtr uas) = 0;
};

template<typename _T>
class PluginFactoryTemplate : public PluginFactory
{
public:
  PluginFactoryTemplate() = default;
  virtual ~PluginFactoryTemplate() = default;

  Plugin::SharedPtr create_plugin_instance(UASPtr uas) override
  {
    static_assert(
      std::is_base_of<Plugin, _T>::value,
      "Plugin should be derived from mavros::plugin::Plugin");
    return std::make_shared<_T>(uas);
  }
};

}   // namespace plugin
}   // namespace mavros

#endif  // MAVROS__PLUGIN_HPP_