diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml index 2291e12a..d0082799 100644 --- a/rmcs_ws/src/rmcs_bringup/config/flight.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -21,6 +21,7 @@ rmcs_executor: - rmcs_core::referee::Status -> referee_status - rmcs_core::referee::command::interaction::Ui -> referee_ui - rmcs_core::referee::app::ui::Flight -> referee_ui_flight + - rmcs_core::referee::app::ui::AutoAimUi -> auto_aim_ui # - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster # - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster @@ -151,3 +152,9 @@ bullet_feeder_velocity_pid_controller: kp: 0.583 ki: 0.0 kd: 0.0 + +auto_aim_ui: + ros__parameters: + offset_x: 0.000 + offset_y: 0.000 + offset_z: 0.000 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 1f46763e..3ac3bf0c 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -56,4 +56,5 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp new file mode 100644 index 00000000..b874a165 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/auto_aim.cpp @@ -0,0 +1,183 @@ +#include +#include +#include + +#include +#include +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { +using namespace rmcs_description; + +class AutoAimUi + : public rmcs_executor::Component + , public rclcpp::Node { +public: + AutoAimUi() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + offset_ = Eigen::Vector3d{ + get_parameter_or("offset_x", 0.), + get_parameter_or("offset_y", 0.), + get_parameter_or("offset_z", 0.), + }; + + // What's the point of an auto-aim UI if it doesn't have auto-aim? + register_input("/tf", tf_, true); + register_input("/auto_aim/robot_center", robot_center_, true); + register_input("/auto_aim/should_shoot", should_shoot_, true); + } + + void update() override { + if (!robot_center_->allFinite() || robot_center_->isZero()) { + hide_all(); + return; + } + + const auto point = reproject(*robot_center_); + if (!point.allFinite() // + || point.x() >= kScreenW || point.x() < 0 // + || point.y() >= kScreenH || point.y() < 0 // + ) { + hide_all(); + return; + } + + const auto x = std::clamp(std::lround(point.x()), 0, kScreenW - 1); + const auto y = std::clamp(std::lround(point.y()), 0, kScreenH - 1); + + const auto color = *should_shoot_ ? Shape::Color::ORANGE : Shape::Color::GREEN; + const auto radius = *should_shoot_ ? 10 : 15; + + center_ring_.set_color(color); + center_ring_.set_x(x); + center_ring_.set_y(y); + center_ring_.set_r(radius); + center_ring_.set_visible(true); + + if (*should_shoot_) { + constexpr int kCrossLen = 16; + constexpr int kCrossGap = 5; + + constexpr auto clamp_x = [](int x) { + return std::clamp(x, 0, kScreenW - 1); + }; + constexpr auto clamp_y = [](int y) { + return std::clamp(y, 0, kScreenH - 1); + }; + + cross_top_.set_color(color); + cross_top_.set_x(x); + cross_top_.set_y(clamp_y(y - kCrossLen)); + cross_top_.set_x2(x); + cross_top_.set_y2(clamp_y(y - kCrossGap)); + cross_top_.set_visible(true); + + cross_bottom_.set_color(color); + cross_bottom_.set_x(x); + cross_bottom_.set_y(clamp_y(y + kCrossGap)); + cross_bottom_.set_x2(x); + cross_bottom_.set_y2(clamp_y(y + kCrossLen)); + cross_bottom_.set_visible(true); + + cross_left_.set_color(color); + cross_left_.set_x(clamp_x(x - kCrossLen)); + cross_left_.set_y(y); + cross_left_.set_x2(clamp_x(x - kCrossGap)); + cross_left_.set_y2(y); + cross_left_.set_visible(true); + + cross_right_.set_color(color); + cross_right_.set_x(clamp_x(x + kCrossGap)); + cross_right_.set_y(y); + cross_right_.set_x2(clamp_x(x + kCrossLen)); + cross_right_.set_y2(y); + cross_right_.set_visible(true); + } else { + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + } + +private: + static constexpr std::uint16_t kScreenW = 1920; + static constexpr std::uint16_t kScreenH = 1080; + + static constexpr double kFx = 730.7267062695; + static constexpr double kFy = 730.5886055073; + static constexpr double kCx = 961.8345772991; + static constexpr double kCy = 549.3357457590; + + static constexpr double kK1 = -0.1764932263; + static constexpr double kK2 = +0.1582678597; + static constexpr double kK3 = -0.1557993057; + static constexpr double kP1 = -0.0000433367; + static constexpr double kP2 = +0.0003937540; + + Eigen::Vector3d offset_ = Eigen::Vector3d::Zero(); + + InputInterface tf_; + InputInterface robot_center_; + InputInterface should_shoot_; + + Circle center_ring_{Shape::Color::GREEN, 2, 0, 0, 5, 5, false}; + Line cross_top_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_bottom_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_left_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + Line cross_right_{Shape::Color::GREEN, 2, 0, 0, 0, 0, false}; + + void hide_all() { + center_ring_.set_visible(false); + cross_top_.set_visible(false); + cross_bottom_.set_visible(false); + cross_left_.set_visible(false); + cross_right_.set_visible(false); + } + + Eigen::Vector2d reproject(const Eigen::Vector3d& center) const { + const auto camera_pose = fast_tf::lookup_transform(*tf_); + + const auto q_aa = Eigen::Quaterniond{camera_pose.rotation()}; + const auto t_aa = Eigen::Vector3d{camera_pose.translation()}; + + const auto t_ui = Eigen::Vector3d{t_aa - q_aa * offset_}; + + const auto point_ros = Eigen::Vector3d{q_aa.inverse() * (center - t_ui)}; + + const double x_cv = -point_ros.y(); + const double y_cv = +point_ros.z(); + const double z_cv = +point_ros.x(); + + if (z_cv <= 1e-6) + return Eigen::Vector2d{ + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + }; + + const double x_norm = x_cv / z_cv; + const double y_norm = y_cv / z_cv; + + const double r2 = x_norm * x_norm + y_norm * y_norm; + const double r4 = r2 * r2; + const double r6 = r4 * r2; + + const double radial = 1.0 + kK1 * r2 + kK2 * r4 + kK3 * r6; + const double x_dist = + x_norm * radial + 2.0 * kP1 * x_norm * y_norm + kP2 * (r2 + 2.0 * x_norm * x_norm); + const double y_dist = + y_norm * radial + kP1 * (r2 + 2.0 * y_norm * y_norm) + 2.0 * kP2 * x_norm * y_norm; + + return Eigen::Vector2d{kFx * x_dist + kCx, kFy * y_dist + kCy}; + } +}; + +} // namespace rmcs_core::referee::app::ui + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::app::ui::AutoAimUi, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_executor/CMakeLists.txt b/rmcs_ws/src/rmcs_executor/CMakeLists.txt index ec21edbf..8f255f45 100644 --- a/rmcs_ws/src/rmcs_executor/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_executor/CMakeLists.txt @@ -3,7 +3,8 @@ project(rmcs_executor) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-g1 -rdynamic -O2 -Wall -Wextra -Wpedantic) diff --git a/rmcs_ws/src/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/src/executor.hpp index eb454fd0..751d4dfd 100644 --- a/rmcs_ws/src/rmcs_executor/src/executor.hpp +++ b/rmcs_ws/src/rmcs_executor/src/executor.hpp @@ -1,10 +1,18 @@ #pragma once +#include +#include +#include +#include +#include +#include #include #include +#include #include #include #include +#include #include #include #include @@ -16,6 +24,8 @@ #include "predefined_msg_provider.hpp" #include "rmcs_executor/component.hpp" +#include "rmcs_utility/tdigest.hpp" +#include "rmcs_utility/thread_config.hpp" namespace rmcs_executor { @@ -55,36 +65,298 @@ class Executor final : public rclcpp::Node { double update_rate; if (!get_parameter("update_rate", update_rate)) throw std::runtime_error{"Unable to get parameter update_rate"}; + if (!std::isfinite(update_rate) || update_rate <= 0.0) + throw std::runtime_error{"Parameter update_rate must be finite and positive"}; + const auto period = + std::chrono::nanoseconds(static_cast(std::round(1'000'000'000.0 / update_rate))); + if (period.count() <= 0) + throw std::runtime_error{ + "Parameter update_rate is too large to produce a valid period"}; predefined_msg_provider_->set_update_rate(update_rate); + + std::string thread_config_spec; + get_parameter_or("thread_config", thread_config_spec, ""); + auto thread_config = rmcs_utility::ThreadConfig{thread_config_spec, "update"}; + enable_event_outputs(); - thread_ = std::thread{[update_rate, this]() { - const auto period = std::chrono::nanoseconds( - static_cast(std::round(1'000'000'000.0 / update_rate))); - auto next_iteration_time = std::chrono::steady_clock::now(); - try { - while (rclcpp::ok()) { - predefined_msg_provider_->set_timestamp(next_iteration_time); - next_iteration_time += period; - for (const auto& component : updating_order_) { - component->update(); - } - std::this_thread::sleep_until(next_iteration_time); - } - } catch (const std::exception& exception) { - RCLCPP_FATAL( - get_logger(), "Executor update thread terminated by exception: %s", - exception.what()); - rclcpp::shutdown(); - } catch (...) { - RCLCPP_FATAL( - get_logger(), "Executor update thread terminated by unknown exception"); - rclcpp::shutdown(); - } + thread_ = std::thread{[this, period, thread_config = std::move(thread_config)]() { + thread_main(period, thread_config); }}; } private: + using SteadyClock = std::chrono::steady_clock; + + static constexpr size_t digest_size_ = 256; + + struct CumulativeStats { + rmcs_utility::TDigest start_lateness_ms{digest_size_}; + rmcs_utility::TDigest update_duration_ms{digest_size_}; + uint64_t update_count = 0; + uint64_t skipped_count = 0; + uint64_t start_lateness_sample_count = 0; + uint64_t update_duration_sample_count = 0; + double start_lateness_max_ms = 0.0; + double update_duration_max_ms = 0.0; + }; + + struct WindowStats { + explicit WindowStats(size_t component_count) + : component_update_duration_sums(component_count) {} + + uint64_t executed_count = 0; + SteadyClock::duration total_update_duration{}; + std::vector component_update_duration_sums; + }; + + struct ThreadStats { + explicit ThreadStats(size_t component_count, SteadyClock::time_point start_time) + : window(component_count) + , next_report_time(start_time + std::chrono::seconds(5)) {} + + CumulativeStats cumulative; + WindowStats window; + SteadyClock::time_point next_report_time; + }; + + struct ComponentWindowStat { + size_t index; + SteadyClock::duration duration_sum; + }; + + void thread_main( + std::chrono::nanoseconds period, const rmcs_utility::ThreadConfig& thread_config) { + if (const auto result = thread_config.apply_to_current_thread(); !result) + RCLCPP_WARN(get_logger(), "%s", result.error().c_str()); + + auto next_iteration_time = SteadyClock::now(); + auto stats = ThreadStats{updating_order_.size(), next_iteration_time}; + + try { + while (rclcpp::ok()) { + const auto scheduled_start = next_iteration_time; + const auto actual_end = execute_update_iteration(scheduled_start, stats); + next_iteration_time = calculate_next_iteration_time( + scheduled_start, actual_end, period, stats.cumulative); + log_due_reports(actual_end, stats); + std::this_thread::sleep_until(next_iteration_time); + } + } catch (const std::exception& exception) { + RCLCPP_FATAL( + get_logger(), "Executor update thread terminated by exception: %s", + exception.what()); + rclcpp::shutdown(); + } catch (...) { + RCLCPP_FATAL(get_logger(), "Executor update thread terminated by unknown exception"); + rclcpp::shutdown(); + } + } + + SteadyClock::time_point + execute_update_iteration(SteadyClock::time_point scheduled_start, ThreadStats& stats) { + auto current_time = SteadyClock::now(); + const auto actual_start = current_time; + + record_start_lateness(scheduled_start, actual_start, stats.cumulative); + + stats.cumulative.update_count++; + stats.window.executed_count++; + + predefined_msg_provider_->set_timestamp(scheduled_start); + + for (size_t component_index = 0; component_index < updating_order_.size(); + ++component_index) { + const auto component_start_time = current_time; + updating_order_[component_index]->update(); + current_time = SteadyClock::now(); + stats.window.component_update_duration_sums[component_index] += + current_time - component_start_time; + } + + const auto update_duration = current_time - actual_start; + record_update_duration(update_duration, stats); + return current_time; + } + + void record_start_lateness( + SteadyClock::time_point scheduled_start, SteadyClock::time_point actual_start, + CumulativeStats& stats) { + if (actual_start < scheduled_start) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Executor update thread woke early by %.3f ms", + duration_to_ms(scheduled_start - actual_start)); + return; + } + + const auto start_lateness_ms = duration_to_ms(actual_start - scheduled_start); + stats.start_lateness_ms.insert(start_lateness_ms); + stats.start_lateness_max_ms = std::max(stats.start_lateness_max_ms, start_lateness_ms); + stats.start_lateness_sample_count++; + } + + static void record_update_duration(SteadyClock::duration update_duration, ThreadStats& stats) { + const auto update_duration_ms = duration_to_ms(update_duration); + stats.cumulative.update_duration_ms.insert(update_duration_ms); + stats.cumulative.update_duration_max_ms = + std::max(stats.cumulative.update_duration_max_ms, update_duration_ms); + stats.cumulative.update_duration_sample_count++; + stats.window.total_update_duration += update_duration; + } + + static SteadyClock::time_point calculate_next_iteration_time( + SteadyClock::time_point scheduled_start, SteadyClock::time_point actual_end, + std::chrono::nanoseconds period, CumulativeStats& stats) { + auto next_iteration_time = scheduled_start + period; + if (actual_end <= next_iteration_time) + return next_iteration_time; + + const auto overdue_duration = actual_end - next_iteration_time; + const auto skipped_cycles = + static_cast( + (overdue_duration - std::chrono::nanoseconds{1}).count() / period.count()) + + 1; + stats.skipped_count += skipped_cycles; + stats.update_count += skipped_cycles; + return next_iteration_time + period * skipped_cycles; + } + + void log_due_reports(SteadyClock::time_point current_time, ThreadStats& stats) { + while (current_time >= stats.next_report_time) { + log_cumulative_stats(stats.cumulative); + log_window_stats(stats.window); + reset_window_stats(stats.window); + stats.next_report_time += std::chrono::seconds(5); + } + } + + void log_cumulative_stats(CumulativeStats& stats) { + stats.start_lateness_ms.merge(); + stats.update_duration_ms.merge(); + + const double skipped_ratio = stats.update_count == 0 + ? 0.0 + : static_cast(stats.skipped_count) + / static_cast(stats.update_count) * 100.0; + + RCLCPP_INFO( + get_logger(), + "Update/Skipped: %llu/%llu (%s%%), " + "Stat ms p50/p99/max: start_late %s/%s/%s, update %s/%s/%s", + static_cast(stats.update_count), + static_cast(stats.skipped_count), + format_percent(skipped_ratio).c_str(), + format_stat( + maybe_quantile(stats.start_lateness_ms, stats.start_lateness_sample_count, 50.0)) + .c_str(), + format_stat( + maybe_quantile(stats.start_lateness_ms, stats.start_lateness_sample_count, 99.0)) + .c_str(), + format_stat( + stats.start_lateness_sample_count == 0 + ? std::nullopt + : std::optional{stats.start_lateness_max_ms}) + .c_str(), + format_stat( + maybe_quantile(stats.update_duration_ms, stats.update_duration_sample_count, 50.0)) + .c_str(), + format_stat( + maybe_quantile(stats.update_duration_ms, stats.update_duration_sample_count, 99.0)) + .c_str(), + format_stat( + stats.update_duration_sample_count == 0 + ? std::nullopt + : std::optional{stats.update_duration_max_ms}) + .c_str()); + } + + void log_window_stats(const WindowStats& stats) { + if (stats.executed_count == 0 + || stats.total_update_duration == SteadyClock::duration::zero()) + return; + + auto top_component_stats = collect_top_component_stats(stats); + if (top_component_stats.empty()) + return; + + RCLCPP_INFO( + get_logger(), "Component 5s: %s", + format_top_component_stats(top_component_stats, stats).c_str()); + } + + static std::vector collect_top_component_stats(const WindowStats& stats) { + auto top_component_stats = std::vector{}; + top_component_stats.reserve(stats.component_update_duration_sums.size()); + for (size_t component_index = 0; + component_index < stats.component_update_duration_sums.size(); ++component_index) { + const auto duration_sum = stats.component_update_duration_sums[component_index]; + if (duration_sum == SteadyClock::duration::zero()) + continue; + top_component_stats.emplace_back(component_index, duration_sum); + } + + const size_t top_count = std::min(3, top_component_stats.size()); + std::partial_sort( + top_component_stats.begin(), + top_component_stats.begin() + static_cast(top_count), + top_component_stats.end(), [](const auto& lhs, const auto& rhs) { + if (lhs.duration_sum != rhs.duration_sum) + return lhs.duration_sum > rhs.duration_sum; + return lhs.index < rhs.index; + }); + top_component_stats.resize(top_count); + return top_component_stats; + } + + std::string format_top_component_stats( + const std::vector& top_component_stats, + const WindowStats& stats) const { + auto top_components = std::string{}; + for (size_t rank = 0; rank < top_component_stats.size(); ++rank) { + if (rank != 0) + top_components += ", "; + + const auto& stat = top_component_stats[rank]; + top_components += std::format( + "{} {}ms/{}%", updating_order_[stat.index]->get_component_name(), + format_stat( + std::optional{ + duration_to_ms(stat.duration_sum) + / static_cast(stats.executed_count)}), + format_percent( + duration_to_ms(stat.duration_sum) / duration_to_ms(stats.total_update_duration) + * 100.0)); + } + return top_components; + } + + static void reset_window_stats(WindowStats& stats) { + stats.executed_count = 0; + stats.total_update_duration = SteadyClock::duration::zero(); + std::fill( + stats.component_update_duration_sums.begin(), + stats.component_update_duration_sums.end(), SteadyClock::duration::zero()); + } + + static double duration_to_ms(SteadyClock::duration duration) { + return std::chrono::duration(duration).count(); + } + + static std::string format_stat(const std::optional& value) { + if (!value.has_value()) + return "n/a"; + return std::format("{:.3f}", *value); + } + + static std::string format_percent(double value) { return std::format("{:.1f}", value); } + + static std::optional maybe_quantile( + const rmcs_utility::TDigest& digest, uint64_t sample_count, double quantile) { + if (sample_count == 0) + return std::nullopt; + return digest.quantile(quantile); + } + void enable_event_outputs() { for (const auto& component : component_list_) { for (const auto& output : component->output_list_) { diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index da4c29fb..c76a0a3a 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -43,8 +43,7 @@ int main(int argc, char** argv) { rcl_executor.add_node(executor); std::vector component_descriptions; - if (!executor->get_parameter("components", component_descriptions)) - throw std::runtime_error("Missing parameter 'components' or config is not found"); + executor->get_parameter("components", component_descriptions); std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); for (const auto& component_description : component_descriptions) { diff --git a/rmcs_ws/src/rmcs_utility/CMakeLists.txt b/rmcs_ws/src/rmcs_utility/CMakeLists.txt index e9c09fbe..b6c08a8d 100644 --- a/rmcs_ws/src/rmcs_utility/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_utility/CMakeLists.txt @@ -3,7 +3,8 @@ project(rmcs_utility) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +set(CMAKE_CXX_STANDARD_REQUIRED ON) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-O2 -Wall -Wextra -Wpedantic) @@ -14,4 +15,4 @@ ament_auto_find_build_dependencies () include_directories(${PROJECT_SOURCE_DIR}/include) -ament_auto_package() \ No newline at end of file +ament_auto_package() diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp new file mode 100644 index 00000000..0f03c859 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tdigest.hpp @@ -0,0 +1,660 @@ +#pragma once + +/* + * This file is derived from the digestible project: + * https://github.com/SpirentOrion/digestible/blob/master/include/digestible/digestible.h + * + * The digestible project provides a modern C++ implementation of a merging + * t-digest data structure under the Apache License, Version 2.0: + * https://github.com/SpirentOrion/digestible + * + * The underlying t-digest algorithm and reference implementation were originally + * authored by Ted Dunning: + * https://github.com/tdunning/t-digest + * + * As noted in the original t-digest NOTICES file: + * "The code for the t-digest was originally authored by Ted Dunning." + * "Adrien Grand contributed the heart of the AVLTreeDigest (https://github.com/jpountz)." + * + * Modifications for RMCS by Zihan Qin, 2026: + * - Changed namespace + * - Adjusted defaults + * - Formatting changes + * - Miscellaneous fixes and tweaks + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +// XXX: yes, this could be a std::pair. But being able to refer to values by name +// instead of .first and .second makes merge(), quantile(), and +// cumulative_distribution() way more readable. +template +struct Centroid { + Values mean; + Weight weight; + + Centroid(Values new_mean, Weight new_weight) + : mean(new_mean) + , weight(new_weight) {} +}; + +template +inline bool operator<(const Centroid& lhs, const Centroid& rhs) { + return (lhs.mean < rhs.mean); +} + +template +inline bool operator>(const Centroid& lhs, const Centroid& rhs) { + return (lhs.mean > rhs.mean); +} + +template +class TDigest { + using centroid_t = Centroid; + + struct TDigestImpl { + std::vector values; + Weight total_weight; + + explicit TDigestImpl(size_t size) + : total_weight(0) { + values.reserve(size); + } + + TDigestImpl() = delete; + + enum class insert_result { OK, NEED_COMPRESS }; + + insert_result insert(Values value, Weight weight) { + assert(weight); + values.emplace_back(value, weight); + total_weight += weight; + return ( + values.size() != values.capacity() ? insert_result::OK + : insert_result::NEED_COMPRESS); + } + + insert_result insert(const centroid_t& val) { return (insert(val.mean, val.weight)); } + + void reset() { + values.clear(); + total_weight = 0; + } + + size_t capacity() const { return (values.capacity()); } + }; + + TDigestImpl one; + TDigestImpl two; + + // XXX: buffer multiplier must be > 0. BUT how much greater + // will affect size vs speed balance. The effects of which + // have not been studied. Set to 2 to favor size. Informal + // benchmarks for this value yielded acceptable results. + static constexpr size_t buffer_multiplier = 2; + TDigestImpl buffer; + + TDigestImpl* active; + + Values min_val, max_val; + bool run_forward; + + void swap(TDigest&); + +public: + explicit TDigest(size_t size); + + TDigest() = delete; + + TDigest(const TDigest&); + TDigest(TDigest&&) noexcept; + TDigest& operator=(TDigest); + + /** + * Inserts the given value into the t-digest input buffer. + * Assumes a weight of 1 for the sample. + * If this operation fills the input buffer an automatic merge() + * operation is triggered. + * + * @param value + * value to insert + */ + void insert(Values value) { insert(value, 1); } + + /** + * Inserts the given value with the given weight into the t-digest input buffer. + * If this operation fills the input buffer an automatic merge() + * operation is triggered. + * + * @param value + * value to insert + * + * @param weight + * weight associated with the provided value + */ + void insert(Values value, Weight weight) { + if (!weight) + throw std::invalid_argument("TDigest weight must be positive."); + + const auto insert_result = buffer.insert(value, weight); + min_val = std::min(min_val, value); + max_val = std::max(max_val, value); + + if (insert_result == TDigestImpl::insert_result::NEED_COMPRESS) { + merge(); + } + } + + /** + * Insert and merge an existing t-digest. + * + * Assumes `src` has already been flushed/merged and contains no pending + * buffered samples. This function only imports merged centroids from `src`. + * + * @param src + * source t-digest to copy values from + */ + void insert(const TDigest& src); + + /** + * Reset internal state of the t-digest. + * + * Clears t-digest, incoming data buffer, and resets min/max values. + */ + void reset(); + + /** + * Retrieve contents of the t-digest. + * + * @return + * list of pairs sorted by value in ascending order. + */ + std::vector> get() const; + + /** + * Merge incoming data into the t-digest. + */ + void merge(); + + /** + * Retrieve the probability that a random data point in the digest is + * less than or equal to x. + * + * @param x + * value of interest + * + * @return + * a value between [0, 1] + */ + double cumulative_distribution(Values x) const; + + /** + * Retrieve the value such that p percent of all data points or less than or + * equal to that value. + * + * @param p + * percentile of interest; valid input between [0.0, 100.0] + * + * @return + * a value from the dataset that satisfies the above condition + */ + double quantile(double p) const; + + /** + * Return number of centroids in the t-digest + */ + size_t centroid_count() const { return ((*active).values.size()); } + + /** + * Retrieve the number of merged data points in the t-digest + * + * @return + * the total weight of all data + */ + size_t size() const { return ((*active).total_weight); } + + /** + * Retrieve maximum value seen by this t-digest. + * Value is cleared by reset(). + * + * @return + * maximum value seen by this t-digest. + */ + Values max() const { return (max_val); } + + /** + * Retrieve minimum value seen by this t-digest. + * Value is cleared by reset(). + * + * @return + * minimum value seen by this t-digest. + */ + Values min() const { return (min_val); } +}; + +template +TDigest::TDigest(size_t size) + : one(size) + , two(size) + , buffer(size * buffer_multiplier) + , active(&one) + , min_val(std::numeric_limits::max()) + , max_val(std::numeric_limits::lowest()) + , run_forward(true) { + if (size == 0) + throw std::invalid_argument("TDigest size must be positive."); +} + +template +TDigest::TDigest(const TDigest& other) + : one(other.one) + , two(other.two) + , buffer(other.buffer) + , active(other.active == &other.one ? &one : &two) + , min_val(other.min_val) + , max_val(other.max_val) + , run_forward(other.run_forward) {} + +template +void TDigest::swap(TDigest& other) { + const bool this_active_one = (active == &one); + const bool other_active_one = (other.active == &other.one); + + std::swap(one, other.one); + std::swap(two, other.two); + std::swap(buffer, other.buffer); + std::swap(min_val, other.min_val); + std::swap(max_val, other.max_val); + std::swap(run_forward, other.run_forward); + + active = other_active_one ? &one : &two; + other.active = this_active_one ? &other.one : &other.two; +} + +template +TDigest::TDigest(TDigest&& other) noexcept + : TDigest(other.one.capacity()) { + swap(other); +} + +template +TDigest& TDigest::operator=(TDigest other) { + swap(other); + return (*this); +} + +template +void TDigest::insert(const TDigest& src) { + max_val = std::max(max_val, src.max_val); + min_val = std::min(min_val, src.min_val); + + auto insert_fn = [this](const auto& val) { + if (buffer.insert(val) == TDigestImpl::insert_result::NEED_COMPRESS) { + merge(); + } + }; + + std::for_each(src.active->values.begin(), src.active->values.end(), insert_fn); + // Explicitly merge any unmerged data for a consistent end state. + merge(); +} + +template +void TDigest::reset() { + one.reset(); + two.reset(); + buffer.reset(); + active = &one; + min_val = std::numeric_limits::max(); + max_val = std::numeric_limits::lowest(); +} + +template +std::vector> TDigest::get() const { + std::vector> to_return; + + std::transform( + active->values.begin(), active->values.end(), std::back_inserter(to_return), + [](const centroid_t& val) { return (std::make_pair(val.mean, val.weight)); }); + + return (to_return); +} + +/** + * When taken together the following 4 functions (Z, normalizer_fn, k, q) + * comprise the scaling function for t-digests. + */ + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double Z(double compression, double n) { return (4 * log(n / compression) + 24); } + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double normalizer_fn(double compression, double n) { + return (compression / Z(compression, n)); +} + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double k(double q, double normalizer) { + const double q_min = 1e-15; + const double q_max = 1 - q_min; + if (q < q_min) { + return (2 * k(q_min, normalizer)); + } else if (q > q_max) { + return (2 * k(q_max, normalizer)); + } + + return (log(q / (1 - q)) * normalizer); +} + +/** + * C++ translation of the "k_2" version found in the reference implementation + * available here: https://github.com/tdunning/t-digest + */ +inline double q(double k, double normalizer) { + double w = exp(k / normalizer); + return (w / (1 + w)); +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +void TDigest::merge() { + auto& inactive = (&one == active) ? two : one; + + // buffer.values is pre-seeded with the current digest after each merge, so the + // presence of centroids here does not mean there are new samples to merge. + if (buffer.total_weight == 0) { + return; + } + + auto& inputs = buffer.values; + + if (run_forward) { + std::sort(inputs.begin(), inputs.end(), std::less()); + } else { + std::sort(inputs.begin(), inputs.end(), std::greater()); + } + + const Weight new_total_weight = buffer.total_weight + active->total_weight; + const double normalizer = normalizer_fn(inactive.values.capacity(), new_total_weight); + double k1 = k(0, normalizer); + double next_q_limit_weight = new_total_weight * q(k1 + 1, normalizer); + + double weight_so_far = 0; + double weight_to_add = inputs.front().weight; + double mean_to_add = inputs.front().mean; + + auto compress_fn = [&inactive, new_total_weight, &k1, normalizer, &next_q_limit_weight, + &weight_so_far, &weight_to_add, &mean_to_add](const centroid_t& current) { + if ((weight_so_far + weight_to_add + current.weight) <= next_q_limit_weight) { + weight_to_add += current.weight; + assert(weight_to_add); + mean_to_add = + mean_to_add + (current.mean - mean_to_add) * current.weight / weight_to_add; + + } else { + weight_so_far += weight_to_add; + + double new_q = + static_cast(weight_so_far) / static_cast(new_total_weight); + k1 = k(new_q, normalizer); + next_q_limit_weight = new_total_weight * q(k1 + 1, normalizer); + + if constexpr (std::is_integral::value) { + mean_to_add = std::round(mean_to_add); + } + inactive.insert(mean_to_add, weight_to_add); + mean_to_add = current.mean; + weight_to_add = current.weight; + } + }; + + std::for_each(inputs.begin() + 1, inputs.end(), compress_fn); + + if (weight_to_add != 0) { + inactive.insert(mean_to_add, weight_to_add); + } + + if (!run_forward) { + std::sort(inactive.values.begin(), inactive.values.end()); + } + run_forward = !run_forward; + + buffer.reset(); + + // Seed buffer with the current t-digest in preparation for the next + // merge. + inputs.assign(inactive.values.begin(), inactive.values.end()); + + auto new_inactive = active; + active = &inactive; + new_inactive->reset(); +} + +/** + * XXX: replace with std::lerp when C++20 support becomes available. + * This version adapted from libcxx, which is part of the LLVM project + * under an "Apache 2.0 license with LLVM Exceptions." The project can be + * found at https://github.com/llvm-mirror/libcxx + */ +inline double lerp(double a, double b, double t) noexcept { + if ((a <= 0 && b >= 0) || (a >= 0 && b <= 0)) + return t * b + (1 - t) * a; + + if (t == 1) + return b; + const double x = a + t * (b - a); + if ((t > 1) == (b > a)) + return b < x ? x : b; + else + return x < b ? x : b; +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +double TDigest::quantile(double p) const { + if (p < 0 || p > 100) { + throw std::out_of_range("Requested quantile must be between 0 and 100."); + } + + if (active->values.empty()) { + return (0); + } else if (active->values.size() == 1) { + return (active->values.front().mean); + } + + const double total_weight = static_cast(active->total_weight); + const double index = (p / 100.0) * total_weight; + + if (index < 1.0) { + return (min_val); + } + + // For smaller quantiles, interpolate between minimum value and the first + // centroid. + const auto& first = active->values.front(); + const double first_half_weight = static_cast(first.weight) / 2.0; + if (first.weight > 1 && first_half_weight > 1.0 && index < first_half_weight) { + return (lerp(min_val, first.mean, (index - 1.0) / (first_half_weight - 1.0))); + } + + if (index > total_weight - 1.0) { + return (max_val); + } + + // For larger quantiles, interpolate between maximum value and the last + // centroid. + const auto& last = active->values.back(); + const double last_half_weight = static_cast(last.weight) / 2.0; + if (last.weight > 1 && last_half_weight > 1.0 && total_weight - index <= last_half_weight) { + return ( + max_val + - (total_weight - index - 1.0) / (last_half_weight - 1.0) * (max_val - last.mean)); + } + + double weight_so_far = first_half_weight; + double quantile = 0.0; + auto quantile_fn = [index, &weight_so_far, + &quantile](const centroid_t& left, const centroid_t& right) { + const double delta_weight = + (static_cast(left.weight) + static_cast(right.weight)) / 2.0; + if (weight_so_far + delta_weight > index) { + const double lower = index - weight_so_far; + const double upper = weight_so_far + delta_weight - index; + + quantile = (left.mean * upper + right.mean * lower) / (lower + upper); + + return (true); + } + + weight_so_far += delta_weight; + return (false); + }; + + // Even though we're using adjacent_find here, we don't actually intend to find + // anything. We just want to iterate over pairs of centroids until we calculate + // the quantile. + auto it = std::adjacent_find(active->values.begin(), active->values.end(), quantile_fn); + + // Did we fail to find a pair of bracketing centroids? + if (it == active->values.end()) { + // Must be between max_val and the last centroid. + return active->values.back().mean; + } + + return (quantile); +} + +/** + * Based on the equivalent function in the reference implementation available here: + * https://github.com/tdunning/t-digest + */ +template +double TDigest::cumulative_distribution(Values x) const { + if (active->values.empty()) { + return (1.0); + } + + if (active->values.size() == 1) { + if (x < min_val) { + return (0); + } + if (x > max_val) { + return (1.0); + } + if (max_val == min_val) { + return (0.5); + } + return ((x - min_val) / (max_val - min_val)); + } + + // From here on out we divide by active->total_weight in multiple places + // along several code paths. + // Let's make sure we're not going to divide by zero. + assert(active->total_weight); + const double total_weight = static_cast(active->total_weight); + + // Is x at one of the extremes? + if (x < active->values.front().mean) { + const auto& first = active->values.front(); + if (first.mean - min_val > 0) { + if (x == min_val) + return (0.5 / total_weight); + + const double first_half_weight = static_cast(first.weight) / 2.0; + return ( + (1.0 + (x - min_val) / (first.mean - min_val) * (first_half_weight - 1.0)) + / total_weight); + } + return (0); + } + if (x > active->values.back().mean) { + const auto& last = active->values.back(); + if (max_val - last.mean > 0) { + if (x == max_val) + return (1.0 - 0.5 / total_weight); + + const double last_half_weight = static_cast(last.weight) / 2.0; + const double dq = + (1.0 + (max_val - x) / (max_val - last.mean) * (last_half_weight - 1.0)) + / total_weight; + return (1.0 - dq); + } + return (1.0); + } + + double weight_so_far = 0.0; + double cdf = 0.0; + auto cdf_fn = [x, &weight_so_far, &cdf, + total_weight](const centroid_t& left, const centroid_t& right) { + assert(total_weight); + if (left.mean <= x && x < right.mean) { + // x is bracketed between left and right. + + const double delta_weight = + (static_cast(right.weight) + static_cast(left.weight)) / 2.0; + const double base = weight_so_far + static_cast(left.weight) / 2.0; + + cdf = lerp( + base, base + delta_weight, + static_cast((x - left.mean)) / (right.mean - left.mean)) + / total_weight; + return (true); + } + + weight_so_far += static_cast(left.weight); + return (false); + }; + + auto it = std::adjacent_find(active->values.begin(), active->values.end(), cdf_fn); + + // Did we fail to find a pair of bracketing centroids? + if (it == active->values.end()) { + // Might be between max_val and the last centroid. + if (x == active->values.back().mean) { + return ((1.0 - 0.5 / total_weight)); + } + } + + return (cdf); +} + +} // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp new file mode 100644 index 00000000..2192db5a --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/thread_config.hpp @@ -0,0 +1,318 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +/** + * Parsed thread configuration. + * + * Supported fields in `spec` are `name`, `cpus`, `policy`, `priority`, and `nice`. + * Format: `key=value;key=value;...` + * Empty specs are allowed and represent a no-op configuration. + * Examples: + * - `name=rmcs-ctrl;cpus=3;policy=fifo;priority=80` + * - `cpus=4-7;policy=other;nice=-5` + */ +class ThreadConfig { +public: + /** + * Parse a thread configuration spec. + * + * @param spec Thread configuration spec string. + * @throws std::invalid_argument If the spec is malformed or contains invalid values. + */ + explicit ThreadConfig(std::string_view spec) { + spec = trim(spec); + parse_fields(spec); + validate(spec); + } + + /** + * Parse a thread configuration spec and fill in a default thread name when `name` is unset. + * + * @param spec Thread configuration spec string. + * @param default_name Fallback name used only when `spec` does not configure `name`. + * @throws std::invalid_argument If the spec is malformed, contains invalid values, or + * `default_name` is invalid when used. + */ + ThreadConfig(std::string_view spec, std::string_view default_name) + : ThreadConfig(spec) { + if (name_) + return; + if (const auto invalid_reason = check_name(default_name)) + throw std::invalid_argument(std::string(*invalid_reason)); + name_ = to_string(default_name); + } + + [[nodiscard]] auto name() const -> const std::optional& { return name_; } + [[nodiscard]] auto cpus() const -> const std::optional& { return cpus_; } + [[nodiscard]] auto policy() const -> const std::optional& { return policy_; } + [[nodiscard]] auto priority() const -> const std::optional& { return priority_; } + [[nodiscard]] auto nice() const -> const std::optional& { return nice_; } + + /** + * Apply the stored configuration to the calling thread. + * + * This operation is not atomic. If a later step fails, earlier changes may already have been + * applied to the current thread. + * + * @return `std::expected{}` on success, or an error string describing the + * failing runtime step. + */ + auto apply_to_current_thread() const -> std::expected { + const auto current_thread = pthread_self(); + + if (policy_) { + sched_param param{}; + param.sched_priority = priority_.value_or(0); + const int error_code = pthread_setschedparam(current_thread, *policy_, ¶m); + if (error_code != 0) { + return std::unexpected( + std::format( + "Failed to set thread scheduling policy: {}", std::strerror(error_code))); + } + } + + if (nice_) { + errno = 0; + const auto tid = static_cast(syscall(SYS_gettid)); + if (setpriority(PRIO_PROCESS, tid, *nice_) != 0) + return std::unexpected( + std::format("Failed to set thread nice value: {}", std::strerror(errno))); + } + + if (cpus_) { + const int error_code = + pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &*cpus_); + if (error_code != 0) + return std::unexpected( + std::format("Failed to set thread affinity: {}", std::strerror(error_code))); + } + + if (name_) { + const int error_code = pthread_setname_np(current_thread, name_->c_str()); + if (error_code != 0) + return std::unexpected( + std::format("Failed to set thread name: {}", std::strerror(error_code))); + } + + return {}; + } + +private: + static auto trim(std::string_view text) -> std::string_view { + while (!text.empty() && (text.front() == ' ' || text.front() == '\t')) + text.remove_prefix(1); + while (!text.empty() && (text.back() == ' ' || text.back() == '\t')) + text.remove_suffix(1); + return text; + } + + static auto to_string(std::string_view text) -> std::string { return std::string{text}; } + + [[noreturn]] static void throw_invalid_spec(std::string_view reason, std::string_view spec) { + throw std::invalid_argument( + std::format("Invalid thread config spec ({}): \"{}\"", reason, spec)); + } + + static auto parse_int(std::string_view value, std::string_view key, std::string_view spec) + -> int { + int result = 0; + const auto* begin = value.data(); + const auto* end = value.data() + value.size(); + const auto [ptr, error_code] = std::from_chars(begin, end, result); + if (error_code != std::errc{} || ptr != end) { + throw_invalid_spec(std::format("invalid integer for key '{}'", key), spec); + } + return result; + } + + static auto check_name(std::string_view name) -> std::optional { + if (name.empty()) + return "Thread name must not be empty"; + if (name.size() > 15) + return std::format("Thread name exceeds 15 characters: \"{}\"", name); + return std::nullopt; + } + + void parse_fields(std::string_view spec) { + bool seen_name = false; + bool seen_cpus = false; + bool seen_policy = false; + bool seen_priority = false; + bool seen_nice = false; + + auto remaining_spec = spec; + while (true) { + const auto separator = remaining_spec.find(';'); + const auto field = trim(remaining_spec.substr(0, separator)); + + if (!field.empty()) { + const auto equals = field.find('='); + if (equals == std::string_view::npos) + throw_invalid_spec("missing '='", spec); + + const auto key = trim(field.substr(0, equals)); + const auto value = trim(field.substr(equals + 1)); + if (key.empty() || value.empty()) + throw_invalid_spec("empty key or value", spec); + + assign_field( + key, value, spec, seen_name, seen_cpus, seen_policy, seen_priority, seen_nice); + } + + if (separator == std::string_view::npos) + break; + remaining_spec.remove_prefix(separator + 1); + } + } + + void assign_field( + std::string_view key, std::string_view value, std::string_view spec, bool& seen_name, + bool& seen_cpus, bool& seen_policy, bool& seen_priority, bool& seen_nice) { + if (key == "name") { + if (seen_name) + throw_invalid_spec("duplicate key 'name'", spec); + seen_name = true; + if (const auto invalid_reason = check_name(value)) + throw_invalid_spec(*invalid_reason, spec); + name_ = to_string(value); + return; + } + if (key == "cpus") { + if (seen_cpus) + throw_invalid_spec("duplicate key 'cpus'", spec); + seen_cpus = true; + cpus_ = parse_cpu_list(value, spec); + return; + } + if (key == "policy") { + if (seen_policy) + throw_invalid_spec("duplicate key 'policy'", spec); + seen_policy = true; + policy_ = parse_policy(value, spec); + return; + } + if (key == "priority") { + if (seen_priority) + throw_invalid_spec("duplicate key 'priority'", spec); + seen_priority = true; + priority_ = parse_int(value, key, spec); + return; + } + if (key == "nice") { + if (seen_nice) + throw_invalid_spec("duplicate key 'nice'", spec); + seen_nice = true; + nice_ = parse_int(value, key, spec); + return; + } + throw_invalid_spec("unknown key '" + to_string(key) + "'", spec); + } + + static auto parse_policy(std::string_view value, std::string_view spec) -> int { + if (value == "other") + return SCHED_OTHER; + if (value == "batch") + return SCHED_BATCH; + if (value == "idle") + return SCHED_IDLE; + if (value == "fifo") + return SCHED_FIFO; + if (value == "rr") + return SCHED_RR; + throw_invalid_spec("unknown policy", spec); + } + + static auto parse_cpu_list(std::string_view value, std::string_view spec) -> cpu_set_t { + cpu_set_t cpus; + CPU_ZERO(&cpus); + + bool has_any_cpu = false; + while (true) { + const auto separator = value.find(','); + const auto token = trim(value.substr(0, separator)); + if (token.empty()) + throw_invalid_spec("empty cpu list token", spec); + + const auto dash = token.find('-'); + if (dash == std::string_view::npos) { + const int cpu = parse_int(token, "cpus", spec); + set_cpu_range(cpus, cpu, cpu, spec); + } else { + if (token.find('-', dash + 1) != std::string_view::npos) + throw_invalid_spec("invalid cpu range token", spec); + const int first_cpu = parse_int(trim(token.substr(0, dash)), "cpus", spec); + const int last_cpu = parse_int(trim(token.substr(dash + 1)), "cpus", spec); + set_cpu_range(cpus, first_cpu, last_cpu, spec); + } + + has_any_cpu = true; + if (separator == std::string_view::npos) + break; + value.remove_prefix(separator + 1); + } + + if (!has_any_cpu) + throw_invalid_spec("empty cpu list", spec); + return cpus; + } + + static void set_cpu_range(cpu_set_t& cpus, int first_cpu, int last_cpu, std::string_view spec) { + if (first_cpu < 0 || last_cpu < 0 || first_cpu > last_cpu) + throw_invalid_spec("invalid cpu range", spec); + if (last_cpu >= CPU_SETSIZE) + throw_invalid_spec("cpu index exceeds CPU_SETSIZE", spec); + + for (int cpu = first_cpu; cpu <= last_cpu; ++cpu) + CPU_SET(cpu, &cpus); + } + + void validate(std::string_view spec) const { + if (priority_ && !policy_) + throw_invalid_spec("priority requires policy", spec); + + if (policy_) { + const bool is_realtime = *policy_ == SCHED_FIFO || *policy_ == SCHED_RR; + if (is_realtime) { + if (!priority_) + throw_invalid_spec("realtime policy requires priority", spec); + if (nice_) + throw_invalid_spec("realtime policy cannot use nice", spec); + + const int min_priority = sched_get_priority_min(*policy_); + const int max_priority = sched_get_priority_max(*policy_); + if (*priority_ < min_priority || *priority_ > max_priority) + throw_invalid_spec("priority out of range for policy", spec); + } else if (priority_) { + throw_invalid_spec("non-realtime policy cannot use priority", spec); + } + } + + if (nice_ && (*nice_ < -20 || *nice_ > 19)) + throw_invalid_spec("nice out of range", spec); + } + + std::optional name_; + std::optional cpus_; + std::optional policy_; + std::optional priority_; + std::optional nice_; +}; + +} // namespace rmcs_utility