From 2ca6e0557dcf7a307c3584e128805a8b62421802 Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 13:06:04 +0000 Subject: [PATCH 1/5] [sas_robot_driver] Added the callback feature in RobotDriver. It is still a test. --- include/sas_core/sas_robot_driver.hpp | 11 +++++++++++ src/sas_robot_driver.cpp | 5 +++++ 2 files changed, 16 insertions(+) diff --git a/include/sas_core/sas_robot_driver.hpp b/include/sas_core/sas_robot_driver.hpp index 8d731c2..85c8278 100644 --- a/include/sas_core/sas_robot_driver.hpp +++ b/include/sas_core/sas_robot_driver.hpp @@ -85,6 +85,9 @@ class RobotDriver std::exception_ptr watchdog_exception_{nullptr}; std::mutex watchdog_exception_mutex_; + //std::shared_ptr> control_loop_callback_; + std::function control_loop_callback_; + public: /** * @brief Enumeration of optional driver functionalities @@ -204,6 +207,14 @@ class RobotDriver * @brief Deinitialize the driver resources */ virtual void deinitialize()=0; + + + /** + * @brief Set the control loop callback function + * @param callback The callback function to be executed in the control loop + * + */ + void set_control_loop_callback(std::function callback); }; } diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp index 7a98842..cc3c914 100644 --- a/src/sas_robot_driver.cpp +++ b/src/sas_robot_driver.cpp @@ -202,5 +202,10 @@ void RobotDriver::check_for_watchdog_exceptions() std::rethrow_exception(watchdog_exception_); } +void RobotDriver::set_control_loop_callback(std::function callback) +{ + control_loop_callback_ = std::move(callback); +} + } From f427ba627acbcbe52174af5c3d857abbe5e45a2a Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 13:44:44 +0000 Subject: [PATCH 2/5] Added a new public method to call the callback --- include/sas_core/sas_robot_driver.hpp | 10 ++++++++++ src/sas_robot_driver.cpp | 23 +++++++++++++++++++++++ 2 files changed, 33 insertions(+) diff --git a/include/sas_core/sas_robot_driver.hpp b/include/sas_core/sas_robot_driver.hpp index 85c8278..c21bc48 100644 --- a/include/sas_core/sas_robot_driver.hpp +++ b/include/sas_core/sas_robot_driver.hpp @@ -215,6 +215,16 @@ class RobotDriver * */ void set_control_loop_callback(std::function callback); + + + /** + * @brief Execute the control loop callback if it has been set + * + * This method should be called by RobotDriverROS or any other class + * that runs the control loop. It will execute the callback that was + * set by the concrete RobotDriver implementation. + */ + void execute_control_loop_callback(); }; } diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp index cc3c914..fa25679 100644 --- a/src/sas_robot_driver.cpp +++ b/src/sas_robot_driver.cpp @@ -202,10 +202,33 @@ void RobotDriver::check_for_watchdog_exceptions() std::rethrow_exception(watchdog_exception_); } +/** + * @brief RobotDriver::set_control_loop_callback + * @param callback + */ void RobotDriver::set_control_loop_callback(std::function callback) { control_loop_callback_ = std::move(callback); } +/** + * @brief RobotDriver::execute_control_loop_callback + */ +void RobotDriver::execute_control_loop_callback() +{ + if (control_loop_callback_) + { + try + { + control_loop_callback_(); + } + catch (const std::exception& e) + { + throw std::runtime_error("Control loop callback exception: " + std::string(e.what())); + } + } +} + + } From 00d6b4e38a4bd5156e229bba09cc575ad2a5ceef Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 14:59:59 +0000 Subject: [PATCH 3/5] Added a new public method to check if a callback function is set --- include/sas_core/sas_robot_driver.hpp | 7 +++++++ src/sas_robot_driver.cpp | 13 +++++++++++-- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/include/sas_core/sas_robot_driver.hpp b/include/sas_core/sas_robot_driver.hpp index c21bc48..db03a3b 100644 --- a/include/sas_core/sas_robot_driver.hpp +++ b/include/sas_core/sas_robot_driver.hpp @@ -225,6 +225,13 @@ class RobotDriver * set by the concrete RobotDriver implementation. */ void execute_control_loop_callback(); + + + /** + * @brief Check if a control loop callback has been set + * @return true if a callback has been set, false otherwise + */ + bool control_loop_callback_is_set(); }; } diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp index fa25679..7ac33ae 100644 --- a/src/sas_robot_driver.cpp +++ b/src/sas_robot_driver.cpp @@ -206,7 +206,7 @@ void RobotDriver::check_for_watchdog_exceptions() * @brief RobotDriver::set_control_loop_callback * @param callback */ -void RobotDriver::set_control_loop_callback(std::function callback) +void RobotDriver::set_control_loop_callback(std::function callback) { control_loop_callback_ = std::move(callback); } @@ -223,12 +223,21 @@ void RobotDriver::execute_control_loop_callback() { control_loop_callback_(); } - catch (const std::exception& e) + catch(const std::exception& e) { throw std::runtime_error("Control loop callback exception: " + std::string(e.what())); } } } +/** + * @brief Check if a control loop callback has been set + * @return true if a callback has been set, false otherwise + */ +bool RobotDriver::control_loop_callback_is_set() +{ + return static_cast(control_loop_callback_); +} + } From 9dfe5f8af487e370842f1d08da02e4de01ad82d8 Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 15:54:10 +0000 Subject: [PATCH 4/5] Added missing documentation --- src/sas_robot_driver.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp index 7ac33ae..63463ea 100644 --- a/src/sas_robot_driver.cpp +++ b/src/sas_robot_driver.cpp @@ -213,7 +213,11 @@ void RobotDriver::set_control_loop_callback(std::function callback) /** - * @brief RobotDriver::execute_control_loop_callback + * @brief Execute the control loop callback if it has been set + * + * This method should be called by RobotDriverROS or any other class + * that runs the control loop. It will execute the callback that was + * set by the concrete RobotDriver implementation. */ void RobotDriver::execute_control_loop_callback() { From 2f6011a7e4978888b91e12fc97c40ba34046f12e Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 15:58:27 +0000 Subject: [PATCH 5/5] removed a commented line --- include/sas_core/sas_robot_driver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/sas_core/sas_robot_driver.hpp b/include/sas_core/sas_robot_driver.hpp index db03a3b..b7373ce 100644 --- a/include/sas_core/sas_robot_driver.hpp +++ b/include/sas_core/sas_robot_driver.hpp @@ -85,7 +85,7 @@ class RobotDriver std::exception_ptr watchdog_exception_{nullptr}; std::mutex watchdog_exception_mutex_; - //std::shared_ptr> control_loop_callback_; + std::function control_loop_callback_; public: