mavros_plugin.h

Go to the documentation of this file.

/*
 * Copyright 2013,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 <tuple>
#include <vector>
#include <functional>
#include <diagnostic_updater/diagnostic_updater.h>
#include <mavconn/interface.h>
#include <mavros/mavros_uas.h>

namespace mavros {
namespace plugin {
using mavros::UAS;
typedef std::lock_guard<std::recursive_mutex> lock_guard;
typedef std::unique_lock<std::recursive_mutex> unique_lock;

class PluginBase
{
private:
        PluginBase(const PluginBase&) = delete;

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

        // pluginlib return boost::shared_ptr
        using Ptr = boost::shared_ptr<PluginBase>;
        using ConstPtr = boost::shared_ptr<PluginBase const>;

        virtual ~PluginBase() {};

        virtual void initialize(UAS &uas) {
                m_uas = &uas;
        }

        virtual Subscriptions get_subscriptions() = 0;

protected:
        PluginBase() : m_uas(nullptr) {};

        UAS *m_uas;

        // TODO: filtered handlers

        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, static_cast<_C*>(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>
        HandlerInfo make_handler(void (_C::*fn)(const mavlink::mavlink_message_t*, _T &)) {
                auto bfn = std::bind(fn, static_cast<_C*>(this), std::placeholders::_1, std::placeholders::_2);
                const auto id = _T::MSG_ID;
                const auto name = _T::NAME;
                const auto type_hash_ = typeid(_T).hash_code();

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

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

                                       bfn(msg, obj);
                               }
                };
        }

        virtual void connection_cb(bool connected) {
                ROS_BREAK();
        }

        inline void enable_connection_cb() {
                m_uas->add_connection_change_handler(std::bind(&PluginBase::connection_cb, this, std::placeholders::_1));
        }

        virtual void capabilities_cb(UAS::MAV_CAP capabilities) {
                ROS_BREAK();
        }

        void enable_capabilities_cb() {
                m_uas->add_capabilities_change_handler(std::bind(&PluginBase::capabilities_cb, this, std::placeholders::_1));
        }
};
}       // namespace plugin
}       // namespace mavros