From 7983150c7362b73f9fd21d12a5bda580cf4a1842 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 4 Dec 2025 07:38:41 +0000 Subject: [PATCH 1/3] Fix upstream API --- .../src/picknik_reset_fault_controller.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp index e9197e2..2ce4dcf 100644 --- a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp +++ b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp @@ -62,11 +62,10 @@ CallbackReturn PicknikResetFaultController::on_init() { return CallbackReturn::S controller_interface::return_type PicknikResetFaultController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - if (realtime_publisher_ && realtime_publisher_->trylock()) + if (realtime_publisher_) { - state_.data = static_cast(state_interfaces_[StateInterfaces::IN_FAULT].get_value()); - realtime_publisher_->msg_.data = state_.data; - realtime_publisher_->unlockAndPublish(); + state_.data = static_cast(state_interfaces_[StateInterfaces::IN_FAULT].get_optional().value()); + realtime_publisher_->try_publish(state_); } return controller_interface::return_type::OK; @@ -121,14 +120,14 @@ bool PicknikResetFaultController::resetFault( RCLCPP_INFO(get_node()->get_logger(), "Trying to reset faults on hardware controller."); - while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value() == + while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value() == ASYNC_WAITING) { // Asynchronous wait until the hardware interface has set the io value std::this_thread::sleep_for(std::chrono::milliseconds(50)); } resp->success = static_cast( - command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_value()); + command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value()); RCLCPP_INFO( get_node()->get_logger(), "Resetting fault on hardware controller '%s'!", From 6e2a152a49779d9a3f824f934a6ad092bcd74cff Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 6 Dec 2025 21:33:49 +0000 Subject: [PATCH 2/3] Fix potential bad optional access --- .../src/picknik_reset_fault_controller.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp index 2ce4dcf..4e4e9c1 100644 --- a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp +++ b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp @@ -64,7 +64,12 @@ controller_interface::return_type PicknikResetFaultController::update( { if (realtime_publisher_) { - state_.data = static_cast(state_interfaces_[StateInterfaces::IN_FAULT].get_optional().value()); + auto state_op = + command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional(); + if (state_op.has_value()) + { + state_.data = static_cast(state_op.value()); + } realtime_publisher_->try_publish(state_); } @@ -120,14 +125,23 @@ bool PicknikResetFaultController::resetFault( RCLCPP_INFO(get_node()->get_logger(), "Trying to reset faults on hardware controller."); - while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value() == - ASYNC_WAITING) + while (command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value_or( + ASYNC_WAITING) == ASYNC_WAITING) { // Asynchronous wait until the hardware interface has set the io value std::this_thread::sleep_for(std::chrono::milliseconds(50)); } - resp->success = static_cast( - command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional().value()); + auto async_result_op = + command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional(); + if (!async_result_op.has_value()) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Failed to get result of fault reset command from hardware controller."); + resp->success = false; + return resp->success; + } + resp->success = static_cast(async_result_op.value()); RCLCPP_INFO( get_node()->get_logger(), "Resetting fault on hardware controller '%s'!", From e3544943f4d1e847742d52a0a66a8cd9e82a874f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 7 Dec 2025 19:32:39 +0000 Subject: [PATCH 3/3] Use correct state interface again --- .../src/picknik_reset_fault_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp index 4e4e9c1..b9afe30 100644 --- a/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp +++ b/picknik_reset_fault_controller/src/picknik_reset_fault_controller.cpp @@ -64,8 +64,7 @@ controller_interface::return_type PicknikResetFaultController::update( { if (realtime_publisher_) { - auto state_op = - command_interfaces_[CommandInterfaces::RESET_FAULT_ASYNC_SUCCESS].get_optional(); + auto state_op = state_interfaces_[StateInterfaces::IN_FAULT].get_optional(); if (state_op.has_value()) { state_.data = static_cast(state_op.value());