mavros.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2013,2014,2015,2016 Vladimir Ermakov.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <ros/console.h>
15 #include <mavros/mavros.h>
16 #include <mavros/utils.h>
17 #include <fnmatch.h>
18 
19 // MAVLINK_VERSION string
20 #include <mavlink/config.h>
21 #include <mavconn/mavlink_dialect.h>
22 
23 #include <mavconn/udp.h>
24 #include <std_msgs/String.h>
25 
26 using namespace mavros;
28 using mavconn::Framing;
29 using mavlink::mavlink_message_t;
30 using plugin::PluginBase;
31 using utils::enum_value;
32 
33 
35  mavlink_nh("mavlink"), // allow to namespace it
36  fcu_link_diag("FCU connection"),
37  gcs_link_diag("GCS bridge"),
38  plugin_loader("mavros", "mavros::plugin::PluginBase"),
39  last_message_received_from_gcs(0),
40  plugin_subscriptions{}
41 {
42  std::string fcu_url, gcs_url;
43  std::string fcu_protocol;
44  int system_id, component_id;
45  int tgt_system_id, tgt_component_id;
46  bool px4_usb_quirk;
47  double conn_timeout_d;
48  ros::V_string plugin_blacklist{}, plugin_whitelist{};
49  MAVConnInterface::Ptr fcu_link;
50 
51  ros::NodeHandle nh("~");
52 
53  nh.param<std::string>("fcu_url", fcu_url, "serial:///dev/ttyACM0");
54  nh.param<std::string>("gcs_url", gcs_url, "udp://@");
55  nh.param<bool>("gcs_quiet_mode", gcs_quiet_mode, false);
56  nh.param("conn/timeout", conn_timeout_d, 30.0);
57 
58  nh.param<std::string>("fcu_protocol", fcu_protocol, "v2.0");
59  nh.param("system_id", system_id, 1);
60  nh.param<int>("component_id", component_id, mavconn::MAV_COMP_ID_UDP_BRIDGE);
61  nh.param("target_system_id", tgt_system_id, 1);
62  nh.param("target_component_id", tgt_component_id, 1);
63  nh.param("startup_px4_usb_quirk", px4_usb_quirk, false);
64  nh.getParam("plugin_blacklist", plugin_blacklist);
65  nh.getParam("plugin_whitelist", plugin_whitelist);
66 
67  conn_timeout = ros::Duration(conn_timeout_d);
68 
69  // Now we use FCU URL as a hardware Id
70  UAS_DIAG(&mav_uas).setHardwareID(fcu_url);
71 
72  ROS_INFO_STREAM("FCU URL: " << fcu_url);
73  try {
74  fcu_link = MAVConnInterface::open_url(fcu_url, system_id, component_id);
75  // may be overridden by URL
76  system_id = fcu_link->get_system_id();
77  component_id = fcu_link->get_component_id();
78 
79  fcu_link_diag.set_mavconn(fcu_link);
81  }
82  catch (mavconn::DeviceError &ex) {
83  ROS_FATAL("FCU: %s", ex.what());
84  ros::shutdown();
85  return;
86  }
87 
88  if (fcu_protocol == "v1.0") {
89  fcu_link->set_protocol_version(mavconn::Protocol::V10);
90  }
91  else if (fcu_protocol == "v2.0") {
92  fcu_link->set_protocol_version(mavconn::Protocol::V20);
93  }
94  //else if (fcu_protocol == "auto") { // XXX TODO
95  // fcu_link->set_protocol_version(mavconn::Protocol::V20);
96  //}
97  else {
98  ROS_WARN("Unknown FCU protocol: \"%s\", should be: \"v1.0\" or \"v2.0\". Used default v1.0.", fcu_protocol.c_str());
99  fcu_link->set_protocol_version(mavconn::Protocol::V10);
100  }
101 
102  if (gcs_url != "") {
103  ROS_INFO_STREAM("GCS URL: " << gcs_url);
104  try {
105  gcs_link = MAVConnInterface::open_url(gcs_url, system_id, component_id);
106 
110  }
111  catch (mavconn::DeviceError &ex) {
112  ROS_FATAL("GCS: %s", ex.what());
113  ros::shutdown();
114  return;
115  }
116  }
117  else
118  ROS_INFO("GCS bridge disabled");
119 
120  // ROS mavlink bridge
121  mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("from", 100);
124  .unreliable().maxDatagramSize(1024)
125  .reliable());
126 
127  // setup UAS and diag
128  mav_uas.set_tgt(tgt_system_id, tgt_component_id);
129  UAS_FCU(&mav_uas) = fcu_link;
130 
132  mav_uas.add_connection_change_handler(std::bind(&MavRos::log_connect_change, this, std::placeholders::_1));
133 
134  // prepare plugin lists
135  // issue #257 2: assume that all plugins blacklisted
136  if (plugin_blacklist.empty() and !plugin_whitelist.empty())
137  plugin_blacklist.emplace_back("*");
138 
139  for (auto &name : plugin_loader.getDeclaredClasses())
140  add_plugin(name, plugin_blacklist, plugin_whitelist);
141 
142  // connect FCU link
143 
144  // XXX TODO: move workers to ROS Spinner, let mavconn threads to do only IO
145  fcu_link->message_received_cb = [this](const mavlink_message_t *msg, const Framing framing) {
146  mavlink_pub_cb(msg, framing);
147  plugin_route_cb(msg, framing);
148 
149  if (gcs_link) {
150  if (this->gcs_quiet_mode && msg->msgid != mavlink::minimal::msg::HEARTBEAT::MSG_ID &&
151  (ros::Time::now() - this->last_message_received_from_gcs > this->conn_timeout)) {
152  return;
153  }
154 
155  gcs_link->send_message_ignore_drop(msg);
156  }
157  };
158 
159  fcu_link->port_closed_cb = []() {
160  ROS_ERROR("FCU connection closed, mavros will be terminated.");
162  };
163 
164  if (gcs_link) {
165  // setup GCS link bridge
166  gcs_link->message_received_cb = [this, fcu_link](const mavlink_message_t *msg, const Framing framing) {
168  fcu_link->send_message_ignore_drop(msg);
169  };
170 
172  }
173 
174  if (px4_usb_quirk)
176 
177  std::stringstream ss;
179  ss << " " << s;
180 
181  ROS_INFO("Built-in SIMD instructions: %s", Eigen::SimdInstructionSetsInUse());
182  ROS_INFO("Built-in MAVLink package version: %s", MAVLINK_VERSION);
183  ROS_INFO("Known MAVLink dialects:%s", ss.str().c_str());
184  ROS_INFO("MAVROS started. MY ID %u.%u, TARGET ID %u.%u",
185  system_id, component_id,
186  tgt_system_id, tgt_component_id);
187 }
188 
190 {
191  ros::AsyncSpinner spinner(4 /* threads */);
192 
193  auto diag_timer = mavlink_nh.createTimer(
194  ros::Duration(0.5),
195  [&](const ros::TimerEvent &) {
196  UAS_DIAG(&mav_uas).update();
197 
198  if (gcs_link)
200  });
201  diag_timer.start();
202 
203  auto remote_endpoint_timer = mavlink_nh.createTimer(
204  ros::Duration(1.0), [&](const ros::TimerEvent&) {
205  static auto pub = mavlink_nh.advertise<std_msgs::String>("gcs_ip", 1, true /* latch */);
206  static auto finished = false;
207 
208  if (finished)
209  return;
210 
211  if (const auto* p = dynamic_cast<mavconn::MAVConnUDP*>(gcs_link.get())) {
212  const auto endpoint = p->get_remote_endpoint();
213  const auto endpoint_valid = endpoint.find("255.255.255.255") == std::string::npos;
214 
215  if (endpoint_valid) {
216  std_msgs::String msg;
217  msg.data = endpoint.substr(0, endpoint.find(":"));
218  pub.publish(msg);
219  finished = true;
220  }
221  }
222  });
223  remote_endpoint_timer.start();
224 
225  spinner.start();
227 
228  ROS_INFO("Stopping mavros...");
229  spinner.stop();
230 }
231 
232 void MavRos::mavlink_pub_cb(const mavlink_message_t *mmsg, Framing framing)
233 {
234  auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
235 
236  if (mavlink_pub.getNumSubscribers() == 0)
237  return;
238 
239  rmsg->header.stamp = ros::Time::now();
240  mavros_msgs::mavlink::convert(*mmsg, *rmsg, enum_value(framing));
241  mavlink_pub.publish(rmsg);
242 }
243 
245 {
246  mavlink_message_t mmsg;
247 
248  if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
249  UAS_FCU(&mav_uas)->send_message_ignore_drop(&mmsg);
250  else
251  ROS_ERROR("Drop mavlink packet: convert error.");
252 }
253 
254 void MavRos::plugin_route_cb(const mavlink_message_t *mmsg, const Framing framing)
255 {
256  auto it = plugin_subscriptions.find(mmsg->msgid);
257  if (it == plugin_subscriptions.end())
258  return;
259 
260  for (auto &info : it->second)
261  std::get<3>(info)(mmsg, framing);
262 }
263 
264 static bool pattern_match(std::string &pattern, std::string &pl_name)
265 {
266  int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD);
267  if (cmp == 0)
268  return true;
269  else if (cmp != FNM_NOMATCH) {
270  // never see that, i think that it is fatal error.
271  ROS_FATAL("Plugin list check error! fnmatch('%s', '%s', FNM_CASEFOLD) -> %d",
272  pattern.c_str(), pl_name.c_str(), cmp);
273  ros::shutdown();
274  }
275 
276  return false;
277 }
278 
291 static bool is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
292 {
293  for (auto &bl_pattern : blacklist) {
294  if (pattern_match(bl_pattern, pl_name)) {
295  for (auto &wl_pattern : whitelist) {
296  if (pattern_match(wl_pattern, pl_name))
297  return false;
298  }
299 
300  return true;
301  }
302  }
303 
304  return false;
305 }
306 
307 inline bool is_mavlink_message_t(const size_t rt)
308 {
309  static const auto h = typeid(mavlink_message_t).hash_code();
310  return h == rt;
311 }
312 
316 void MavRos::add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
317 {
318  if (is_blacklisted(pl_name, blacklist, whitelist)) {
319  ROS_INFO_STREAM("Plugin " << pl_name << " blacklisted");
320  return;
321  }
322 
323  try {
324  auto plugin = plugin_loader.createInstance(pl_name);
325 
326  ROS_INFO_STREAM("Plugin " << pl_name << " loaded");
327 
328  for (auto &info : plugin->get_subscriptions()) {
329  auto msgid = std::get<0>(info);
330  auto msgname = std::get<1>(info);
331  auto type_hash_ = std::get<2>(info);
332 
333  std::string log_msgname;
334 
335  if (is_mavlink_message_t(type_hash_))
336  log_msgname = utils::format("MSG-ID (%u) <%zu>", msgid, type_hash_);
337  else
338  log_msgname = utils::format("%s (%u) <%zu>", msgname, msgid, type_hash_);
339 
340  ROS_DEBUG_STREAM("Route " << log_msgname << " to " << pl_name);
341 
342  auto it = plugin_subscriptions.find(msgid);
343  if (it == plugin_subscriptions.end()) {
344  // new entry
345 
346  ROS_DEBUG_STREAM(log_msgname << " - new element");
347  plugin_subscriptions[msgid] = PluginBase::Subscriptions{{info}};
348  }
349  else {
350  // existing: check handler message type
351 
352  bool append_allowed = is_mavlink_message_t(type_hash_);
353  if (!append_allowed) {
354  append_allowed = true;
355  for (auto &e : it->second) {
356  auto t2 = std::get<2>(e);
357  if (!is_mavlink_message_t(t2) && t2 != type_hash_) {
358  ROS_ERROR_STREAM(log_msgname << " routed to different message type (hash: " << t2 << ")");
359  append_allowed = false;
360  }
361  }
362  }
363 
364  if (append_allowed) {
365  ROS_DEBUG_STREAM(log_msgname << " - emplace");
366  it->second.emplace_back(info);
367  }
368  else
369  ROS_ERROR_STREAM(log_msgname << " handler dropped because this ID are used for another message type");
370  }
371  }
372 
373  plugin->initialize(mav_uas);
374  loaded_plugins.push_back(plugin);
375 
376  ROS_INFO_STREAM("Plugin " << pl_name << " initialized");
377  } catch (pluginlib::PluginlibException &ex) {
378  ROS_ERROR_STREAM("Plugin " << pl_name << " load exception: " << ex.what());
379  }
380 }
381 
383 {
384  /* sample code from QGC */
385  const uint8_t init[] = {0x0d, 0x0d, 0x0d, 0};
386  const uint8_t nsh[] = "sh /etc/init.d/rc.usb\n";
387 
388  ROS_INFO("Autostarting mavlink via USB on PX4");
389  UAS_FCU(&mav_uas)->send_bytes(init, 3);
390  UAS_FCU(&mav_uas)->send_bytes(nsh, sizeof(nsh) - 1);
391  UAS_FCU(&mav_uas)->send_bytes(init, 4); /* NOTE in original init[3] */
392 }
393 
394 void MavRos::log_connect_change(bool connected)
395 {
397 
398  /* note: sys_status plugin required */
399  if (connected)
400  ROS_INFO("CON: Got HEARTBEAT, connected. FCU: %s", ap.c_str());
401  else
402  ROS_WARN("CON: Lost connection, HEARTBEAT timed out.");
403 }
msg
UAS mav_uas
UAS object passed to all plugins.
Definition: mavros.h:64
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
static bool pattern_match(std::string &pattern, std::string &pl_name)
Definition: mavros.cpp:264
void add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
load plugin
Definition: mavros.cpp:316
std::shared_ptr< MAVConnInterface const > ConstPtr
#define ROS_FATAL(...)
h
std::string format(const std::string &fmt, Args...args)
void publish(const boost::shared_ptr< M > &message) const
MAV_AUTOPILOT get_autopilot()
Returns autopilot type.
Definition: mavros_uas.h:131
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void setHardwareID(const std::string &hwid)
diagnostic_updater::Updater gcs_diag_updater
Definition: mavros.h:53
MavRos node implementation class.
void log_connect_change(bool connected)
Definition: mavros.cpp:394
bool gcs_quiet_mode
Definition: mavros.h:46
void start()
void add(const std::string &name, TaskFunction f)
ros::Subscriber mavlink_sub
Definition: mavros.h:51
#define UAS_DIAG(uasobjptr)
helper accessor to diagnostic updater
Definition: mavros_uas.h:48
static constexpr auto MAV_COMP_ID_UDP_BRIDGE
std::unordered_map< mavlink::msgid_t, plugin::PluginBase::Subscriptions > plugin_subscriptions
FCU link -> router -> plugin handler.
Definition: mavros.h:61
#define ROS_WARN(...)
void spinner()
void mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg)
ros -> fcu link
Definition: mavros.cpp:244
void set_connection_status(bool connected)
Definition: mavlink_diag.h:34
void mavlink_pub_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing)
fcu link -> ros
Definition: mavros.cpp:232
std::vector< std::string > V_string
ros::Publisher mavlink_pub
Definition: mavros.h:50
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
mavconn::MAVConnInterface::Ptr gcs_link
Definition: mavros.h:45
std::vector< std::string > getDeclaredClasses()
void add_connection_change_handler(ConnectionCb cb)
Add connection change handler callback.
Definition: uas_data.cpp:83
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:42
ros::Duration conn_timeout
Definition: mavros.h:48
bool is_mavlink_message_t(const size_t rt)
Definition: mavros.cpp:307
void set_tgt(uint8_t sys, uint8_t comp)
Definition: mavros_uas.h:170
std::string to_string(timesync_mode e)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
MAVConnInterface(const MAVConnInterface &)=delete
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void plugin_route_cb(const mavlink::mavlink_message_t *mmsg, const mavconn::Framing framing)
message router
Definition: mavros.cpp:254
#define ROS_DEBUG_STREAM(args)
ros::Time last_message_received_from_gcs
Definition: mavros.h:47
static bool is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist)
Checks that plugin blacklisted.
Definition: mavros.cpp:291
static std::vector< std::string > get_known_dialects()
static Ptr open_url(std::string url, uint8_t system_id=1, uint8_t component_id=MAV_COMP_ID_UDP_BRIDGE)
ROSCPP_DECL void requestShutdown()
uint32_t getNumSubscribers() const
#define ROS_INFO_STREAM(args)
void startup_px4_usb_quirk()
start mavlink app on USB
Definition: mavros.cpp:382
bool getParam(const std::string &key, std::string &s) const
pluginlib::ClassLoader< plugin::PluginBase > plugin_loader
Definition: mavros.h:57
std::vector< plugin::PluginBase::Ptr > loaded_plugins
Definition: mavros.h:58
static Time now()
MavlinkDiag gcs_link_diag
Definition: mavros.h:55
ROSCPP_DECL void shutdown()
#define ROS_ERROR_STREAM(args)
MavlinkDiag fcu_link_diag
Definition: mavros.h:54
#define ROS_ERROR(...)
ros::NodeHandle mavlink_nh
Definition: mavros.h:43
void set_mavconn(const mavconn::MAVConnInterface::Ptr &link)
Definition: mavlink_diag.h:30
void spin()
Definition: mavros.cpp:189
ROSCPP_DECL void waitForShutdown()
#define MAVLINK_VERSION
constexpr std::underlying_type< _T >::type enum_value(_T e)
Definition: utils.h:55
std::shared_ptr< MAVConnInterface > Ptr


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26