mavros: use shared io_service in router endpoints

Author: vooon

mavros/include/mavros/mavros_router.hpp

@@ -31,6 +31,7 @@
 #include <Eigen/Eigen>      // NOLINT
 
 #include "mavconn/interface.hpp"
+#include "mavconn/io_context_runner.hpp"
 #include "mavconn/mavlink_dialect.hpp"
 #include "mavros/utils.hpp"
 #include "rclcpp/macros.hpp"
@@ -145,6 +146,7 @@ class Router : public rclcpp::Node
     const std::string & node_name = "mavros_router")
   : rclcpp::Node(node_name,
       options /* rclcpp::NodeOptions(options).use_intra_process_comms(true) */),
+    router_io_runner(),
     endpoints{}, stat_msg_routed(0), stat_msg_sent(0), stat_msg_dropped(0),
     diagnostic_updater(this, 1.0)
   {
@@ -192,6 +194,8 @@ class Router : public rclcpp::Node
     RCLCPP_INFO(get_logger(), "Known MAVLink dialects:%s", ss.str().c_str());
     RCLCPP_INFO(get_logger(), "MAVROS Router started");
 
+    router_io_runner.start([this]() {this->router_io_runner.io().run();});
+
     // Delay parameter callback initialization because
     // add/del endpoints calls have to use shared_from_this(),
     // which cannot be used before we leave the constructor.
@@ -202,13 +206,24 @@ class Router : public rclcpp::Node
       });
   }
 
+  ~Router() override
+  {
+    router_io_runner.shutdown_owned();
+  }
+
   void route_message(Endpoint::SharedPtr src, const mavlink_message_t * msg, const Framing framing);
 
+  [[nodiscard]] asio::io_service * get_shared_io()
+  {
+    return &router_io_runner.io();
+  }
+
 private:
   friend class Endpoint;
   friend class TestRouter;
 
   static std::atomic<id_t> id_counter;
+  mavconn::IoContextRunner router_io_runner;
 
   std::shared_mutex mu;
 

mavros/src/lib/mavros_router.cpp

@@ -403,6 +403,11 @@ bool MAVConnEndpoint::is_open()
 
 std::pair<bool, std::string> MAVConnEndpoint::open()
 {
+  auto nh = this->parent;
+  if (!nh) {
+    return {false, "parent not set"};
+  }
+
   try {
     auto weak_self = weak_from_this();
     this->link = mavconn::MAVConnInterface::open_url(
@@ -411,7 +416,9 @@ std::pair<bool, std::string> MAVConnEndpoint::open()
         if (auto self = weak_self.lock()) {
           self->recv_message(msg, framing);
         }
-      });
+      },
+      mavconn::MAVConnInterface::ClosedCb(),
+      nh->get_shared_io());
   } catch (mavconn::DeviceError & ex) {
     return {false, ex.what()};
   }