From 93174327ee677c63bd39c74346c84b3b37e6a2ca Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 17:12:13 +0100 Subject: [PATCH 1/9] router: reduce critical section --- mavros/src/lib/mavros_router.cpp | 52 ++++++++++++++++++-------------- 1 file changed, 29 insertions(+), 23 deletions(-) diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp index 7a07a32a1..7a8b38b04 100644 --- a/mavros/src/lib/mavros_router.cpp +++ b/mavros/src/lib/mavros_router.cpp @@ -37,7 +37,6 @@ void Router::route_message( Endpoint::SharedPtr src, const mavlink_message_t * msg, const Framing framing) { - shared_lock lock(mu); this->stat_msg_routed++; // find message destination target @@ -52,35 +51,42 @@ void Router::route_message( } } - size_t sent_cnt = 0, retry_cnt = 0; -retry: - for (auto & kv : this->endpoints) { - auto & dest = kv.second; + auto collect_targets = [this, &src](addr_t addr) { + std::vector targets; + shared_lock lock(mu); + targets.reserve(this->endpoints.size()); - if (src->id == dest->id) { - continue; // do not echo message - } - if (src->link_type == dest->link_type) { - continue; // drop messages between same type FCU/GCS/UAS - } + for (const auto & kv : this->endpoints) { + const auto & dest = kv.second; + + if (src->id == dest->id) { + continue; // do not echo message + } + if (src->link_type == dest->link_type) { + continue; // drop messages between same type FCU/GCS/UAS + } - // NOTE(vooon): current router do not allow to speak drone-to-drone. - // if it is needed perhaps better to add mavlink-router in front of mavros-router. + // NOTE(vooon): current router do not allow to speak drone-to-drone. + // if it is needed perhaps better to add mavlink-router in front of mavros-router. + if (dest->remote_addrs.find(addr) != dest->remote_addrs.end()) { + targets.emplace_back(dest); + } + } - bool has_target = dest->remote_addrs.find(target_addr) != dest->remote_addrs.end(); + return targets; + }; - if (has_target) { - dest->send_message(msg, framing, src->id); - sent_cnt++; - } + auto targets = collect_targets(target_addr); + if (targets.empty() && target_addr != 0) { + // if targeted message hasn't been sent, retry as broadcast + target_addr = 0; + targets = collect_targets(target_addr); } - // if message haven't been sent retry broadcast it - if (sent_cnt == 0 && retry_cnt < 2) { - target_addr = 0; - retry_cnt++; - goto retry; + for (const auto & dest : targets) { + dest->send_message(msg, framing, src->id); } + const auto sent_cnt = targets.size(); // update stats this->stat_msg_sent.fetch_add(sent_cnt); From 441c4267fefcdae3eebbcdd12377a4f62c4a0719 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:13:45 +0000 Subject: [PATCH 2/9] mavros: reduce router lock hold time --- mavros/src/lib/mavros_router.cpp | 53 +++++++++++++++++++++----------- 1 file changed, 35 insertions(+), 18 deletions(-) diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp index 7a8b38b04..2e77f3266 100644 --- a/mavros/src/lib/mavros_router.cpp +++ b/mavros/src/lib/mavros_router.cpp @@ -108,7 +108,6 @@ void Router::add_endpoint( const mavros_msgs::srv::EndpointAdd::Request::SharedPtr request, mavros_msgs::srv::EndpointAdd::Response::SharedPtr response) { - unique_lock lock(mu); auto lg = get_logger(); RCLCPP_INFO( @@ -139,7 +138,10 @@ void Router::add_endpoint( ep->link_type = static_cast(request->type); ep->url = request->url; - this->endpoints[id] = ep; + { + unique_lock lock(mu); + this->endpoints[id] = ep; + } this->diagnostic_updater.add(ep->diag_name(), std::bind(&Endpoint::diag_run, ep, _1)); RCLCPP_INFO(lg, "Endpoint link[%d] created", id); @@ -159,17 +161,25 @@ void Router::del_endpoint( const mavros_msgs::srv::EndpointDel::Request::SharedPtr request, mavros_msgs::srv::EndpointDel::Response::SharedPtr response) { - unique_lock lock(mu); auto lg = get_logger(); + Endpoint::SharedPtr endpoint_to_remove; + std::string endpoint_diag_name; if (request->id != 0) { RCLCPP_INFO(lg, "Requested to del endpoint id: %d", request->id); - auto it = this->endpoints.find(request->id); - if (it != this->endpoints.end() ) { - it->second->close(); - this->diagnostic_updater.removeByName(it->second->diag_name()); - this->endpoints.erase(it); - response->successful = true; + { + unique_lock lock(mu); + auto it = this->endpoints.find(request->id); + if (it != this->endpoints.end() ) { + endpoint_to_remove = it->second; + endpoint_diag_name = it->second->diag_name(); + this->endpoints.erase(it); + response->successful = true; + } + } + if (endpoint_to_remove) { + endpoint_to_remove->close(); + this->diagnostic_updater.removeByName(endpoint_diag_name); } return; } @@ -177,17 +187,24 @@ void Router::del_endpoint( RCLCPP_INFO( lg, "Requested to del endpoint type: %d url: %s", request->type, request->url.c_str()); - for (auto it = this->endpoints.cbegin(); it != this->endpoints.cend(); it++) { - if (it->second->url == request->url && - it->second->link_type == static_cast( request->type)) - { - it->second->close(); - this->diagnostic_updater.removeByName(it->second->diag_name()); - this->endpoints.erase(it); - response->successful = true; - return; + { + unique_lock lock(mu); + for (auto it = this->endpoints.cbegin(); it != this->endpoints.cend(); it++) { + if (it->second->url == request->url && + it->second->link_type == static_cast(request->type)) + { + endpoint_to_remove = it->second; + endpoint_diag_name = it->second->diag_name(); + this->endpoints.erase(it); + response->successful = true; + break; + } } } + if (endpoint_to_remove) { + endpoint_to_remove->close(); + this->diagnostic_updater.removeByName(endpoint_diag_name); + } } rcl_interfaces::msg::SetParametersResult Router::on_set_parameters_cb( From 0e8d8ba12956dcbf8dcc174d3eac08c22e47143d Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:16:43 +0000 Subject: [PATCH 3/9] mavros: modernize router for C++20 --- mavros/include/mavros/mavros_router.hpp | 28 ++++++++++++++++++------- mavros/src/lib/mavros_router.cpp | 24 ++++++++++++++------- 2 files changed, 37 insertions(+), 15 deletions(-) diff --git a/mavros/include/mavros/mavros_router.hpp b/mavros/include/mavros/mavros_router.hpp index 000d05558..a253d72fe 100644 --- a/mavros/include/mavros/mavros_router.hpp +++ b/mavros/include/mavros/mavros_router.hpp @@ -53,7 +53,6 @@ using mavconn::Framing; using ::mavlink::mavlink_message_t; using ::mavlink::msgid_t; -using namespace std::placeholders; // NOLINT using namespace std::chrono_literals; // NOLINT class Router; @@ -157,18 +156,28 @@ class Router : public rclcpp::Node add_service = this->create_service( "~/add_endpoint", - std::bind(&Router::add_endpoint, this, _1, _2)); + [this]( + const mavros_msgs::srv::EndpointAdd::Request::SharedPtr request, + mavros_msgs::srv::EndpointAdd::Response::SharedPtr response) + { + this->add_endpoint(request, response); + }); del_service = this->create_service( "~/del_endpoint", - std::bind(&Router::del_endpoint, this, _1, _2)); + [this]( + const mavros_msgs::srv::EndpointDel::Request::SharedPtr request, + mavros_msgs::srv::EndpointDel::Response::SharedPtr response) + { + this->del_endpoint(request, response); + }); // try to reconnect endpoint each 30 seconds reconnect_timer = - this->create_wall_timer(30s, std::bind(&Router::periodic_reconnect_endpoints, this)); + this->create_wall_timer(30s, [this]() {this->periodic_reconnect_endpoints();}); // collect garbage addrs each minute stale_addrs_timer = - this->create_wall_timer(60s, std::bind(&Router::periodic_clear_stale_remote_addrs, this)); + this->create_wall_timer(60s, [this]() {this->periodic_clear_stale_remote_addrs();}); diagnostic_updater.setHardwareID("none"); // NOTE: router connects several hardwares diagnostic_updater.add("MAVROS Router", this, &Router::diag_run); @@ -201,7 +210,7 @@ class Router : public rclcpp::Node static std::atomic id_counter; - std::shared_timed_mutex mu; + std::shared_mutex mu; // map stores all routing endpoints std::unordered_map endpoints; @@ -232,7 +241,10 @@ class Router : public rclcpp::Node void param_init() { set_parameters_handle_ptr = - this->add_on_set_parameters_callback(std::bind(&Router::on_set_parameters_cb, this, _1)); + this->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + return this->on_set_parameters_cb(parameters); + }); auto params = get_parameters({"fcu_urls", "gcs_urls", "uas_urls"}); on_set_parameters_cb(params); @@ -240,7 +252,7 @@ class Router : public rclcpp::Node void param_init_once() { - std::call_once(param_init_flag, std::bind(&Router::param_init, this)); + std::call_once(param_init_flag, &Router::param_init, this); } rcl_interfaces::msg::SetParametersResult on_set_parameters_cb( diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp index 2e77f3266..49615eb4f 100644 --- a/mavros/src/lib/mavros_router.cpp +++ b/mavros/src/lib/mavros_router.cpp @@ -23,8 +23,8 @@ using namespace mavros::router; // NOLINT using rclcpp::QoS; -using unique_lock = std::unique_lock; -using shared_lock = std::shared_lock; +using unique_lock = std::unique_lock; +using shared_lock = std::shared_lock; std::atomic Router::id_counter {1000}; @@ -142,7 +142,11 @@ void Router::add_endpoint( unique_lock lock(mu); this->endpoints[id] = ep; } - this->diagnostic_updater.add(ep->diag_name(), std::bind(&Endpoint::diag_run, ep, _1)); + this->diagnostic_updater.add( + ep->diag_name(), + [ep](diagnostic_updater::DiagnosticStatusWrapper & stat) { + ep->diag_run(stat); + }); RCLCPP_INFO(lg, "Endpoint link[%d] created", id); auto result = ep->open(); @@ -400,10 +404,14 @@ bool MAVConnEndpoint::is_open() std::pair MAVConnEndpoint::open() { try { + auto weak_self = weak_from_this(); this->link = mavconn::MAVConnInterface::open_url( - this->url, 1, mavconn::MAV_COMP_ID_UDP_BRIDGE, std::bind( - &MAVConnEndpoint::recv_message, - shared_from_this(), _1, _2)); + this->url, 1, mavconn::MAV_COMP_ID_UDP_BRIDGE, + [weak_self](const mavlink_message_t * msg, const Framing framing) { + if (auto self = weak_self.lock()) { + self->recv_message(msg, framing); + } + }); } catch (mavconn::DeviceError & ex) { return {false, ex.what()}; } @@ -501,7 +509,9 @@ std::pair ROSEndpoint::open() "mavlink_source"), qos); this->sink = nh->create_subscription( utils::format("%s/%s", this->url.c_str(), "mavlink_sink"), qos, - std::bind(&ROSEndpoint::ros_recv_message, this, _1)); + [this](const mavros_msgs::msg::Mavlink::SharedPtr rmsg) { + this->ros_recv_message(rmsg); + }); } catch (rclcpp::exceptions::InvalidTopicNameError & ex) { return {false, ex.what()}; } From ed05833c78b3e2f0d6c714609f90d09a83acb8b8 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:18:33 +0000 Subject: [PATCH 4/9] mavros: use shared io_service in router endpoints --- mavros/include/mavros/mavros_router.hpp | 15 +++++++++++++++ mavros/src/lib/mavros_router.cpp | 9 ++++++++- 2 files changed, 23 insertions(+), 1 deletion(-) diff --git a/mavros/include/mavros/mavros_router.hpp b/mavros/include/mavros/mavros_router.hpp index a253d72fe..01b9deea8 100644 --- a/mavros/include/mavros/mavros_router.hpp +++ b/mavros/include/mavros/mavros_router.hpp @@ -31,6 +31,7 @@ #include // 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_counter; + mavconn::IoContextRunner router_io_runner; std::shared_mutex mu; diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp index 49615eb4f..1b650d719 100644 --- a/mavros/src/lib/mavros_router.cpp +++ b/mavros/src/lib/mavros_router.cpp @@ -403,6 +403,11 @@ bool MAVConnEndpoint::is_open() std::pair 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 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()}; } From f81a5d2617cb169d41eb0824a9c1361d45f023bd Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:30:25 +0000 Subject: [PATCH 5/9] mavros: add skippable router stress test --- mavros/test/test_router.cpp | 100 ++++++++++++++++++++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp index 331a43ed7..2504f4877 100644 --- a/mavros/test/test_router.cpp +++ b/mavros/test/test_router.cpp @@ -14,8 +14,11 @@ #include #include +#include +#include #include #include +#include #include #include #include @@ -65,6 +68,36 @@ class MockEndpoint : public Endpoint MOCK_METHOD1(diag_run, void(diagnostic_updater::DiagnosticStatusWrapper &)); }; +class StressEndpoint : public Endpoint +{ +public: + using SharedPtr = std::shared_ptr; + + std::atomic send_count {0}; + + bool is_open() override + { + return true; + } + + std::pair open() override + { + return {true, ""}; + } + + void close() override {} + + void send_message( + const mavlink_message_t * msg [[maybe_unused]], + const Framing framing [[maybe_unused]], + id_t src_id [[maybe_unused]]) override + { + send_count.fetch_add(1, std::memory_order_relaxed); + } + + void diag_run(diagnostic_updater::DiagnosticStatusWrapper & stat [[maybe_unused]]) override {} +}; + namespace mavros { namespace router @@ -434,6 +467,73 @@ TEST_F(TestRouter, endpoint_recv_message) ASSERT_EQ(size_t(1), get_stat_msg_dropped(router)); } +TEST_F(TestRouter, route_stress_multithreaded_broadcast) +{ + if (const char * skip = std::getenv("MAVROS_SKIP_STRESS_TESTS"); + skip && std::string(skip) == "1") + { + GTEST_SKIP() << "skipped by MAVROS_SKIP_STRESS_TESTS=1"; + } + + auto router = create_node_no_endpoints(); + + auto make_ep = [router](id_t id, LT type) { + auto ep = std::make_shared(); + ep->parent = router; + ep->id = id; + ep->link_type = type; + ep->remote_addrs = {0x0000}; + return ep; + }; + + auto & endpoints = get_endpoints(router); + + auto src = make_ep(1000, LT::fcu); + auto dst1 = make_ep(1001, LT::gcs); + auto dst2 = make_ep(1002, LT::gcs); + auto dst3 = make_ep(1003, LT::uas); + auto dst4 = make_ep(1004, LT::uas); + endpoints[src->id] = src; + endpoints[dst1->id] = dst1; + endpoints[dst2->id] = dst2; + endpoints[dst3->id] = dst3; + endpoints[dst4->id] = dst4; + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x0101); + constexpr auto fr = Framing::ok; + + constexpr size_t thread_count = 8; + constexpr size_t iterations_per_thread = 2000; + const auto expected_routed = thread_count * iterations_per_thread; + const auto expected_sent_per_endpoint = expected_routed; + const auto expected_sent_total = expected_routed * 4; + + std::vector workers; + workers.reserve(thread_count); + + for (size_t i = 0; i < thread_count; i++) { + workers.emplace_back([&]() { + for (size_t n = 0; n < iterations_per_thread; n++) { + router->route_message(src, &hbmsg, fr); + } + }); + } + + for (auto & w : workers) { + w.join(); + } + + EXPECT_EQ(get_stat_msg_routed(router), expected_routed); + EXPECT_EQ(get_stat_msg_sent(router), expected_sent_total); + EXPECT_EQ(get_stat_msg_dropped(router), 0U); + + EXPECT_EQ(dst1->send_count.load(), expected_sent_per_endpoint); + EXPECT_EQ(dst2->send_count.load(), expected_sent_per_endpoint); + EXPECT_EQ(dst3->send_count.load(), expected_sent_per_endpoint); + EXPECT_EQ(dst4->send_count.load(), expected_sent_per_endpoint); +} + #if 0 // TODO(vooon): TEST_F(TestRouter, uas_recv_message) { From 9256316f3345083f0c2ab5a72b501e658afecb74 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:32:47 +0000 Subject: [PATCH 6/9] mavros: add router fallback and drop-path tests --- mavros/test/test_router.cpp | 64 +++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp index 2504f4877..44f124120 100644 --- a/mavros/test/test_router.cpp +++ b/mavros/test/test_router.cpp @@ -430,6 +430,70 @@ TEST_F(TestRouter, route_targeted_system_component) VERIFY_EPS(); } +TEST_F(TestRouter, route_targeted_miss_falls_back_to_broadcast) +{ + using MF = mavlink::common::MAV_MODE_FLAG; + using utils::enum_value; + + auto router = this->create_node(); + + auto set_mode = mavlink::common::msg::SET_MODE(); + set_mode.target_system = 0x42; // unknown target in test topology + set_mode.base_mode = enum_value(MF::SAFETY_ARMED) | enum_value(MF::TEST_ENABLED); + set_mode.custom_mode = 7; + + auto smmsg = convert_message(set_mode, 0x0101); + auto fr = Framing::ok; + + DEFINE_EPS(); + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*uas1, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*uas2, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*gcs1, send_message(_, fr, _)).Times(1); + EXPECT_CALL(*gcs2, send_message(_, fr, _)).Times(1); + + router->route_message(fcu1, &smmsg, fr); + + VERIFY_EPS(); +} + +TEST_F(TestRouter, route_drops_when_only_same_link_type_exists) +{ + auto router = this->create_node_no_endpoints(); + auto & endpoints = get_endpoints(router); + + auto make_ep = [router](id_t id, const std::string & url) { + auto ep = std::make_shared(); + ep->parent = router; + ep->id = id; + ep->link_type = LT::fcu; + ep->url = url; + ep->remote_addrs = {0x0000, 0x0100, 0x0101}; + testing::Mock::AllowLeak(&(*ep)); + return ep; + }; + + auto fcu1 = make_ep(1000, "mock://fcu1"); + auto fcu2 = make_ep(1001, "mock://fcu2"); + endpoints[fcu1->id] = fcu1; + endpoints[fcu2->id] = fcu2; + + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x0101); + auto fr = Framing::ok; + + EXPECT_CALL(*fcu1, send_message(_, fr, _)).Times(0); + EXPECT_CALL(*fcu2, send_message(_, fr, _)).Times(0); + + router->route_message(fcu1, &hbmsg, fr); + + EXPECT_EQ(size_t(1), get_stat_msg_routed(router)); + EXPECT_EQ(size_t(0), get_stat_msg_sent(router)); + EXPECT_EQ(size_t(1), get_stat_msg_dropped(router)); +} + TEST_F(TestRouter, endpoint_recv_message) { auto router = create_node_no_endpoints(); From e75f02a83c8769b91b779bae3fd006442ac40aea Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:34:42 +0000 Subject: [PATCH 7/9] mavros: add stale-address lifecycle test --- mavros/test/test_router.cpp | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/mavros/test/test_router.cpp b/mavros/test/test_router.cpp index 44f124120..61cae5414 100644 --- a/mavros/test/test_router.cpp +++ b/mavros/test/test_router.cpp @@ -224,6 +224,11 @@ class TestRouter : public ::testing::Test { return router->stat_msg_dropped.load(); } + + void run_clear_stale_remote_addrs(Router::SharedPtr router) + { + router->periodic_clear_stale_remote_addrs(); + } }; TEST_F(TestRouter, set_parameter) @@ -531,6 +536,37 @@ TEST_F(TestRouter, endpoint_recv_message) ASSERT_EQ(size_t(1), get_stat_msg_dropped(router)); } +TEST_F(TestRouter, clear_stale_remote_addrs_keeps_active_and_reaps_stale) +{ + auto router = this->create_node(); + auto uas1 = getep(router, 1002); + + // baseline from fixture + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0000)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x0100)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x01BF)); + + // add one stale-only remote address + uas1->remote_addrs.emplace(0x1234); + + // pass #1 only primes stale set from current remotes + run_clear_stale_remote_addrs(router); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x1234)); + ASSERT_NE(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x1234)); + + // emulate activity on one address between timer passes + auto hb = make_heartbeat(); + auto hbmsg = convert_message(hb, 0x01BF); + uas1->Endpoint::recv_message(&hbmsg, Framing::ok); + ASSERT_EQ(uas1->stale_addrs.end(), uas1->stale_addrs.find(0x01BF)); + + // pass #2 removes untouched stale-only address but keeps active one + run_clear_stale_remote_addrs(router); + + ASSERT_EQ(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x1234)); + ASSERT_NE(uas1->remote_addrs.end(), uas1->remote_addrs.find(0x01BF)); +} + TEST_F(TestRouter, route_stress_multithreaded_broadcast) { if (const char * skip = std::getenv("MAVROS_SKIP_STRESS_TESTS"); From fa5b22162ce34e4806a654d6acf7600a0db820b3 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 15 Feb 2026 16:34:58 +0000 Subject: [PATCH 8/9] ci: enable coverage job for mavros --- .github/workflows/coverage.yml | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/.github/workflows/coverage.yml b/.github/workflows/coverage.yml index 3c9257453..9093c2f56 100644 --- a/.github/workflows/coverage.yml +++ b/.github/workflows/coverage.yml @@ -25,14 +25,12 @@ jobs: matrix: ros_distro: [kilted] component: - # Add more entries here when enabling coverage for more modules. - # Example: - # - name: mavros - # packages: mavros - # source_filter: ".*/mavros/.*" - name: libmavconn packages: libmavconn source_filter: ".*/libmavconn/.*" + - name: mavros + packages: mavros + source_filter: ".*/mavros/.*" env: ROS_DISTRO: ${{ matrix.ros_distro }} From 427d496a1b74e216452c0daeeede39b1bc2ae210 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 16 Feb 2026 10:00:30 +0000 Subject: [PATCH 9/9] mavros: fix router cpplint line length --- mavros/src/lib/mavros_router.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mavros/src/lib/mavros_router.cpp b/mavros/src/lib/mavros_router.cpp index 1b650d719..bc1863193 100644 --- a/mavros/src/lib/mavros_router.cpp +++ b/mavros/src/lib/mavros_router.cpp @@ -67,7 +67,8 @@ void Router::route_message( } // NOTE(vooon): current router do not allow to speak drone-to-drone. - // if it is needed perhaps better to add mavlink-router in front of mavros-router. + // if needed, perhaps better to add mavlink-router in front of + // mavros-router. if (dest->remote_addrs.find(addr) != dest->remote_addrs.end()) { targets.emplace_back(dest); }