diff --git a/ur_controllers/include/ur_controllers/force_mode_controller.hpp b/ur_controllers/include/ur_controllers/force_mode_controller.hpp
index 9ae45a04b..5ce586f56 100644
--- a/ur_controllers/include/ur_controllers/force_mode_controller.hpp
+++ b/ur_controllers/include/ur_controllers/force_mode_controller.hpp
@@ -37,17 +37,17 @@
 #pragma once
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
-#include <realtime_tools/realtime_buffer.h>
 #include <array>
 #include <memory>
 
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <realtime_tools/realtime_buffer.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <ur_msgs/srv/set_force_mode.hpp>
 
-#include "force_mode_controller_parameters.hpp"
+#include "ur_controllers/force_mode_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp
index 04e90805e..3b01e358b 100644
--- a/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp
+++ b/ur_controllers/include/ur_controllers/freedrive_mode_controller.hpp
@@ -53,7 +53,7 @@
 #include <rclcpp/duration.hpp>
 #include "std_msgs/msg/bool.hpp"
 
-#include "freedrive_mode_controller_parameters.hpp"
+#include "ur_controllers/freedrive_mode_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/gpio_controller.hpp b/ur_controllers/include/ur_controllers/gpio_controller.hpp
index b06b5139a..502254a9a 100644
--- a/ur_controllers/include/ur_controllers/gpio_controller.hpp
+++ b/ur_controllers/include/ur_controllers/gpio_controller.hpp
@@ -57,7 +57,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/bool.hpp"
-#include "gpio_controller_parameters.hpp"
+#include "ur_controllers/gpio_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp
index a3c91d10f..3d9c785be 100644
--- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp
+++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp
@@ -43,9 +43,6 @@
 
 #include <stdint.h>
 
-#include <realtime_tools/realtime_buffer.h>
-#include <realtime_tools/realtime_server_goal_handle.h>
-
 #include <functional>
 #include <limits>
 #include <memory>
@@ -54,6 +51,8 @@
 #include <vector>
 
 #include <controller_interface/controller_interface.hpp>
+#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_server_goal_handle.hpp>
 #include <rclcpp_action/server.hpp>
 #include <rclcpp_action/create_server.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -65,7 +64,7 @@
 #include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include <control_msgs/action/follow_joint_trajectory.hpp>
 
-#include "passthrough_trajectory_controller_parameters.hpp"
+#include "ur_controllers/passthrough_trajectory_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
index 58bd12426..7ec1814f1 100644
--- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
+++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
@@ -45,7 +45,7 @@
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
-#include "scaled_joint_trajectory_controller_parameters.hpp"
+#include "ur_controllers/scaled_joint_trajectory_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp
index 4778f7ce1..d2cf62e08 100644
--- a/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp
+++ b/ur_controllers/include/ur_controllers/speed_scaling_state_broadcaster.hpp
@@ -48,7 +48,7 @@
 #include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
 #include "std_msgs/msg/float64.hpp"
-#include "speed_scaling_state_broadcaster_parameters.hpp"
+#include "ur_controllers/speed_scaling_state_broadcaster_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp
index 30a7378a1..e15478f3e 100644
--- a/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp
+++ b/ur_controllers/include/ur_controllers/ur_configuration_controller.hpp
@@ -44,10 +44,10 @@
 #include <memory>
 
 #include <controller_interface/controller_interface.hpp>
-#include <realtime_tools/realtime_box.h>
+#include <realtime_tools/realtime_box.hpp>
 
 #include "ur_msgs/srv/get_robot_software_version.hpp"
