mavros.cpp

Go to the documentation of this file.

 
/*
 * Copyright 2013,2014,2015,2016 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
 */
 
#include <ros/console.h>
#include <mavros/mavros.h>
#include <mavros/utils.h>
#include <fnmatch.h>
 
// MAVLINK_VERSION string
#include <mavlink/config.h>
#include <mavconn/mavlink_dialect.h>
 
#include <mavconn/udp.h>
#include <std_msgs/String.h>
 
using namespace mavros;
using mavconn::MAVConnInterface;
using mavconn::Framing;
using mavlink::mavlink_message_t;
using plugin::PluginBase;
using utils::enum_value;
 
 
MavRos::MavRos() :
        mavlink_nh("mavlink"),          // allow to namespace it
        last_message_received_from_gcs(0),
        fcu_link_diag("FCU connection"),
        gcs_link_diag("GCS bridge"),
        plugin_loader("mavros", "mavros::plugin::PluginBase"),
        plugin_subscriptions{}
{
        std::string fcu_url, gcs_url;
        std::string fcu_protocol;
        int system_id, component_id;
        int tgt_system_id, tgt_component_id;
        bool px4_usb_quirk;
        double conn_timeout_d;
        ros::V_string plugin_blacklist{}, plugin_whitelist{};
        MAVConnInterface::Ptr fcu_link;
 
        ros::NodeHandle nh("~");
 
        nh.param<std::string>("fcu_url", fcu_url, "serial:///dev/ttyACM0");
        nh.param<std::string>("gcs_url", gcs_url, "udp://@");
        nh.param<bool>("gcs_quiet_mode", gcs_quiet_mode, false);
        nh.param("conn/timeout", conn_timeout_d, 30.0);
 
        nh.param<std::string>("fcu_protocol", fcu_protocol, "v2.0");
        nh.param("system_id", system_id, 1);
        nh.param<int>("component_id", component_id, mavconn::MAV_COMP_ID_UDP_BRIDGE);
        nh.param("target_system_id", tgt_system_id, 1);
        nh.param("target_component_id", tgt_component_id, 1);
        nh.param("startup_px4_usb_quirk", px4_usb_quirk, false);
        nh.getParam("plugin_blacklist", plugin_blacklist);
        nh.getParam("plugin_whitelist", plugin_whitelist);
 
        // Get frame_id
        std::string base_link_frame_id, odom_frame_id, map_frame_id;
        if (nh.param<std::string>("base_link_frame_id", base_link_frame_id, "base_link"))
                ROS_INFO("Find param base_link_frame_id: %s", base_link_frame_id.c_str());
        if (nh.param<std::string>("odom_frame_id", odom_frame_id, "odom"))
                ROS_INFO("Find param odom_frame_id: %s", odom_frame_id.c_str());
        if (nh.param<std::string>("map_frame_id", map_frame_id, "map"))
                ROS_INFO("Find param map_frame_id: %s", map_frame_id.c_str());
        mav_uas.set_base_link_frame_id(base_link_frame_id);
        mav_uas.set_odom_frame_id(odom_frame_id);
        mav_uas.set_map_frame_id(map_frame_id);
        mav_uas.setup_static_tf();
 
        conn_timeout = ros::Duration(conn_timeout_d);
 
        // Now we use FCU URL as a hardware Id
        UAS_DIAG(&mav_uas).setHardwareID(fcu_url);
 
        ROS_INFO_STREAM("FCU URL: " << fcu_url);
        try {
                fcu_link = MAVConnInterface::open_url_no_connect(fcu_url, system_id, component_id);
                // may be overridden by URL
                system_id = fcu_link->get_system_id();
                component_id = fcu_link->get_component_id();
 
                fcu_link_diag.set_mavconn(fcu_link);
                UAS_DIAG(&mav_uas).add(fcu_link_diag);
        }
        catch (mavconn::DeviceError &ex) {
                ROS_FATAL("FCU: %s", ex.what());
                ros::shutdown();
                return;
        }
 
        if (fcu_protocol == "v1.0") {
                fcu_link->set_protocol_version(mavconn::Protocol::V10);
        }
        else if (fcu_protocol == "v2.0") {
                fcu_link->set_protocol_version(mavconn::Protocol::V20);
        }
        //else if (fcu_protocol == "auto") {    // XXX TODO
        //      fcu_link->set_protocol_version(mavconn::Protocol::V20);
        //}
        else {
                ROS_WARN("Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v1.0.", fcu_protocol.c_str());
                fcu_link->set_protocol_version(mavconn::Protocol::V10);
        }
 
        if (gcs_url != "") {
                ROS_INFO_STREAM("GCS URL: " << gcs_url);
                try {
                        gcs_link = MAVConnInterface::open_url_no_connect(gcs_url, system_id, component_id);
 
                        gcs_link_diag.set_mavconn(gcs_link);
                        gcs_diag_updater.setHardwareID(gcs_url);
                        gcs_diag_updater.add(gcs_link_diag);
                }
                catch (mavconn::DeviceError &ex) {
                        ROS_FATAL("GCS: %s", ex.what());
                        ros::shutdown();
                        return;
                }
        }
        else
                ROS_INFO("GCS bridge disabled");
 
        // ROS mavlink bridge
        mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("from", 100);
        mavlink_sub = mavlink_nh.subscribe("to", 100, &MavRos::mavlink_sub_cb, this,
                ros::TransportHints()
                        .unreliable().maxDatagramSize(1024)
                        .reliable());
 
        // setup UAS and diag
        mav_uas.set_tgt(tgt_system_id, tgt_component_id);
        UAS_FCU(&mav_uas) = fcu_link;
 
        mav_uas.add_connection_change_handler(std::bind(&MavlinkDiag::set_connection_status, &fcu_link_diag, std::placeholders::_1));
        mav_uas.add_connection_change_handler(std::bind(&MavRos::log_connect_change, this, std::placeholders::_1));
 
        // prepare plugin lists
        // issue #257 2: assume that all plugins blacklisted
        if (plugin_blacklist.empty() and !plugin_whitelist.empty())
                plugin_blacklist.emplace_back("*");
 
        for (auto &name : plugin_loader.getDeclaredClasses())
                add_plugin(name, plugin_blacklist, plugin_whitelist);
 
        // connect FCU link
 
        // XXX TODO: move workers to ROS Spinner, let mavconn threads to do only IO
        fcu_link->connect(
                [this](const mavlink_message_t *msg, const Framing framing) {
                        mavlink_pub_cb(msg, framing);
                        plugin_route_cb(msg, framing);
 
                        if (gcs_link) {
                                if (this->gcs_quiet_mode && msg->msgid != mavlink::minimal::msg::HEARTBEAT::MSG_ID &&
                                        (ros::Time::now() - this->last_message_received_from_gcs > this->conn_timeout)) {
                                        return;
                                }
 
                                gcs_link->send_message_ignore_drop(msg);
                        }
                },
                []() {
                        ROS_ERROR("FCU connection closed, mavros will be terminated.");
                        ros::requestShutdown();
                });
 
        if (gcs_link) {
                // setup GCS link bridge
                gcs_link->connect([this, fcu_link](const mavlink_message_t *msg, const Framing framing) {
                        this->last_message_received_from_gcs = ros::Time::now();
                        fcu_link->send_message_ignore_drop(msg);
                });
 
                gcs_link_diag.set_connection_status(true);
        }
 
        if (px4_usb_quirk)
                startup_px4_usb_quirk();
 
        std::stringstream ss;
        for (auto &s : mavconn::MAVConnInterface::get_known_dialects())
                ss << " " << s;
 
        ROS_INFO("Built-in SIMD instructions: %s", Eigen::SimdInstructionSetsInUse());
        ROS_INFO("Built-in MAVLink package version: %s", MAVLINK_VERSION);
        ROS_INFO("Known MAVLink dialects:%s", ss.str().c_str());
        ROS_INFO("MAVROS started. MY ID %u.%u, TARGET ID %u.%u",
                system_id, component_id,
                tgt_system_id, tgt_component_id);
}
 
