mavros.cpp
Go to the documentation of this file.
00001 
00006 /*
00007  * Copyright 2013,2014,2015,2016 Vladimir Ermakov.
00008  *
00009  * This file is part of the mavros package and subject to the license terms
00010  * in the top-level LICENSE file of the mavros repository.
00011  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
00012  */
00013 
00014 #include <mavros/mavros.h>
00015 #include <ros/console.h>
00016 #include <mavros/utils.h>
00017 #include <fnmatch.h>
00018 
00019 #ifdef MAVLINK_VERSION
00020 #undef MAVLINK_VERSION
00021 #endif
00022 #include <mavlink/config.h>
00023 
00024 using namespace mavros;
00025 using namespace mavconn;
00026 using namespace mavplugin;
00027 
00028 
00029 MavRos::MavRos() :
00030         mavlink_nh("mavlink"),          // allow to namespace it
00031         fcu_link_diag("FCU connection"),
00032         gcs_link_diag("GCS bridge"),
00033         plugin_loader("mavros", "mavplugin::MavRosPlugin"),
00034         message_route_table {}
00035 {
00036         std::string fcu_url, gcs_url;
00037         int system_id, component_id;
00038         int tgt_system_id, tgt_component_id;
00039         bool px4_usb_quirk;
00040         ros::V_string plugin_blacklist{}, plugin_whitelist{};
00041         MAVConnInterface::Ptr fcu_link;
00042 
00043         ros::NodeHandle nh("~");
00044 
00045         nh.param<std::string>("fcu_url", fcu_url, "serial:///dev/ttyACM0");
00046         nh.param<std::string>("gcs_url", gcs_url, "udp://@");
00047         nh.param("system_id", system_id, 1);
00048         nh.param<int>("component_id", component_id, MAV_COMP_ID_UDP_BRIDGE);
00049         nh.param("target_system_id", tgt_system_id, 1);
00050         nh.param("target_component_id", tgt_component_id, 1);
00051         nh.param("startup_px4_usb_quirk", px4_usb_quirk, false);
00052         nh.getParam("plugin_blacklist", plugin_blacklist);
00053         nh.getParam("plugin_whitelist", plugin_whitelist);
00054 
00055         // Now we use FCU URL as a hardware Id
00056         UAS_DIAG(&mav_uas).setHardwareID(fcu_url);
00057 
00058         ROS_INFO_STREAM("FCU URL: " << fcu_url);
00059         try {
00060                 fcu_link = MAVConnInterface::open_url(fcu_url, system_id, component_id);
00061                 // may be overridden by URL
00062                 system_id = fcu_link->get_system_id();
00063                 component_id = fcu_link->get_component_id();
00064 
00065                 fcu_link_diag.set_mavconn(fcu_link);
00066                 UAS_DIAG(&mav_uas).add(fcu_link_diag);
00067         }
00068         catch (mavconn::DeviceError &ex) {
00069                 ROS_FATAL("FCU: %s", ex.what());
00070                 ros::shutdown();
00071                 return;
00072         }
00073 
00074         if (gcs_url != "") {
00075                 ROS_INFO_STREAM("GCS URL: " << gcs_url);
00076                 try {
00077                         gcs_link = MAVConnInterface::open_url(gcs_url, system_id, component_id);
00078 
00079                         gcs_link_diag.set_mavconn(gcs_link);
00080                         UAS_DIAG(&mav_uas).add(gcs_link_diag);
00081                 }
00082                 catch (mavconn::DeviceError &ex) {
00083                         ROS_FATAL("GCS: %s", ex.what());
00084                         ros::shutdown();
00085                         return;
00086                 }
00087         }
00088         else
00089                 ROS_INFO("GCS bridge disabled");
00090 
00091         // ROS mavlink bridge
00092         mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("from", 100);
00093         mavlink_sub = mavlink_nh.subscribe("to", 100, &MavRos::mavlink_sub_cb, this,
00094                 ros::TransportHints()
00095                         .unreliable().maxDatagramSize(1024)
00096                         .reliable());
00097 
00098         // setup UAS and diag
00099         mav_uas.set_tgt(tgt_system_id, tgt_component_id);
00100         UAS_FCU(&mav_uas) = fcu_link;
00101         mav_uas.sig_connection_changed.connect(boost::bind(&MavlinkDiag::set_connection_status, &fcu_link_diag, _1));
00102         mav_uas.sig_connection_changed.connect(boost::bind(&MavRos::log_connect_change, this, _1));
00103 
00104         // connect FCU link
00105         fcu_link->message_received.connect(boost::bind(&MavRos::mavlink_pub_cb, this, _1, _2, _3));
00106         fcu_link->message_received.connect(boost::bind(&MavRos::plugin_route_cb, this, _1, _2, _3));
00107         fcu_link->port_closed.connect(boost::bind(&MavRos::terminate_cb, this));
00108 
00109         if (gcs_link) {
00110                 // setup GCS link bridge
00111                 fcu_link->message_received.connect(
00112                         boost::bind(&MAVConnInterface::send_message, gcs_link, _1, _2, _3));
00113                 gcs_link->message_received.connect(
00114                         boost::bind(&MAVConnInterface::send_message, fcu_link, _1, _2, _3));
00115                 gcs_link_diag.set_connection_status(true);
00116         }
00117 
00118         // prepare plugin lists
00119         // issue #257 2: assume that all plugins blacklisted
00120         if (plugin_blacklist.empty() and !plugin_whitelist.empty())
00121                 plugin_blacklist.push_back("*");
00122 
00123         for (auto &name : plugin_loader.getDeclaredClasses())
00124                 add_plugin(name, plugin_blacklist, plugin_whitelist);
00125 
00126         if (px4_usb_quirk)
00127                 startup_px4_usb_quirk();
00128 
00129 #define STR2(x) #x
00130 #define STR(x)  STR2(x)
00131 
00132         ROS_INFO("Built-in SIMD instructions: %s", Eigen::SimdInstructionSetsInUse());
00133         ROS_INFO("Built-in MAVLink package version: %s", MAVLINK_VERSION);
00134         ROS_INFO("Built-in MAVLink dialect: %s", STR(MAVLINK_DIALECT));
00135         ROS_INFO("MAVROS started. MY ID %d.%d, TARGET ID %d.%d",
00136                 system_id, component_id,
00137                 tgt_system_id, tgt_component_id);
00138 }
00139 
00140 void MavRos::spin() {
00141         ros::AsyncSpinner spinner(4 /* threads */);
00142 
00143         auto diag_timer = mavlink_nh.createTimer(
00144                         ros::Duration(0.5),
00145                         [&](const ros::TimerEvent &) {
00146                                 UAS_DIAG(&mav_uas).update();
00147                         });
00148         diag_timer.start();
00149 
00150         spinner.start();
00151         ros::waitForShutdown();
00152 
00153         ROS_INFO("Stopping mavros...");
00154         mav_uas.stop();
00155         spinner.stop();
00156 }
00157 
00158 void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
00159         auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
00160 
00161         if  (mavlink_pub.getNumSubscribers() == 0)
00162                 return;
00163 
00164         rmsg->header.stamp = ros::Time::now();
00165         mavros_msgs::mavlink::convert(*mmsg, *rmsg);
00166         mavlink_pub.publish(rmsg);
00167 }
00168 
00169 void MavRos::mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) {
00170         mavlink_message_t mmsg;
00171 
00172         if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
00173                 UAS_FCU(&mav_uas)->send_message(&mmsg, rmsg->sysid, rmsg->compid);
00174         else
00175                 ROS_ERROR("Drop mavlink packet: illegal payload64 size");
00176 }
00177 
00178 void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
00179         message_route_table[mmsg->msgid](mmsg, sysid, compid);
00180 }
00181 
00182 static bool pattern_match(std::string &pattern, std::string &pl_name) {
00183         int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD);
00184         if (cmp == 0)
00185                 return true;
00186         else if (cmp != FNM_NOMATCH) {
00187                 // never see that, i think that it is fatal error.
00188                 ROS_FATAL("Plugin list check error! fnmatch('%s', '%s', FNM_CASEFOLD) -> %d",
00189                                 pattern.c_str(), pl_name.c_str(), cmp);
00190                 ros::shutdown();
00191         }
00192 
00193         return false;
00194 }
00195 
00208 bool MavRos::is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) {
00209         for (auto &bl_pattern : blacklist) {
00210                 if (pattern_match(bl_pattern, pl_name)) {
00211                         for (auto &wl_pattern : whitelist) {
00212                                 if (pattern_match(wl_pattern, pl_name))
00213                                         return false;
00214                         }
00215 
00216                         return true;
00217                 }
00218         }
00219 
00220         return false;
00221 }
00222 
00226 void MavRos::add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) {
00227         if (is_blacklisted(pl_name, blacklist, whitelist)) {
00228                 ROS_INFO_STREAM("Plugin " << pl_name << " blacklisted");
00229                 return;
00230         }
00231 
00232         try {
00233                 auto plugin = plugin_loader.createInstance(pl_name);
00234                 plugin->initialize(mav_uas);
00235                 loaded_plugins.push_back(plugin);
00236 
00237                 ROS_INFO_STREAM("Plugin " << pl_name << " loaded and initialized");
00238 
00239                 for (auto &pair : plugin->get_rx_handlers()) {
00240                         ROS_DEBUG_STREAM("Route msgid " << int(pair.first) << " to " << pl_name);
00241                         message_route_table[pair.first].connect(pair.second);
00242                 }
00243         } catch (pluginlib::PluginlibException &ex) {
00244                 ROS_ERROR_STREAM("Plugin " << pl_name << " load exception: " << ex.what());
00245         }
00246 }
00247 
00248 void MavRos::terminate_cb() {
00249         ROS_ERROR("FCU connection closed, mavros will be terminated.");
00250         ros::requestShutdown();
00251 }
00252 
00253 void MavRos::startup_px4_usb_quirk(void) {
00254         /* sample code from QGC */
00255         const uint8_t init[] = {0x0d, 0x0d, 0x0d, 0};
00256         const uint8_t nsh[] = "sh /etc/init.d/rc.usb\n";
00257 
00258         ROS_INFO("Autostarting mavlink via USB on PX4");
00259         UAS_FCU(&mav_uas)->send_bytes(init, 3);
00260         UAS_FCU(&mav_uas)->send_bytes(nsh, sizeof(nsh) - 1);
00261         UAS_FCU(&mav_uas)->send_bytes(init, 4); /* NOTE in original init[3] */
00262 }
00263 
00264 void MavRos::log_connect_change(bool connected) {
00265         auto ap = mav_uas.str_autopilot(mav_uas.get_autopilot());
00266 
00267         /* note: sys_status plugin required */
00268         if (connected)
00269                 ROS_INFO("CON: Got HEARTBEAT, connected. FCU: %s", ap.c_str());
00270         else
00271                 ROS_WARN("CON: Lost connection, HEARTBEAT timed out.");
00272 }
00273 


mavros
Author(s): Vladimir Ermakov
autogenerated on Sat Jun 8 2019 19:55:19