-#include "ur_configuration_controller_parameters.hpp"
+#include "ur_controllers/ur_configuration_controller_parameters.hpp"
 
 namespace ur_controllers
 {
diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml
index 5d1e0b3b8..f3ee5080b 100644
--- a/ur_controllers/package.xml
+++ b/ur_controllers/package.xml
@@ -25,6 +25,7 @@
 
   <depend>angles</depend>
   <depend>controller_interface</depend>
+  <depend>generate_parameter_library</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>joint_trajectory_controller</depend>
diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp
index f13d5b7f0..b64a26bbc 100644
--- a/ur_controllers/src/force_mode_controller.cpp
+++ b/ur_controllers/src/force_mode_controller.cpp
@@ -156,7 +156,9 @@ controller_interface::CallbackReturn
 ur_controllers::ForceModeController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
   // Stop force mode if this controller is deactivated.
-  command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0);
+  if (!command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0)) {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
   return LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 
@@ -175,54 +177,80 @@ controller_interface::return_type ur_controllers::ForceModeController::update(co
 
   // Publish state of force_mode?
   if (change_requested_) {
+    bool write_successful = true;
     if (force_mode_active_) {
       const auto force_mode_parameters = force_mode_params_buffer_.readFromRT();
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(force_mode_parameters->task_frame[0]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(force_mode_parameters->task_frame[1]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(force_mode_parameters->task_frame[2]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(force_mode_parameters->task_frame[3]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(force_mode_parameters->task_frame[4]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(force_mode_parameters->task_frame[5]);
-
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_X].set_value(
+          force_mode_parameters->task_frame[0]);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Y].set_value(
+          force_mode_parameters->task_frame[1]);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_Z].set_value(
+          force_mode_parameters->task_frame[2]);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RX].set_value(
+          force_mode_parameters->task_frame[3]);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RY].set_value(
+          force_mode_parameters->task_frame[4]);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_TASK_FRAME_RZ].set_value(
+          force_mode_parameters->task_frame[5]);
+
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_X].set_value(
           force_mode_parameters->selection_vec[0]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Y].set_value(
           force_mode_parameters->selection_vec[1]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_Z].set_value(
           force_mode_parameters->selection_vec[2]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RX].set_value(
           force_mode_parameters->selection_vec[3]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RY].set_value(
           force_mode_parameters->selection_vec[4]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_SELECTION_VECTOR_RZ].set_value(
           force_mode_parameters->selection_vec[5]);
 
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(force_mode_parameters->wrench.force.x);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(force_mode_parameters->wrench.force.y);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(force_mode_parameters->wrench.force.z);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(force_mode_parameters->wrench.torque.x);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(force_mode_parameters->wrench.torque.y);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(force_mode_parameters->wrench.torque.z);
-
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]);
-
-      command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(force_mode_parameters->gain_scaling);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_X].set_value(force_mode_parameters->wrench.force.x);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Y].set_value(force_mode_parameters->wrench.force.y);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_Z].set_value(force_mode_parameters->wrench.force.z);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RX].set_value(
+          force_mode_parameters->wrench.torque.x);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RY].set_value(
+          force_mode_parameters->wrench.torque.y);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_WRENCH_RZ].set_value(
+          force_mode_parameters->wrench.torque.z);
+
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(force_mode_parameters->limits[0]);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(force_mode_parameters->limits[1]);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(force_mode_parameters->limits[2]);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(force_mode_parameters->limits[3]);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(force_mode_parameters->limits[4]);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(force_mode_parameters->limits[5]);
+
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_TYPE].set_value(force_mode_parameters->type);
+      write_successful &=
+          command_interfaces_[CommandInterfaces::FORCE_MODE_DAMPING].set_value(force_mode_parameters->damping_factor);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_GAIN_SCALING].set_value(
+          force_mode_parameters->gain_scaling);
 
       // Signal that we are waiting for confirmation that force mode is activated
-      command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
       async_state_ = ASYNC_WAITING;
     } else {
-      command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0);
-      command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_DISABLE_CMD].set_value(1.0);
+      write_successful &= command_interfaces_[CommandInterfaces::FORCE_MODE_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
       async_state_ = ASYNC_WAITING;
     }
+    if (!write_successful) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not write to a command interfaces.");
+      return controller_interface::return_type::ERROR;
+    }
     change_requested_ = false;
   }
 
diff --git a/ur_controllers/src/freedrive_mode_controller.cpp b/ur_controllers/src/freedrive_mode_controller.cpp
index e1ae016e5..50f893e7a 100644
--- a/ur_controllers/src/freedrive_mode_controller.cpp
+++ b/ur_controllers/src/freedrive_mode_controller.cpp
@@ -153,7 +153,10 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != command_interfaces_.end()) {
       abort_command_interface_ = *it;
-      abort_command_interface_->get().set_value(0.0);
+      if (!abort_command_interface_->get().set_value(0.0)) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Could not write to abort command interface.");
+        return controller_interface::CallbackReturn::ERROR;
+      }
     } else {
       RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str());
       return controller_interface::CallbackReturn::ERROR;
