00001 00010 /* 00011 * Copyright 2014 Vladimir Ermakov. 00012 * 00013 * This program is free software; you can redistribute it and/or modify 00014 * it under the terms of the GNU General Public License as published by 00015 * the Free Software Foundation; either version 3 of the License, or 00016 * (at your option) any later version. 00017 * 00018 * This program is distributed in the hope that it will be useful, but 00019 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 00020 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 00021 * for more details. 00022 * 00023 * You should have received a copy of the GNU General Public License along 00024 * with this program; if not, write to the Free Software Foundation, Inc., 00025 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00026 */ 00027 00028 #pragma once 00029 00030 #include <array> 00031 #include <ros/ros.h> 00032 #include <pluginlib/class_loader.h> 00033 #include <mavconn/interface.h> 00034 #include <mavros/mavros_plugin.h> 00035 #include <mavros/mavlink_diag.h> 00036 00037 #include <mavros/Mavlink.h> 00038 00039 namespace mavros { 00040 00046 class MavRos 00047 { 00048 public: 00049 explicit MavRos(const ros::NodeHandle &nh_); 00050 ~MavRos() {}; 00051 00052 void spin(); 00053 00054 private: 00055 ros::NodeHandle node_handle; 00056 ros::NodeHandle mavlink_node_handle; 00057 // fcu_link stored in mav_uas 00058 mavconn::MAVConnInterface::Ptr gcs_link; 00059 00060 ros::Publisher mavlink_pub; 00061 ros::Subscriber mavlink_sub; 00062 00063 diagnostic_updater::Updater diag_updater; 00064 MavlinkDiag fcu_link_diag; 00065 MavlinkDiag gcs_link_diag; 00066 00067 pluginlib::ClassLoader<mavplugin::MavRosPlugin> plugin_loader; 00068 std::vector<mavplugin::MavRosPlugin::Ptr> loaded_plugins; 00069 std::vector<std::string> plugin_blacklist; 00070 std::array<mavconn::MAVConnInterface::MessageSig, 256> 00071 message_route_table; // link interface -> router -> plugin callback 00072 UAS mav_uas; 00073 00074 void mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid); 00075 void mavlink_sub_cb(const Mavlink::ConstPtr &rmsg); 00076 void plugin_route_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid); 00077 bool check_in_blacklist(std::string &pl_name); 00078 void add_plugin(std::string &pl_name); 00079 void terminate_cb(); 00080 void startup_px4_usb_quirk(void); 00081 void log_connect_change(bool connected); 00082 }; 00083 00084 }; // namespace mavros 00085