From e91db991a97ad56f70e97a23201e8195f1d84fa1 Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 14:11:59 +0000 Subject: [PATCH 1/2] Added the callback in the control loop --- src/sas_robot_driver_ros.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/sas_robot_driver_ros.cpp b/src/sas_robot_driver_ros.cpp index 497cda9..f679f20 100644 --- a/src/sas_robot_driver_ros.cpp +++ b/src/sas_robot_driver_ros.cpp @@ -161,6 +161,13 @@ int RobotDriverROS::control_loop() } + // execute the callback (if any) + try { + robot_driver_->execute_control_loop_callback(); + }catch (const std::exception& e) { + throw std::runtime_error("Invalid control loop callback operation: " + std::string(e.what())); + } + auto joint_positions{robot_driver_->get_joint_positions()}; VectorXd joint_velocities; From c3f6aa33e62906d73d9f7377993a5d9ef2e53271 Mon Sep 17 00:00:00 2001 From: Juancho Date: Wed, 8 Jul 2026 14:58:49 +0000 Subject: [PATCH 2/2] Improved the checking order for the control loop callback --- src/sas_robot_driver_ros.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/sas_robot_driver_ros.cpp b/src/sas_robot_driver_ros.cpp index f679f20..19aa2a7 100644 --- a/src/sas_robot_driver_ros.cpp +++ b/src/sas_robot_driver_ros.cpp @@ -161,11 +161,15 @@ int RobotDriverROS::control_loop() } - // execute the callback (if any) - try { - robot_driver_->execute_control_loop_callback(); - }catch (const std::exception& e) { - throw std::runtime_error("Invalid control loop callback operation: " + std::string(e.what())); + + // Execute the control loop callback if one has been set + if (robot_driver_->control_loop_callback_is_set()) { + RCLCPP_INFO_STREAM_ONCE(node_->get_logger(), "::Control loop callback is set and will be executed!"); + try { + robot_driver_->execute_control_loop_callback(); + } catch (const std::exception& e) { + throw std::runtime_error("Control loop callback execution failed: " + std::string(e.what())); + } }