From b6f4ff133578efae9979c4f02deca45d7711e0e7 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Tue, 11 Aug 2026 16:42:11 +0200 Subject: [PATCH 01/11] control_layer: reading and publishing unified new ControlSignal msgs --- .../include/dls2/controller/control_modes.hpp | 10 ++ .../include/dls2/controller/controller.hpp | 1 + .../dls2/controller/controller_data.hpp | 1 + .../dls2/core_framework/control_layer.hpp | 8 +- .../core_framework/src/control_layer.cpp.in | 107 +++++++++++++++--- modules/plugin/test/CMakeLists.txt | 1 + .../test/include/hello_world_plugin.hpp | 1 + .../plugin/test/src/hello_world_plugin.cpp | 15 +-- 8 files changed, 118 insertions(+), 26 deletions(-) create mode 100644 modules/controller/include/dls2/controller/control_modes.hpp diff --git a/modules/controller/include/dls2/controller/control_modes.hpp b/modules/controller/include/dls2/controller/control_modes.hpp new file mode 100644 index 00000000..22969816 --- /dev/null +++ b/modules/controller/include/dls2/controller/control_modes.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include + +enum class ControlMode : uint8_t { + TORQUE_MODE = 0, + IMPEDANCE_MODE, + POSITION_MODE, + VELOCITY_MODE +}; diff --git a/modules/controller/include/dls2/controller/controller.hpp b/modules/controller/include/dls2/controller/controller.hpp index 105558ae..185ebcea 100644 --- a/modules/controller/include/dls2/controller/controller.hpp +++ b/modules/controller/include/dls2/controller/controller.hpp @@ -6,6 +6,7 @@ // ============================================================================= #include "dls2/application/periodic_app.hpp" #include "dls2/controller/controller_data.hpp" +#include "dls2/controller/control_modes.hpp" #include "dls2/util/messaging/dds_participant.hpp" diff --git a/modules/controller/include/dls2/controller/controller_data.hpp b/modules/controller/include/dls2/controller/controller_data.hpp index 5b00badb..f13b4b2a 100644 --- a/modules/controller/include/dls2/controller/controller_data.hpp +++ b/modules/controller/include/dls2/controller/controller_data.hpp @@ -5,6 +5,7 @@ #include "dls2/signal/reader.hpp" #include "dls2/math/ramp.hpp" #include "robotlib/robot_base.hpp" +#include "dls2/controller/control_modes.hpp" #include diff --git a/modules/core_framework/include/dls2/core_framework/control_layer.hpp b/modules/core_framework/include/dls2/core_framework/control_layer.hpp index 1fc27ba0..22ed9bd0 100644 --- a/modules/core_framework/include/dls2/core_framework/control_layer.hpp +++ b/modules/core_framework/include/dls2/core_framework/control_layer.hpp @@ -79,7 +79,9 @@ class ControlLayer : public Layer /// @return true if the generator unloads correctly. bool unloadMotionGenerator(const std::string&); - /// Returns the last published desired torques + /// Returns the last published joint reference + std::vector getPublishedDesiredPosition(); + std::vector getPublishedDesiredVelocity(); std::vector getPublishedDesiredTorques(); private: @@ -118,9 +120,9 @@ class ControlLayer : public Layer std::shared_ptr pRobot; /// @brief Output signals - Writer writer_control_signal; + Writer writer_control_signal; - dls2_interface::msg::DesiredTorques torques; + dls2_interface::msg::ControlSignal control_signal_msg; bool unloadController(std::shared_ptr pData); diff --git a/modules/core_framework/src/control_layer.cpp.in b/modules/core_framework/src/control_layer.cpp.in index 7b953381..2effc374 100644 --- a/modules/core_framework/src/control_layer.cpp.in +++ b/modules/core_framework/src/control_layer.cpp.in @@ -57,11 +57,21 @@ ControlLayer::ControlLayer(std::string ID, std::string robot_name) , pRobot(robotlib::RobotFactory::openRobot(robot_name)) , writer_control_signal( ddsSignalLink, - dls::topics::desired_torques) - , torques() + dls::topics::control_signal) + , control_signal_msg() { - writer_control_signal.msg.desired_torques().resize(pRobot->getNJOINTS()); - torques.desired_torques().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_position().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_velocity().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.joints_torques().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.kp().resize(pRobot->getNJOINTS()); + writer_control_signal.msg.kd().resize(pRobot->getNJOINTS()); + + control_signal_msg.joints_position().resize(pRobot->getNJOINTS()); + control_signal_msg.joints_velocity().resize(pRobot->getNJOINTS()); + control_signal_msg.joints_torques().resize(pRobot->getNJOINTS()); + control_signal_msg.kp().resize(pRobot->getNJOINTS()); + control_signal_msg.kd().resize(pRobot->getNJOINTS()); + command_manager.addCommand ( "loadGenerator", @@ -238,18 +248,77 @@ void *ControlLayer::controlSignalGather(void *data) auto epoch = std::chrono::time_point_cast(now).time_since_epoch(); // Read the control signals - std::fill(layer->torques.desired_torques().begin(), layer->torques.desired_torques().end(), 0); - - for(auto pair : layer->controllers) + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + layer->control_signal_msg.control_mode() = static_cast(ControlMode::TORQUE_MODE); + bool active_position_controller = false; + bool active_velocity_controller = false; + bool active_impedence_controller = false; + bool control_mode_set_once = false; + + for(auto [_, controller_data] : layer->controllers) { - pair.second->reader_control_signal.read(); - for(long unsigned int i=0; ireader_control_signal.msg.torques().size();i++){ - layer->torques.desired_torques()[i] += pair.second->premultiplier * pair.second->reader_control_signal.msg.torques()[i]; + controller_data->reader_control_signal.read(); + const auto& controller_msg = controller_data->reader_control_signal.msg; + const auto controller_mode = static_cast(controller_msg.control_mode()); + + if(!control_mode_set_once){ + layer->control_signal_msg.control_mode() = controller_msg.control_mode(); + control_mode_set_once = true; + } + + const auto aggregate_mode = static_cast(layer->control_signal_msg.control_mode()); + bool torque_controller_in_torque_mode = controller_mode == ControlMode::TORQUE_MODE && aggregate_mode == ControlMode::TORQUE_MODE; + bool torque_controller_in_impedence_mode = aggregate_mode == ControlMode::IMPEDANCE_MODE && controller_mode == ControlMode::TORQUE_MODE; + bool first_impedence_controller = controller_mode == ControlMode::IMPEDANCE_MODE && !active_impedence_controller; + bool first_position_controller = controller_mode == ControlMode::POSITION_MODE && !active_position_controller; + bool first_velocity_controller = controller_mode == ControlMode::VELOCITY_MODE && !active_velocity_controller; + + + if(first_position_controller){ + layer->control_signal_msg.control_mode() = static_cast(ControlMode::POSITION_MODE); + layer->control_signal_msg.joints_position() = controller_msg.joints_position(); + + // Reset all other fields to zero + std::fill(layer->control_signal_msg.joints_velocity().begin(), layer->control_signal_msg.joints_velocity().end(), 0); + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0); + std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0); + break; + } + + if(first_velocity_controller){ + layer->control_signal_msg.control_mode() = static_cast(ControlMode::VELOCITY_MODE); + layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity(); + + // Reset all other fields to zero + std::fill(layer->control_signal_msg.joints_position().begin(), layer->control_signal_msg.joints_position().end(), 0); + std::fill(layer->control_signal_msg.joints_torques().begin(), layer->control_signal_msg.joints_torques().end(), 0); + std::fill(layer->control_signal_msg.kp().begin(), layer->control_signal_msg.kp().end(), 0.0); + std::fill(layer->control_signal_msg.kd().begin(), layer->control_signal_msg.kd().end(), 0.0); + break; + } + + + if(torque_controller_in_torque_mode || torque_controller_in_impedence_mode || first_impedence_controller){ + if(first_impedence_controller){ + active_impedence_controller = true; + layer->control_signal_msg.control_mode() = static_cast(ControlMode::IMPEDANCE_MODE); + layer->control_signal_msg.joints_position() = controller_msg.joints_position(); + layer->control_signal_msg.joints_velocity() = controller_msg.joints_velocity(); + layer->control_signal_msg.kp() = controller_msg.kp(); + layer->control_signal_msg.kd() = controller_msg.kd(); + } + + for(size_t i = 0; i < controller_msg.joints_torques().size();i++){ + // Torque contributions summed up + layer->control_signal_msg.joints_torques()[i] += controller_data->premultiplier * controller_msg.joints_torques()[i]; + } } } - // Send the desired torques to HAL - layer->writer_control_signal.msg.desired_torques() = layer->saturateTorques(layer->torques.desired_torques()); + // Fill in the control signal to be sent to HAL + layer->control_signal_msg.joints_torques() = layer->saturateTorques(layer->control_signal_msg.joints_torques()); + layer->writer_control_signal.msg = layer->control_signal_msg; layer->writer_control_signal.msg.timestamp() = std::chrono::duration_cast(epoch).count(); layer->writer_control_signal.publish(); @@ -263,7 +332,6 @@ void *ControlLayer::controlSignalGather(void *data) { std::stringstream ss; ss << "Control Layer is probably breaking real-time. Has period of 1 mseconds but spent: " << mean << " useconds "; - // ss << "Control layer published torques " << desired_torques.transpose(); // << std::endl; layer->app_logger.warning(ss.str(), EventID::WRONG_PROCESS_FREQUENCY); } @@ -591,7 +659,14 @@ std::vector ControlLayer::saturateTorques(const std::vector& req return req; } +std::vector ControlLayer::getPublishedDesiredPosition(){ + return this->writer_control_signal.msg.joints_position(); +} + +std::vector ControlLayer::getPublishedDesiredVelocity(){ + return this->writer_control_signal.msg.joints_velocity(); +} + std::vector ControlLayer::getPublishedDesiredTorques(){ - // std::lock_guard lock(this->last_published_desired_torquesmutex); - return this->writer_control_signal.msg.desired_torques(); -} \ No newline at end of file + return this->writer_control_signal.msg.joints_torques(); +} diff --git a/modules/plugin/test/CMakeLists.txt b/modules/plugin/test/CMakeLists.txt index 7ca08a61..91824ac6 100644 --- a/modules/plugin/test/CMakeLists.txt +++ b/modules/plugin/test/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(${PLUGIN_NAME} dls_signal dls_messages dls_topics + dls_controller periodic_app_plugin ) target_include_directories(${PLUGIN_NAME} diff --git a/modules/plugin/test/include/hello_world_plugin.hpp b/modules/plugin/test/include/hello_world_plugin.hpp index 17c8d534..c3173652 100644 --- a/modules/plugin/test/include/hello_world_plugin.hpp +++ b/modules/plugin/test/include/hello_world_plugin.hpp @@ -7,6 +7,7 @@ #include "dls2/signal/reader.hpp" #include "dls2/signal/writer.hpp" // ****************************************** +#include "dls2/controller/control_modes.hpp" // plugin loaded at run-time class HelloWorldPlugin : public dls::PeriodicAppPlugin diff --git a/modules/plugin/test/src/hello_world_plugin.cpp b/modules/plugin/test/src/hello_world_plugin.cpp index e201c6db..7a6213a5 100644 --- a/modules/plugin/test/src/hello_world_plugin.cpp +++ b/modules/plugin/test/src/hello_world_plugin.cpp @@ -5,7 +5,8 @@ HelloWorldPlugin::HelloWorldPlugin(const std::string& ID) : dls::PeriodicAppPlugin(ID){ reader_bs = buildInput(dls::topics::low_level_estimation::blind_state, [](){}, false); // false: not required on activation writer_cs = buildOutput(dls::topics::control_signal); - writer_cs->msg.torques().resize(12); + writer_cs->msg.joints_torques().resize(12); + writer_cs->msg.control_mode() = static_cast(ControlMode::TORQUE_MODE); //define console functions here if needed command_manager.addCommand("set_joint_torque", @@ -18,12 +19,12 @@ HelloWorldPlugin::~HelloWorldPlugin(){} void HelloWorldPlugin::run(const std::chrono::system_clock::time_point &time){ read(); - for (size_t i=0; imsg.torques().size(); i++){ - writer_cs->msg.torques()[i] += i; + for (size_t i=0; imsg.joints_torques().size(); i++){ + writer_cs->msg.joints_torques()[i] += i; } std::cout << "[" << std::chrono::duration_cast(time.time_since_epoch()).count() - << "]: Received joint size " << reader_bs->msg.joints_position().size() << "; publishing dummy torques...\n"; + << "]: Received joint size " << reader_bs->msg.joints_position().size() << "; publishing dummy joints_torques...\n"; write(); } @@ -33,9 +34,9 @@ bool HelloWorldPlugin::setJointTorque(){ int idx{0}; dls::CommandHelper::readValue("Joint_ID", idx); double tau{0.0}; - if(dls::CommandHelper::readValue("torque", tau, writer_cs->msg.torques()[idx])) + if(dls::CommandHelper::readValue("torque", tau, writer_cs->msg.joints_torques()[idx])) { - writer_cs->msg.torques()[idx] = tau; + writer_cs->msg.joints_torques()[idx] = tau; } return true; } @@ -48,4 +49,4 @@ extern "C" PeriodicAppPlugin *create(const std::string& ID, const std::string& n } extern "C" void destroy(PeriodicAppPlugin *p){ delete p; -} \ No newline at end of file +} From 0bdd1126250e503f832b105b4b22a6585f2383ab Mon Sep 17 00:00:00 2001 From: mich-pest Date: Thu, 13 Aug 2026 16:09:03 +0200 Subject: [PATCH 02/11] utils: waypointer added --- modules/utils/CMakeLists.txt | 7 +- .../utils/include/dls2/util/waypointer.hpp | 47 +++++++ modules/utils/src/waypointer.cpp | 87 +++++++++++++ modules/utils/test/CMakeLists.txt | 18 +++ modules/utils/test/test_waypointer.cpp | 122 ++++++++++++++++++ 5 files changed, 280 insertions(+), 1 deletion(-) create mode 100644 modules/utils/include/dls2/util/waypointer.hpp create mode 100644 modules/utils/src/waypointer.cpp create mode 100644 modules/utils/test/CMakeLists.txt create mode 100644 modules/utils/test/test_waypointer.cpp diff --git a/modules/utils/CMakeLists.txt b/modules/utils/CMakeLists.txt index 2d11856a..647af2f2 100644 --- a/modules/utils/CMakeLists.txt +++ b/modules/utils/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(dls_utils ${CMAKE_CURRENT_BINARY_DIR}/src/utils.cpp src/pose.cpp src/screw.cpp + src/waypointer.cpp src/process_resource_monitor.cpp src/system_resource_monitor.cpp ) @@ -26,4 +27,8 @@ target_include_directories(dls_utils include ) -dls_install(dls_utils) \ No newline at end of file +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +dls_install(dls_utils) diff --git a/modules/utils/include/dls2/util/waypointer.hpp b/modules/utils/include/dls2/util/waypointer.hpp new file mode 100644 index 00000000..dcdba9b0 --- /dev/null +++ b/modules/utils/include/dls2/util/waypointer.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace dls +{ + namespace utils + { + using Waypoint = std::pair; + + class Waypointer + { + public: + Waypointer() = default; + + bool init(const std::vector& path); + virtual bool run(const Waypoint& robot_pose, Waypoint& waypoint) = 0; + + protected: + std::vector path_; + }; + + class EuclideanWaypointer : public Waypointer + { + public: + EuclideanWaypointer(std::string config_path); + EuclideanWaypointer(const YAML::Node& config); + + bool init(const std::vector& path); + bool run(const Waypoint& robot_pose, Waypoint& waypoint) override; + std::pair closestWaypoint(const Waypoint& robot_pose); + + private: + bool panic_ { false }; + double panic_threshold_ { 10.0 }; + + bool monotonic_{ true }; + size_t last_min_dist_idx_ { 0 }; + size_t lookahead_ { 0 }; + }; + } // namespace utils +} // namespace dls diff --git a/modules/utils/src/waypointer.cpp b/modules/utils/src/waypointer.cpp new file mode 100644 index 00000000..379bfbc7 --- /dev/null +++ b/modules/utils/src/waypointer.cpp @@ -0,0 +1,87 @@ +#include "dls2/util/waypointer.hpp" + +namespace dls +{ + namespace utils + { + bool Waypointer::init(const std::vector& path){ + if(path.empty()){ + return false; + } + path_ = path; + + return true; + } + + EuclideanWaypointer::EuclideanWaypointer(std::string config_path) + : Waypointer() + { + auto config = YAML::LoadFile(config_path); + monotonic_ = config["monotonic"].as(); + lookahead_ = config["lookahead"].as(); + panic_threshold_ = config["panic_threshold"].as(); + } + + EuclideanWaypointer::EuclideanWaypointer(const YAML::Node& config) + : Waypointer() + { + monotonic_ = config["monotonic"].as(); + lookahead_ = config["lookahead"].as(); + panic_threshold_ = config["panic_threshold"].as(); + } + + bool EuclideanWaypointer::init(const std::vector& path){ + panic_ = false; + last_min_dist_idx_ = 0; + return Waypointer::init(path); + } + + std::pair EuclideanWaypointer::closestWaypoint(const Waypoint& robot_pose){ + + double min_dist = std::numeric_limits::infinity(); + size_t min_dist_idx = path_.size() - 1; + size_t start_idx = (monotonic_ && !panic_) ? last_min_dist_idx_ : 0; + + for(size_t i = start_idx; i < path_.size(); i++){ + const auto [x, y] = path_.at(i); + const auto delta_x = x - robot_pose.first; + const auto delta_y = y - robot_pose.second; + const auto euclidean_distance = sqrt(delta_x * delta_x + delta_y * delta_y); + + if(euclidean_distance < min_dist){ + min_dist = euclidean_distance; + min_dist_idx = i; + } + } + + + std::pair best {min_dist, min_dist_idx}; + return best; + } + + + bool EuclideanWaypointer::run(const Waypoint& robot_pose, Waypoint& waypoint){ + if(path_.empty()){ + return false; + } + + auto [min_dist, min_dist_idx] = closestWaypoint(robot_pose); + + if(min_dist > panic_threshold_){ + panic_ = true; + std::tie(min_dist, min_dist_idx) = closestWaypoint(robot_pose); + }else{ + panic_ = false; + } + + if(lookahead_ > 0){ + min_dist_idx = std::min(min_dist_idx + lookahead_, path_.size() - 1); + } + + last_min_dist_idx_ = min_dist_idx; + waypoint = path_.at(min_dist_idx); + return true; + + } + } // namespace utils +} // namespace dls diff --git a/modules/utils/test/CMakeLists.txt b/modules/utils/test/CMakeLists.txt new file mode 100644 index 00000000..0d072159 --- /dev/null +++ b/modules/utils/test/CMakeLists.txt @@ -0,0 +1,18 @@ +add_executable(dls_utils_waypointer_test + test_waypointer.cpp +) + +target_link_libraries(dls_utils_waypointer_test + PRIVATE + dls_utils +) + +target_include_directories(dls_utils_waypointer_test + PRIVATE + ../include +) + +add_test( + NAME dls_utils_waypointer_test + COMMAND dls_utils_waypointer_test +) diff --git a/modules/utils/test/test_waypointer.cpp b/modules/utils/test/test_waypointer.cpp new file mode 100644 index 00000000..caa7f886 --- /dev/null +++ b/modules/utils/test/test_waypointer.cpp @@ -0,0 +1,122 @@ +#include +#include +#include +#include +#include +#include + +#include "dls2/util/waypointer.hpp" + +namespace +{ +bool check(bool condition, const std::string& message) +{ + if (!condition) { + std::cerr << "FAIL: " << message << std::endl; + return false; + } + return true; +} + +std::string writeConfigFile( + const std::string& file_name, + bool monotonic, + int lookahead, + double panic_threshold) +{ + const auto path = std::filesystem::temp_directory_path() / file_name; + std::ofstream config_file(path); + config_file << "monotonic: " << (monotonic ? "true" : "false") << "\n"; + config_file << "lookahead: " << lookahead << "\n"; + config_file << "panic_threshold: " << panic_threshold << "\n"; + config_file.close(); + return path.string(); +} + +bool testInitRejectsEmptyPath() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_empty.yaml", true, 0, 5.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector empty_path; + + return check(!waypointer.init(empty_path), "empty path should be rejected"); +} + +bool testMonotonicSearchUsesLookahead() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_lookahead.yaml", true, 1, 100.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector path{ + {0.0, 0.0}, + {1.0, 0.0}, + {2.0, 0.0}, + {3.0, 0.0}, + }; + + if (!check(waypointer.init(path), "path init should succeed")) { + return false; + } + + dls::utils::Waypoint waypoint{}; + if (!check(waypointer.run({0.2, 0.0}, waypoint), "first run should succeed")) { + return false; + } + if (!check(waypoint.first == 1.0 && waypoint.second == 0.0, + "first run should pick lookahead waypoint (1.0, 0.0)")) { + return false; + } + + if (!check(waypointer.run({1.9, 0.0}, waypoint), "second run should succeed")) { + return false; + } + return check(waypoint.first == 3.0 && waypoint.second == 0.0, + "second run should progress monotonically to (3.0, 0.0)"); +} + +bool testPanicFallbackRescansWholePath() +{ + const std::string config_path = + writeConfigFile("euclidean_waypointer_panic.yaml", true, 0, 5.0); + + dls::utils::EuclideanWaypointer waypointer(config_path); + const std::vector path{ + {0.0, 0.0}, + {10.0, 0.0}, + {20.0, 0.0}, + }; + + if (!check(waypointer.init(path), "panic test path init should succeed")) { + return false; + } + + dls::utils::Waypoint waypoint{}; + if (!check(waypointer.run({19.5, 0.0}, waypoint), + "pre-panic run should succeed")) { + return false; + } + if (!check(waypoint.first == 20.0 && waypoint.second == 0.0, + "pre-panic run should pick final waypoint")) { + return false; + } + + if (!check(waypointer.run({0.1, 0.0}, waypoint), + "panic fallback run should succeed")) { + return false; + } + return check(waypoint.first == 0.0 && waypoint.second == 0.0, + "panic fallback should rescan from start and recover waypoint 0"); +} +} // namespace + +int main() +{ + const bool ok = + testInitRejectsEmptyPath() && + testMonotonicSearchUsesLookahead() && + testPanicFallbackRescansWholePath(); + return ok ? EXIT_SUCCESS : EXIT_FAILURE; +} From 0f55dbe19f45e457b7520b40f5562db38705028e Mon Sep 17 00:00:00 2001 From: mich-pest Date: Thu, 27 Aug 2026 15:36:32 +0200 Subject: [PATCH 03/11] orchestrator: orchestrate method sign changed --- .../include/dls2/supervisor/orchestrator_base.hpp | 2 +- modules/supervisor/src/orchestrator_base.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp b/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp index fe0d2d38..0924c1f8 100644 --- a/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp +++ b/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp @@ -44,7 +44,7 @@ namespace dls * fill in dls status msgs * fill in dls action msgs */ - virtual void orchestrate(const std::chrono::system_clock::time_point&, const EventsPriorityQueue&) {}; + virtual void orchestrate(const std::chrono::system_clock::time_point&, EventsPriorityQueue&) {}; /** * @brief Virtual method implementing core processing in telemetry callback related to specifically typed messages. diff --git a/modules/supervisor/src/orchestrator_base.cpp b/modules/supervisor/src/orchestrator_base.cpp index 90b05d9c..7d2d0032 100644 --- a/modules/supervisor/src/orchestrator_base.cpp +++ b/modules/supervisor/src/orchestrator_base.cpp @@ -102,6 +102,12 @@ namespace dls } orchestrate(time, events_priority_queue_tmp); + + { + std::lock_guard lock(event_mutex_); + events_priority_queue_ = std::move(events_priority_queue_tmp); + } + write(); } catch (const std::exception& e) From bf916d3bac3b7d1aba1af61fe950e52bc1641a1f Mon Sep 17 00:00:00 2001 From: mich-pest Date: Thu, 27 Aug 2026 15:36:59 +0200 Subject: [PATCH 04/11] supervisor: cpu temperature check added --- modules/log/include/dls2/log/event_config.hpp | 1 + modules/supervisor/data/safety_layer.yaml | 2 ++ modules/supervisor/src/supervisor.cpp | 11 +++++++++++ .../utils/include/dls2/util/config/safety_layer.hpp | 4 ++++ 4 files changed, 18 insertions(+) diff --git a/modules/log/include/dls2/log/event_config.hpp b/modules/log/include/dls2/log/event_config.hpp index 7aeadbaf..6280cb6a 100644 --- a/modules/log/include/dls2/log/event_config.hpp +++ b/modules/log/include/dls2/log/event_config.hpp @@ -11,6 +11,7 @@ enum class EventID : uint8_t { WRONG_PROCESS_FREQUENCY, MISSING_INPUT, CPU_USAGE_TOO_HIGH, + CPU_TEMP_TOO_HIGH, MEM_USAGE_TOO_HIGH, INPUTS_NOT_SYNCHRONIZED, WRONG_INPUT_FREQUENCY, diff --git a/modules/supervisor/data/safety_layer.yaml b/modules/supervisor/data/safety_layer.yaml index bcf05ca0..473a6df5 100644 --- a/modules/supervisor/data/safety_layer.yaml +++ b/modules/supervisor/data/safety_layer.yaml @@ -7,6 +7,7 @@ checks: enable_wrong_process_frequency: true enable_missing_input: false enable_cpu_usage_too_high: true + enable_cpu_temp_too_high: true enable_mem_usage_too_high: true enable_inputs_not_synchronized: true enable_wrong_input_frequency: false @@ -20,4 +21,5 @@ checks: process_monitor_window_size: 400 # samples in cpu/mem usage window process_cpu_threshold: 300.0 # [%] value in [0, N_cores * 100] max single process cpu percentage usage without warning (moving window mean) system_cpu_threshold: 90.0 # [%] value in [0, 100] max overall cpu percentage usage without warning (moving window mean) + system_cpu_temp_threshold: 120.0 # [°C] mem_threshold: 90.0 # [%] max mem percentage without warning (moving window mean) \ No newline at end of file diff --git a/modules/supervisor/src/supervisor.cpp b/modules/supervisor/src/supervisor.cpp index 4baa8ec4..671f834d 100644 --- a/modules/supervisor/src/supervisor.cpp +++ b/modules/supervisor/src/supervisor.cpp @@ -133,6 +133,17 @@ namespace dls ); } + if(this->safety_layer_config_->enable_cpu_temp_too_high && + temperature.at(i).second > this->safety_layer_config_->system_cpu_temp_threshold) + { + this->robust_event_notifier.notify( + EventID::CPU_TEMP_TOO_HIGH, + EventSeverity::WARNING, + this->getID() + ": overall cpu temperature - on core n. " + std::to_string(i) + " - is " + + std::to_string(temperature.at(i).second) + "(threshold is " + std::to_string(this->safety_layer_config_->system_cpu_temp_threshold) + ")" + ); + } + } if (this->safety_layer_config_->enable_mem_usage_too_high && diff --git a/modules/utils/include/dls2/util/config/safety_layer.hpp b/modules/utils/include/dls2/util/config/safety_layer.hpp index dc0ab096..fde3f6f6 100644 --- a/modules/utils/include/dls2/util/config/safety_layer.hpp +++ b/modules/utils/include/dls2/util/config/safety_layer.hpp @@ -15,6 +15,7 @@ namespace dls bool enable_wrong_process_frequency{false}; bool enable_missing_input{false}; bool enable_cpu_usage_too_high{false}; + bool enable_cpu_temp_too_high{false}; bool enable_mem_usage_too_high{false}; bool enable_inputs_not_synchronized{false}; bool enable_wrong_input_frequency{false}; @@ -28,6 +29,7 @@ namespace dls size_t process_monitor_window_size {100}; size_t resource_monitor_window_size {100}; double system_cpu_threshold{100.0}; + double system_cpu_temp_threshold{100.0}; double process_cpu_threshold{300.0}; double mem_threshold{100.0}; std::unordered_map nodes_specs; @@ -50,6 +52,7 @@ namespace dls enable_wrong_process_frequency = config["checks"]["enable_wrong_process_frequency"].as(); enable_missing_input = config["checks"]["enable_missing_input"].as(); enable_cpu_usage_too_high = config["checks"]["enable_cpu_usage_too_high"].as(); + enable_cpu_temp_too_high = config["checks"]["enable_cpu_temp_too_high"].as(); enable_mem_usage_too_high = config["checks"]["enable_mem_usage_too_high"].as(); enable_inputs_not_synchronized = config["checks"]["enable_inputs_not_synchronized"].as(); enable_wrong_input_frequency = config["checks"]["enable_wrong_input_frequency"].as(); @@ -62,6 +65,7 @@ namespace dls process_monitor_window_size = config["checks"]["process_monitor_window_size"].as(); resource_monitor_window_size = config["checks"]["resource_monitor_window_size"].as(); system_cpu_threshold = config["checks"]["system_cpu_threshold"].as(); + system_cpu_temp_threshold = config["checks"]["system_cpu_temp_threshold"].as(); process_cpu_threshold = config["checks"]["process_cpu_threshold"].as(); mem_threshold = config["checks"]["mem_threshold"].as(); From 78884e5188f2847c5ef25f38090489a915816352 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:49:27 +0200 Subject: [PATCH 05/11] periodic_app: current frequency in moving window --- .../include/dls2/application/periodic_app.hpp | 2 ++ modules/application/src/periodic_app.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/modules/application/include/dls2/application/periodic_app.hpp b/modules/application/include/dls2/application/periodic_app.hpp index e66e344c..c6bc5f2a 100644 --- a/modules/application/include/dls2/application/periodic_app.hpp +++ b/modules/application/include/dls2/application/periodic_app.hpp @@ -4,6 +4,7 @@ #include "dls2/application/app.hpp" #include #include "dls2/util/process_resource_monitor.hpp" +#include "dls2/util/numerical_moving_window.hpp" #include #include @@ -124,6 +125,7 @@ namespace dls std::chrono::time_point loop_time_prec; std::unique_ptr process_resource_monitor_; + std::unique_ptr> frequency_moving_window_; double current_frequency_; std::mutex frequency_mutex_; diff --git a/modules/application/src/periodic_app.cpp b/modules/application/src/periodic_app.cpp index b21b7f6a..d60d69f5 100644 --- a/modules/application/src/periodic_app.cpp +++ b/modules/application/src/periodic_app.cpp @@ -1,5 +1,7 @@ #include "dls2/application/periodic_app.hpp" +#include + using namespace dls; PeriodicApp::PeriodicApp(const std::string &ID) @@ -68,6 +70,8 @@ PeriodicApp::PeriodicApp(const std::string &ID) process_resource_monitor_ = std::make_unique( this->pid, this->safety_layer_config_->process_monitor_window_size); + frequency_moving_window_ = std::make_unique>( + this->safety_layer_config_->process_frequency_window_size); } std::string PeriodicApp::getSchedulerPath(const std::string &ID) @@ -251,7 +255,13 @@ void PeriodicApp::checkRT() { { std::lock_guard lock(this->frequency_mutex_); - realtime_curr = Time::checkFrequency(this->safety_layer_config_->realtime_tolerance_factor, getDesiredFrequency(), loop_time_prec, current_frequency_); + double instantaneous_frequency = 0.0; + Time::checkFrequency(this->safety_layer_config_->realtime_tolerance_factor, + getDesiredFrequency(), loop_time_prec, instantaneous_frequency); + frequency_moving_window_->push(instantaneous_frequency); + current_frequency_ = frequency_moving_window_->mean(); + realtime_curr = std::abs(getDesiredFrequency() - current_frequency_) < + this->safety_layer_config_->realtime_tolerance_factor * getDesiredFrequency(); } // notify if the process is not running in real time From 575d9f629c4b7771d86d15f629eb737171af41e8 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:50:39 +0200 Subject: [PATCH 06/11] event_logger: event buffer mutex added --- modules/log/include/dls2/log/event_logger.hpp | 2 + modules/log/src/event_logger.cpp | 51 +++++++++---------- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/modules/log/include/dls2/log/event_logger.hpp b/modules/log/include/dls2/log/event_logger.hpp index 706d608f..86259a2a 100644 --- a/modules/log/include/dls2/log/event_logger.hpp +++ b/modules/log/include/dls2/log/event_logger.hpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace dls { @@ -87,6 +88,7 @@ namespace dls std::vector readEvents(long int& idx_read); boost::circular_buffer event_buffer_; + mutable std::mutex event_buffer_mutex_; private: const std::string name_; diff --git a/modules/log/src/event_logger.cpp b/modules/log/src/event_logger.cpp index 694be313..44603f69 100644 --- a/modules/log/src/event_logger.cpp +++ b/modules/log/src/event_logger.cpp @@ -126,6 +126,8 @@ EventListener::EventListener(const std::string &name) // << "\ncomponent: " << msg->component_name() // << "\nmessage: " << msg->msg() // << "\n###################" << std::endl; + + std::lock_guard lock(event_buffer_mutex_); event_buffer_.push_back(*msg); // } unbounded_buffer_idx_++; @@ -150,46 +152,43 @@ int EventListener::getNumOfMatches() const unsigned long long int EventListener::getUnboundedBufferIdx() const { + std::lock_guard lock(event_buffer_mutex_); return unbounded_buffer_idx_; } unsigned long int EventListener::getBufferMaxIdx() const { + std::lock_guard lock(event_buffer_mutex_); return event_buffer_.capacity()-1; } std::vector EventListener::readEvents(long int& idx_read) { std::vector events; - events.clear(); - idx_read = 0; - buffer_max_idx_ = getBufferMaxIdx(); + std::lock_guard lock(event_buffer_mutex_); + const long long latest_sequence_id = unbounded_buffer_idx_; + if (latest_sequence_id < 0 || latest_sequence_id < idx_read) { + return events; + } - long int idx_buffer = getUnboundedBufferIdx(); - if(idx_buffer >= idx_read) - { - // mapping unbounded indexes in bounded indexes - long int delta = idx_buffer - idx_read; - if(delta > buffer_max_idx_){ - idx_read = 0; - idx_buffer = buffer_max_idx_; - } - else if (idx_buffer >= buffer_max_idx_) - { - idx_read = buffer_max_idx_ - delta; - idx_buffer = idx_read + delta; - } - - // read values - for(long int i = idx_read; i <= idx_buffer; ++i) - { - events.push_back(event_buffer_[i]); - } - - // update read index - idx_read = idx_buffer + 1; + const long long oldest_retained_sequence_id = + latest_sequence_id - static_cast(event_buffer_.size()) + 1; + long long first_sequence_id = idx_read; + if (first_sequence_id < oldest_retained_sequence_id) { + first_sequence_id = oldest_retained_sequence_id; } + events.reserve(static_cast(latest_sequence_id - first_sequence_id + 1)); + for (long long sequence_id = first_sequence_id; + sequence_id <= latest_sequence_id; + ++sequence_id) { + const auto buffer_index = static_cast( + sequence_id - oldest_retained_sequence_id); + events.push_back(event_buffer_[buffer_index]); + } + + idx_read = static_cast(latest_sequence_id + 1); + return events; } From 10ab2889757054cab6fb8c4e61452e7793444b93 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:51:23 +0200 Subject: [PATCH 07/11] safety_layer: sequence id not checked on process_status topic --- modules/plugin/src/periodic_app_plugin.cpp | 3 ++- modules/supervisor/data/safety_layer.yaml | 4 +++- modules/utils/include/dls2/util/config/safety_layer.hpp | 6 +++++- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/modules/plugin/src/periodic_app_plugin.cpp b/modules/plugin/src/periodic_app_plugin.cpp index 3bf59660..4a0dbe60 100644 --- a/modules/plugin/src/periodic_app_plugin.cpp +++ b/modules/plugin/src/periodic_app_plugin.cpp @@ -44,7 +44,8 @@ namespace dls if(this->safety_layer_config_->enable_wrong_sequence_id){ for(size_t i = 0; i < input_info.size(); ++i){ - if(input_info.at(i).missed_sequence_ids != 0){ + if(input_info.at(i).missed_sequence_ids != 0 + && input_info.at(i).topic_name != this->safety_layer_config_->process_status_topic){ this->robust_event_notifier.notify( EventID::WRONG_SEQUENCE_ID, EventSeverity::WARNING, diff --git a/modules/supervisor/data/safety_layer.yaml b/modules/supervisor/data/safety_layer.yaml index 473a6df5..eaf19690 100644 --- a/modules/supervisor/data/safety_layer.yaml +++ b/modules/supervisor/data/safety_layer.yaml @@ -17,9 +17,11 @@ checks: max_exceeding_factor: 0.5 # value in [0,1]: factor of input topic desired freq sync_threshold: 500 # [ms] realtime_tolerance_factor: 0.25 # value in [0,1]: factor of node desired freq + process_frequency_window_size: 5 # samples used to smooth process frequency resource_monitor_window_size: 30 # samples in cpu/mem usage window process_monitor_window_size: 400 # samples in cpu/mem usage window process_cpu_threshold: 300.0 # [%] value in [0, N_cores * 100] max single process cpu percentage usage without warning (moving window mean) system_cpu_threshold: 90.0 # [%] value in [0, 100] max overall cpu percentage usage without warning (moving window mean) system_cpu_temp_threshold: 120.0 # [°C] - mem_threshold: 90.0 # [%] max mem percentage without warning (moving window mean) \ No newline at end of file + mem_threshold: 90.0 # [%] max mem percentage without warning (moving window mean) + process_status_topic: "rt/process_status" diff --git a/modules/utils/include/dls2/util/config/safety_layer.hpp b/modules/utils/include/dls2/util/config/safety_layer.hpp index fde3f6f6..33d5263b 100644 --- a/modules/utils/include/dls2/util/config/safety_layer.hpp +++ b/modules/utils/include/dls2/util/config/safety_layer.hpp @@ -26,6 +26,7 @@ namespace dls double sync_threshold_ms{500}; double realtime_tolerance_factor{0.3}; size_t monitor_period_ms{100}; + size_t process_frequency_window_size {100}; size_t process_monitor_window_size {100}; size_t resource_monitor_window_size {100}; double system_cpu_threshold{100.0}; @@ -33,6 +34,7 @@ namespace dls double process_cpu_threshold{300.0}; double mem_threshold{100.0}; std::unordered_map nodes_specs; + std::string process_status_topic { "/process_status" }; explicit SafetyLayerConfig(const std::string &config_file) { @@ -62,12 +64,14 @@ namespace dls sync_threshold_ms = config["checks"]["sync_threshold"].as(); realtime_tolerance_factor = config["checks"]["realtime_tolerance_factor"].as(); monitor_period_ms = config["checks"]["monitor_period"].as(); + process_frequency_window_size = config["checks"]["process_frequency_window_size"].as(); process_monitor_window_size = config["checks"]["process_monitor_window_size"].as(); resource_monitor_window_size = config["checks"]["resource_monitor_window_size"].as(); system_cpu_threshold = config["checks"]["system_cpu_threshold"].as(); system_cpu_temp_threshold = config["checks"]["system_cpu_temp_threshold"].as(); process_cpu_threshold = config["checks"]["process_cpu_threshold"].as(); mem_threshold = config["checks"]["mem_threshold"].as(); + process_status_topic = config["checks"]["process_status_topic"].as(); spam_threshold = config["events"]["spam_threshold"].as(); @@ -90,4 +94,4 @@ namespace dls } } }; -} \ No newline at end of file +} From 8a66f7ee03ec70d740274a56490f6c7db48a7822 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:51:52 +0200 Subject: [PATCH 08/11] topics: battery status topic added --- modules/topics/include/dls2/topics/topics.hpp | 2 ++ modules/topics/src/topics.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/modules/topics/include/dls2/topics/topics.hpp b/modules/topics/include/dls2/topics/topics.hpp index ed43d013..491db30e 100644 --- a/modules/topics/include/dls2/topics/topics.hpp +++ b/modules/topics/include/dls2/topics/topics.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +48,7 @@ namespace dls extern dls::topicType log_events; extern dls::topicType process_status; extern dls::topicType dls_events; + extern dls::topicType battery_status; // command extern dls::topicType command_call; diff --git a/modules/topics/src/topics.cpp b/modules/topics/src/topics.cpp index ff41a235..19152a21 100644 --- a/modules/topics/src/topics.cpp +++ b/modules/topics/src/topics.cpp @@ -16,6 +16,8 @@ namespace dls new dls2_interface::msg::ProcessStatusPubSubType()); dls::topicType dls_events = dls::topicType("dls/events", new dls2_interface::msg::DlsEventsPubSubType()); + dls::topicType battery_status = dls::topicType("battery_status", + new dls2_interface::msg::BatteryStatusPubSubType()); // command From 206b842f4458478862f7109aba2fab49aa240f2d Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:52:10 +0200 Subject: [PATCH 09/11] supervisor: cpu temperature check added --- modules/supervisor/src/supervisor.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/modules/supervisor/src/supervisor.cpp b/modules/supervisor/src/supervisor.cpp index 671f834d..c944e8f0 100644 --- a/modules/supervisor/src/supervisor.cpp +++ b/modules/supervisor/src/supervisor.cpp @@ -132,18 +132,17 @@ namespace dls + std::to_string(cpus_usage.at(i)) + "(threshold is " + std::to_string(this->safety_layer_config_->system_cpu_threshold) + ")" ); } + } - if(this->safety_layer_config_->enable_cpu_temp_too_high && - temperature.at(i).second > this->safety_layer_config_->system_cpu_temp_threshold) - { - this->robust_event_notifier.notify( - EventID::CPU_TEMP_TOO_HIGH, - EventSeverity::WARNING, - this->getID() + ": overall cpu temperature - on core n. " + std::to_string(i) + " - is " - + std::to_string(temperature.at(i).second) + "(threshold is " + std::to_string(this->safety_layer_config_->system_cpu_temp_threshold) + ")" - ); - } - + if(this->safety_layer_config_->enable_cpu_temp_too_high && + temperature.second > this->safety_layer_config_->system_cpu_temp_threshold) + { + this->robust_event_notifier.notify( + EventID::CPU_TEMP_TOO_HIGH, + EventSeverity::WARNING, + this->getID() + ": overall cpu temperature is " + + std::to_string(temperature.second) + "(threshold is " + std::to_string(this->safety_layer_config_->system_cpu_temp_threshold) + ")" + ); } if (this->safety_layer_config_->enable_mem_usage_too_high && From f73b6c6a8c2707d54eb22370b400df47e872a283 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 13:52:44 +0200 Subject: [PATCH 10/11] orchestrator: telemetry event deque added --- .../include/dls2/supervisor/orchestrator_base.hpp | 2 ++ modules/supervisor/src/orchestrator_base.cpp | 15 +++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp b/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp index 0924c1f8..52b449fd 100644 --- a/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp +++ b/modules/supervisor/include/dls2/supervisor/orchestrator_base.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "dls2/plugin/periodic_app_plugin.hpp" #include "dls2/state_machine/state_machine.hpp" @@ -61,6 +62,7 @@ namespace dls std::mutex event_mutex_; logging::EventListener event_listener_; EventsPriorityQueue events_priority_queue_; + std::deque telemetry_events_; // Telemetry std::vector> telemetry_readers_; diff --git a/modules/supervisor/src/orchestrator_base.cpp b/modules/supervisor/src/orchestrator_base.cpp index 7d2d0032..e254e7e6 100644 --- a/modules/supervisor/src/orchestrator_base.cpp +++ b/modules/supervisor/src/orchestrator_base.cpp @@ -51,16 +51,14 @@ namespace dls // Prepare events for publication std::vector events_to_publish; - EventsPriorityQueue events_priority_queue_tmp; { std::lock_guard lock(event_mutex_); - events_priority_queue_tmp = events_priority_queue_; - } - size_t event_count = 0; - while (!events_priority_queue_tmp.empty() && event_count < event_to_publish_) { - events_to_publish.push_back(events_priority_queue_tmp.top()); - events_priority_queue_tmp.pop(); - event_count++; + size_t event_count = 0; + while (!telemetry_events_.empty() && event_count < event_to_publish_) { + events_to_publish.push_back(telemetry_events_.front()); + telemetry_events_.pop_front(); + event_count++; + } } telemetryMain(events_to_publish); @@ -96,6 +94,7 @@ namespace dls for(const auto& event : events_fifo){ events_priority_queue_.push(event); + telemetry_events_.push_back(event); } events_priority_queue_tmp = events_priority_queue_; From b27662a0906534e2cc2aa05c5371282a89961104 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 31 Aug 2026 16:57:38 +0200 Subject: [PATCH 11/11] topics: removed ArmState --- modules/topics/include/dls2/topics/topics.hpp | 1 - modules/topics/src/topics.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/modules/topics/include/dls2/topics/topics.hpp b/modules/topics/include/dls2/topics/topics.hpp index 491db30e..7a0c8f50 100644 --- a/modules/topics/include/dls2/topics/topics.hpp +++ b/modules/topics/include/dls2/topics/topics.hpp @@ -6,7 +6,6 @@ #include #include #include -#include #include #include #include diff --git a/modules/topics/src/topics.cpp b/modules/topics/src/topics.cpp index 19152a21..54a9bf76 100644 --- a/modules/topics/src/topics.cpp +++ b/modules/topics/src/topics.cpp @@ -80,8 +80,6 @@ namespace dls namespace low_level_estimation { - dls::topicType arm_state = dls::topicType("arm_state", - new dls2_interface::msg::ArmStatePubSubType()); dls::topicType blind_state = dls::topicType("blind_state", new dls2_interface::msg::BlindStatePubSubType()); dls::topicType imu = dls::topicType("imu",