@@ -166,7 +169,10 @@ ur_controllers::FreedriveModeController::on_activate(const rclcpp_lifecycle::Sta
 controller_interface::CallbackReturn
 ur_controllers::FreedriveModeController::on_cleanup(const rclcpp_lifecycle::State& /*previous_state*/)
 {
-  abort_command_interface_->get().set_value(1.0);
+  if (!abort_command_interface_->get().set_value(1.0)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not write to abort command interface.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
 
   stop_logging_thread();
 
@@ -190,6 +196,7 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
   async_state_ = async_success_command_interface_->get().get_value();
 
   if (change_requested_) {
+    bool write_success = true;
     if (freedrive_active_) {
       // Check if the freedrive mode has been aborted from the hardware interface. E.g. the robot was stopped on the
       // teach pendant.
@@ -202,22 +209,27 @@ controller_interface::return_type ur_controllers::FreedriveModeController::updat
         RCLCPP_INFO(get_node()->get_logger(), "Received command to start Freedrive Mode.");
 
         // Set command interface to enable
-        enable_command_interface_->get().set_value(1.0);
+        write_success &= enable_command_interface_->get().set_value(1.0);
 
-        async_success_command_interface_->get().set_value(ASYNC_WAITING);
+        write_success &= async_success_command_interface_->get().set_value(ASYNC_WAITING);
         async_state_ = ASYNC_WAITING;
       }
 
     } else {
       RCLCPP_INFO(get_node()->get_logger(), "Received command to stop Freedrive Mode.");
 
-      abort_command_interface_->get().set_value(1.0);
+      write_success &= abort_command_interface_->get().set_value(1.0);
 
-      async_success_command_interface_->get().set_value(ASYNC_WAITING);
+      write_success &= async_success_command_interface_->get().set_value(ASYNC_WAITING);
       async_state_ = ASYNC_WAITING;
     }
     first_log_ = true;
     change_requested_ = false;
+
+    if (!write_success) {
+      RCLCPP_ERROR(get_node()->get_logger(), "Could not write to a command interfaces.");
+      return controller_interface::return_type::ERROR;
+    }
   }
 
   if ((async_state_ == 1.0) && (first_log_)) {
diff --git a/ur_controllers/src/gpio_controller.cpp b/ur_controllers/src/gpio_controller.cpp
index 188542d0e..64d3ee074 100644
--- a/ur_controllers/src/gpio_controller.cpp
+++ b/ur_controllers/src/gpio_controller.cpp
@@ -345,8 +345,8 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
 {
   if (req->fun == req->FUN_SET_DIGITAL_OUT && req->pin >= 0 && req->pin <= 17) {
     // io async success
-    command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-    command_interfaces_[req->pin].set_value(static_cast<double>(req->state));
+    std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+    std::ignore = command_interfaces_[req->pin].set_value(static_cast<double>(req->state));
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting digital output '%d' to state: '%1.0f'.", req->pin, req->state);
 
@@ -359,8 +359,9 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
     return resp->success;
   } else if (req->fun == req->FUN_SET_ANALOG_OUT && req->pin >= 0 && req->pin <= 2) {
     // io async success
-    command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-    command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->pin].set_value(static_cast<double>(req->state));
+    std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+    std::ignore = command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->pin].set_value(
+        static_cast<double>(req->state));
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f'.", req->pin, req->state);
 
@@ -372,8 +373,8 @@ bool GPIOController::setIO(ur_msgs::srv::SetIO::Request::SharedPtr req, ur_msgs:
     resp->success = static_cast<bool>(command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].get_value());
     return resp->success;
   } else if (req->fun == req->FUN_SET_TOOL_VOLTAGE) {
-    command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-    command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast<double>(req->state));
+    std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+    std::ignore = command_interfaces_[CommandInterfaces::TOOL_VOLTAGE_CMD].set_value(static_cast<double>(req->state));
 
     RCLCPP_INFO(get_node()->get_logger(), "Setting tool voltage to: '%1.0f'.", req->state);
 
@@ -407,16 +408,17 @@ bool GPIOController::setAnalogOutput(ur_msgs::srv::SetAnalogOutput::Request::Sha
       return false;
   }
 
-  if (req->data.pin < 0 || req->data.pin > 1) {
+  if (!(req->data.pin == 0 || req->data.pin == 1)) {
     RCLCPP_ERROR(get_node()->get_logger(), "Invalid pin selected. Only pins 0 and 1 are allowed.");
     resp->success = false;
     return false;
   }
 
-  command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-  command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->data.pin].set_value(
+  std::ignore = command_interfaces_[CommandInterfaces::IO_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  std::ignore = command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_CMD + req->data.pin].set_value(
       static_cast<float>(req->data.state));
-  command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast<double>(req->data.domain));
+  std::ignore =
+      command_interfaces_[CommandInterfaces::ANALOG_OUTPUTS_DOMAIN].set_value(static_cast<double>(req->data.domain));
 
   RCLCPP_INFO(get_node()->get_logger(), "Setting analog output '%d' to state: '%f' in domain %s.", req->data.pin,
               req->data.state, domain_string.c_str());
@@ -436,9 +438,9 @@ bool GPIOController::setSpeedSlider(ur_msgs::srv::SetSpeedSliderFraction::Reques
   if (req->speed_slider_fraction >= 0.01 && req->speed_slider_fraction <= 1.0) {
     RCLCPP_INFO(get_node()->get_logger(), "Setting speed slider to %.2f%%.", req->speed_slider_fraction * 100.0);
     // reset success flag
-    command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+    std::ignore = command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
     // set commanding value for speed slider
-    command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_CMD].set_value(
+    std::ignore = command_interfaces_[CommandInterfaces::TARGET_SPEED_FRACTION_CMD].set_value(
         static_cast<double>(req->speed_slider_fraction));
 
     if (!waitForAsyncCommand([&]() {
@@ -462,9 +464,9 @@ bool GPIOController::resendRobotProgram(std_srvs::srv::Trigger::Request::SharedP
                                         std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
   // reset success flag
-  command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  std::ignore = command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware
-  command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_CMD].set_value(1.0);
+  std::ignore = command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_CMD].set_value(1.0);
 
   if (!waitForAsyncCommand(
           [&]() { return command_interfaces_[CommandInterfaces::RESEND_ROBOT_PROGRAM_ASYNC_SUCCESS].get_value(); })) {
@@ -488,9 +490,9 @@ bool GPIOController::handBackControl(std_srvs::srv::Trigger::Request::SharedPtr
                                      std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
   // reset success flag
-  command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  std::ignore = command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware
-  command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_CMD].set_value(1.0);
+  std::ignore = command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_CMD].set_value(1.0);
 
   if (!waitForAsyncCommand(
           [&]() { return command_interfaces_[CommandInterfaces::HAND_BACK_CONTROL_ASYNC_SUCCESS].get_value(); })) {
@@ -514,12 +516,12 @@ bool GPIOController::setPayload(const ur_msgs::srv::SetPayload::Request::SharedP
                                 ur_msgs::srv::SetPayload::Response::SharedPtr resp)
 {
   // reset success flag
-  command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
 
-  command_interfaces_[CommandInterfaces::PAYLOAD_MASS].set_value(req->mass);
-  command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x);
-  command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
-  command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
+  std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_MASS].set_value(req->mass);
+  std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_X].set_value(req->center_of_gravity.x);
+  std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Y].set_value(req->center_of_gravity.y);
+  std::ignore = command_interfaces_[CommandInterfaces::PAYLOAD_COG_Z].set_value(req->center_of_gravity.z);
 
   if (!waitForAsyncCommand(
           [&]() { return command_interfaces_[CommandInterfaces::PAYLOAD_ASYNC_SUCCESS].get_value(); })) {
@@ -543,9 +545,9 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
                                   std_srvs::srv::Trigger::Response::SharedPtr resp)
 {
   // reset success flag
-  command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
+  std::ignore = command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
   // call the service in the hardware
-  command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0);
+  std::ignore = command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_CMD].set_value(1.0);
 
   if (!waitForAsyncCommand(
           [&]() { return command_interfaces_[CommandInterfaces::ZERO_FTSENSOR_ASYNC_SUCCESS].get_value(); })) {
diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp
index 0ae84cffb..ec9efc994 100644
--- a/ur_controllers/src/passthrough_trajectory_controller.cpp
+++ b/ur_controllers/src/passthrough_trajectory_controller.cpp
@@ -38,13 +38,14 @@
  */
 //----------------------------------------------------------------------
 
+#include <rclcpp/version.h>
+
 #include <algorithm>
 #include <cmath>
 #include <sstream>
 
 #include <controller_interface/controller_interface.hpp>
 #include <rclcpp/logging.hpp>
-#include <rclcpp/version.h>
 #include <builtin_interfaces/msg/duration.hpp>
 #include <lifecycle_msgs/msg/state.hpp>
 
@@ -208,7 +209,10 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat
 
 controller_interface::CallbackReturn PassthroughTrajectoryController::on_deactivate(const rclcpp_lifecycle::State&)
 {
-  abort_command_interface_->get().set_value(1.0);
+  if (!abort_command_interface_->get().set_value(1.0)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not write to abort command interface.");
+    return controller_interface::CallbackReturn::ERROR;
+  }
   if (trajectory_active_) {
     const auto active_goal = *rt_active_goal_.readFromRT();
     std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result> result =
@@ -228,6 +232,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
 
   const auto current_transfer_state = transfer_command_interface_->get().get_value();
 
+  bool write_success = true;
   if (active_goal && trajectory_active_) {
     if (current_transfer_state != TRANSFER_STATE_IDLE) {
       // Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach
@@ -248,7 +253,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
       active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0);
       max_trajectory_time_ =
           rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start));
-      transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT);
+      write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT);
     }
     auto active_goal_time_tol = goal_time_tolerance_.readFromRT();
     auto joint_mapping = joint_trajectory_mapping_.readFromRT();
@@ -257,7 +262,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
     if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) {
       if (current_index_ < active_joint_traj_.points.size()) {
         //  Write the time_from_start parameter.
-        time_from_start_command_interface_->get().set_value(
+        write_success &= time_from_start_command_interface_->get().set_value(
             duration_to_double(active_joint_traj_.points[current_index_].time_from_start));
 
         // Write the positions for each joint of the robot
@@ -265,30 +270,30 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
         // We've added the joint interfaces matching the order of the joint names so we can safely access
         // them by the index.
         for (size_t i = 0; i < number_of_joints_; i++) {
-          command_interfaces_[i * 3].set_value(
+          write_success &= command_interfaces_[i * 3].set_value(
               active_joint_traj_.points[current_index_].positions[joint_mapping->at(joint_names_internal->at(i))]);
           // Optionally, also write velocities and accelerations for each joint.
           if (active_joint_traj_.points[current_index_].velocities.size() > 0) {
-            command_interfaces_[i * 3 + 1].set_value(
+            write_success &= command_interfaces_[i * 3 + 1].set_value(
                 active_joint_traj_.points[current_index_].velocities[joint_mapping->at(joint_names_internal->at(i))]);
             if (active_joint_traj_.points[current_index_].accelerations.size() > 0) {
-              command_interfaces_[i * 3 + 2].set_value(
+              write_success &= command_interfaces_[i * 3 + 2].set_value(
                   active_joint_traj_.points[current_index_]
                       .accelerations[joint_mapping->at(joint_names_internal->at(i))]);
             } else {
-              command_interfaces_[i * 3 + 2].set_value(NO_VAL);
+              write_success &= command_interfaces_[i * 3 + 2].set_value(NO_VAL);
             }
           } else {
-            command_interfaces_[i * 3 + 1].set_value(NO_VAL);
-            command_interfaces_[i * 3 + 2].set_value(NO_VAL);
+            write_success &= command_interfaces_[i * 3 + 1].set_value(NO_VAL);
+            write_success &= command_interfaces_[i * 3 + 2].set_value(NO_VAL);
           }
         }
         // Tell hardware interface that this point is ready to be read.
-        transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFERRING);
+        write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFERRING);
         current_index_++;
         // Check if all points have been written to the hardware interface.
       } else if (current_index_ == active_joint_traj_.points.size()) {
-        transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFER_DONE);
+        write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFER_DONE);
       } else {
         RCLCPP_ERROR(get_node()->get_logger(), "Hardware waiting for trajectory point while none is present!");
       }
@@ -344,12 +349,16 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
     }
   } else if (current_transfer_state != TRANSFER_STATE_IDLE && current_transfer_state != TRANSFER_STATE_DONE) {
     // No goal is active, but we are not in IDLE, either. We have been canceled.
-    abort_command_interface_->get().set_value(1.0);
+    write_success &= abort_command_interface_->get().set_value(1.0);
 
   } else if (current_transfer_state == TRANSFER_STATE_DONE) {
     // We have been informed about the finished trajectory. Let's reset things.
-    transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE);
-    abort_command_interface_->get().set_value(0.0);
+    write_success &= transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE);
+    write_success &= abort_command_interface_->get().set_value(0.0);
+  }
+  if (!write_success) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not write to a command interfaces.");
+    return controller_interface::return_type::ERROR;
   }
 
   return controller_interface::return_type::OK;
