diff --git a/mavros/README.md b/mavros/README.md index 5e2a20d18..1e263e205 100644 --- a/mavros/README.md +++ b/mavros/README.md @@ -109,6 +109,12 @@ Main node. Allow disable GCS proxy by setting empty URL. ros2 run mavros mavros_node --ros-args --params-file params.yaml +For ROS 2 deployments where the UAS plugin executor consumes too much CPU, +set `MAVROS_UAS_EXECUTOR_THREADS` to limit its worker count. Values are clamped +to 2-16 threads; for example: + + MAVROS_UAS_EXECUTOR_THREADS=2 ros2 run mavros mavros_node --ros-args --params-file params.yaml + Launch Files ------------ diff --git a/mavros/src/lib/uas_executor.cpp b/mavros/src/lib/uas_executor.cpp index 567b15a84..feb9ec342 100644 --- a/mavros/src/lib/uas_executor.cpp +++ b/mavros/src/lib/uas_executor.cpp @@ -12,6 +12,10 @@ */ #include +#include +#include +#include +#include #include "mavros/uas_executor.hpp" @@ -19,6 +23,30 @@ using namespace mavros; // NOLINT using namespace mavros::uas; // NOLINT using namespace std::chrono_literals; // NOLINT +namespace +{ +constexpr size_t kMinExecutorThreads = 2; +constexpr size_t kMaxExecutorThreads = 16; +constexpr std::string_view kExecutorThreadsEnv = "MAVROS_UAS_EXECUTOR_THREADS"; + +size_t configured_number_of_threads() +{ + auto value = std::getenv(kExecutorThreadsEnv.data()); + if (value == nullptr || value[0] == '\0') { + return 0; + } + + size_t threads = 0; + const auto input = std::string_view(value); + const auto result = std::from_chars(input.data(), input.data() + input.size(), threads); + if (result.ec != std::errc{} || result.ptr != input.data() + input.size() || threads == 0) { + return 0; + } + + return std::clamp(threads, kMinExecutorThreads, kMaxExecutorThreads); +} +} // namespace + UASExecutor::UASExecutor(const rclcpp::ExecutorOptions & options) : MultiThreadedExecutor(options, select_number_of_threads(), true, 1000ms), source_system(0), @@ -28,6 +56,11 @@ UASExecutor::UASExecutor(const rclcpp::ExecutorOptions & options) size_t UASExecutor::select_number_of_threads() { + auto configured_threads = configured_number_of_threads(); + if (configured_threads > 0) { + return configured_threads; + } + // return std::max(16, std::min(std::thread::hardware_concurrency(), 4)); return std::clamp(std::thread::hardware_concurrency(), 4, 16); }