void MavRos::spin()
{
        ros::AsyncSpinner spinner(4 /* threads */);
 
        auto diag_timer = mavlink_nh.createTimer(
                        ros::Duration(0.5),
                        [&](const ros::TimerEvent &) {
                                UAS_DIAG(&mav_uas).update();
 
                                if (gcs_link)
                                        gcs_diag_updater.update();
                        });
        diag_timer.start();
 
        auto remote_endpoint_timer = mavlink_nh.createTimer(
                ros::Duration(1.0), [&](const ros::TimerEvent&) {
                        static auto pub = mavlink_nh.advertise<std_msgs::String>("gcs_ip", 1, true /* latch */);
                        static auto finished = false;
 
                        if (finished)
                                return;
 
                        if (const auto* p = dynamic_cast<mavconn::MAVConnUDP*>(gcs_link.get())) {
                                const auto endpoint = p->get_remote_endpoint();
                                const auto endpoint_valid = endpoint.find("255.255.255.255") == std::string::npos;
 
                                if (endpoint_valid) {
                                        std_msgs::String msg;
                                        msg.data = endpoint.substr(0, endpoint.find(":"));
                                        pub.publish(msg);
                                        finished = true;
                                }
                        }
                });
        remote_endpoint_timer.start();
 
        spinner.start();
        ros::waitForShutdown();
 
        ROS_INFO("Stopping mavros...");
        spinner.stop();
}
 
void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, Framing framing)
{
        auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
 
        if  (mavlink_pub.getNumSubscribers() == 0)
                return;
 
        rmsg->header.stamp = ros::Time::now();
        mavros_msgs::mavlink::convert(*mmsg, *rmsg, enum_value(framing));
        mavlink_pub.publish(rmsg);
}
 