@@ -606,7 +615,9 @@ bool PassthroughTrajectoryController::check_goal_tolerance()
 void PassthroughTrajectoryController::end_goal()
 {
   trajectory_active_ = false;
-  transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE);
+  if (!transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE)) {
+    RCLCPP_ERROR(get_node()->get_logger(), "Could not write to transfer command interface.");
+  }
 }
 
 std::unordered_map<std::string, size_t>
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
index 0fdb5c6c6..4b99cee75 100644
--- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp
+++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -141,9 +141,11 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   // TODO(anyone): can I here also use const on joint_interface since the reference_wrapper is not
   // changed, but its value only?
   auto assign_interface_from_point = [&](auto& joint_interface, const std::vector<double>& trajectory_point_interface) {
+    bool success = true;
     for (size_t index = 0; index < dof_; ++index) {
-      joint_interface[index].get().set_value(trajectory_point_interface[index]);
+      success &= joint_interface[index].get().set_value(trajectory_point_interface[index]);
     }
+    return success;
   };
 
   // current state update
@@ -241,21 +243,26 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
         }
 
         // set values for next hardware write()
+        bool write_success = true;
         if (has_position_command_interface_) {
-          assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
+          write_success &= assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
         }
         if (has_velocity_command_interface_) {
           if (use_closed_loop_pid_adapter_) {
-            assign_interface_from_point(joint_command_interface_[1], tmp_command_);
+            write_success &= assign_interface_from_point(joint_command_interface_[1], tmp_command_);
           } else {
-            assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
+            write_success &= assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
           }
         }
         if (has_acceleration_command_interface_) {
-          assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
+          write_success &= assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
         }
         if (has_effort_command_interface_) {
-          assign_interface_from_point(joint_command_interface_[3], tmp_command_);
+          write_success &= assign_interface_from_point(joint_command_interface_[3], tmp_command_);
+        }
+        if (!write_success) {
+          RCLCPP_ERROR(get_node()->get_logger(), "Could not write to a command interfaces.");
+          return controller_interface::return_type::ERROR;
         }
 
         // store the previous command. Used in open-loop control mode
diff --git a/ur_controllers/src/ur_configuration_controller.cpp b/ur_controllers/src/ur_configuration_controller.cpp
index ee131cc52..2c118fdcb 100644
--- a/ur_controllers/src/ur_configuration_controller.cpp
+++ b/ur_controllers/src/ur_configuration_controller.cpp
@@ -39,7 +39,7 @@
 //----------------------------------------------------------------------
 
 #include <ur_controllers/ur_configuration_controller.hpp>
-#include <realtime_tools/realtime_box.h>
+#include <realtime_tools/realtime_box.hpp>
 namespace ur_controllers
 {