00001
00006
00007
00008
00009
00010
00011
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"),
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
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
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
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
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
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
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
00119
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 );
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
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
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);
00262 }
00263
00264 void MavRos::log_connect_change(bool connected) {
00265 auto ap = mav_uas.str_autopilot(mav_uas.get_autopilot());
00266
00267
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