void MavRos::mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg)
{
        mavlink_message_t mmsg;
 
        if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
                UAS_FCU(&mav_uas)->send_message_ignore_drop(&mmsg);
        else
                ROS_ERROR("Drop mavlink packet: convert error.");
}
 
void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, const Framing framing)
{
        auto it = plugin_subscriptions.find(mmsg->msgid);
        if (it == plugin_subscriptions.end())
                return;
 
        for (auto &info : it->second)
                std::get<3>(info)(mmsg, framing);
}
 
static bool pattern_match(std::string &pattern, std::string &pl_name)
{
        int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD);
        if (cmp == 0)
                return true;
        else if (cmp != FNM_NOMATCH) {
                // never see that, i think that it is fatal error.
                ROS_FATAL("Plugin list check error! fnmatch('%s', '%s', FNM_CASEFOLD) -> %d",
                                pattern.c_str(), pl_name.c_str(), cmp);
                ros::shutdown();
        }
 
        return false;
}
 
static bool is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
{
        for (auto &bl_pattern : blacklist) {
                if (pattern_match(bl_pattern, pl_name)) {
                        for (auto &wl_pattern : whitelist) {
                                if (pattern_match(wl_pattern, pl_name))
                                        return false;
                        }
 
                        return true;
                }
        }
 
        return false;
}
 
inline bool is_mavlink_message_t(const size_t rt)
{
        static const auto h = typeid(mavlink_message_t).hash_code();
        return h == rt;
}
 
void MavRos::add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
{
        if (is_blacklisted(pl_name, blacklist, whitelist)) {
                ROS_INFO_STREAM("Plugin " << pl_name << " blacklisted");
                return;
        }
 
        try {
                auto plugin = plugin_loader.createInstance(pl_name);
 
                ROS_INFO_STREAM("Plugin " << pl_name << " loaded");
 
                for (auto &info : plugin->get_subscriptions()) {
                        auto msgid = std::get<0>(info);
                        auto msgname = std::get<1>(info);
                        auto type_hash_ = std::get<2>(info);
 
                        std::string log_msgname;
 
                        if (is_mavlink_message_t(type_hash_))
                                log_msgname = utils::format("MSG-ID (%u) <%zu>", msgid, type_hash_);
                        else
                                log_msgname = utils::format("%s (%u) <%zu>", msgname, msgid, type_hash_);
 
                        ROS_DEBUG_STREAM("Route " << log_msgname << " to " << pl_name);
 
                        auto it = plugin_subscriptions.find(msgid);
                        if (it == plugin_subscriptions.end()) {
                                // new entry
 
                                ROS_DEBUG_STREAM(log_msgname << " - new element");
                                plugin_subscriptions[msgid] = PluginBase::Subscriptions{{info}};
                        }
                        else {
                                // existing: check handler message type
 
                                bool append_allowed = is_mavlink_message_t(type_hash_);
                                if (!append_allowed) {
                                        append_allowed = true;
                                        for (auto &e : it->second) {
                                                auto t2 = std::get<2>(e);
                                                if (!is_mavlink_message_t(t2) && t2 != type_hash_) {
                                                        ROS_ERROR_STREAM(log_msgname << " routed to different message type (hash: " << t2 << ")");
                                                        append_allowed = false;
                                                }
                                        }
                                }
 
                                if (append_allowed) {
                                        ROS_DEBUG_STREAM(log_msgname << " - emplace");
                                        it->second.emplace_back(info);
                                }
                                else
                                        ROS_ERROR_STREAM(log_msgname << " handler dropped because this ID are used for another message type");
                        }
                }
 
                plugin->initialize(mav_uas);
                loaded_plugins.push_back(plugin);
 
                ROS_INFO_STREAM("Plugin " << pl_name << " initialized");
        } catch (pluginlib::PluginlibException &ex) {
                ROS_ERROR_STREAM("Plugin " << pl_name << " load exception: " << ex.what());
        }
}
 
void MavRos::startup_px4_usb_quirk()
{
       /* sample code from QGC */
       const uint8_t init[] = {0x0d, 0x0d, 0x0d, 0};
       const uint8_t nsh[] = "sh /etc/init.d/rc.usb\n";
 
       ROS_INFO("Autostarting mavlink via USB on PX4");
       UAS_FCU(&mav_uas)->send_bytes(init, 3);
       UAS_FCU(&mav_uas)->send_bytes(nsh, sizeof(nsh) - 1);
       UAS_FCU(&mav_uas)->send_bytes(init, 4); /* NOTE in original init[3] */
}
 
void MavRos::log_connect_change(bool connected)
{
        auto ap = utils::to_string(mav_uas.get_autopilot());
 
        /* note: sys_status plugin required */
        if (connected)
                ROS_INFO("CON: Got HEARTBEAT, connected. FCU: %s", ap.c_str());
        else
                ROS_WARN("CON: Lost connection, HEARTBEAT timed out.");
}