diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
index 77ba55812a..a0d99f755d 100644
--- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp
@@ -56,18 +56,39 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom
 
 bool AckermannSteeringController::update_odometry(const rclcpp::Duration & period)
 {
+  auto logger = get_node()->get_logger();
+
   if (params_.open_loop)
   {
     odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
   }
   else
   {
-    const double traction_right_wheel_value =
-      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double traction_left_wheel_value =
-      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double steering_right_position = state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value();
-    const double steering_left_position = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_value();
+    const auto traction_right_wheel_value_op =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional();
+    const auto traction_left_wheel_value_op =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional();
+    const auto steering_right_position_op =
+      state_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional();
+    const auto steering_left_position_op = state_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional();
+
+    if (
+      !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() ||
+      !steering_right_position_op.has_value() || !steering_left_position_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        logger,
+        "Unable to retrieve the data from right wheel or left wheel or right steering position or "
+        "left steering position!");
+
+      return true;
+    }
+
+    const double traction_right_wheel_value = traction_right_wheel_value_op.value();
+    const double traction_left_wheel_value = traction_left_wheel_value_op.value();
+    const double steering_right_position = steering_right_position_op.value();
+    const double steering_left_position = steering_left_position_op.value();
+
     if (
       std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
       std::isfinite(steering_right_position) && std::isfinite(steering_left_position))
diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
index 15eb39f1a4..af8af22d7c 100644
--- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
+++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp
@@ -143,9 +143,9 @@ TEST_F(AckermannSteeringControllerTest, reactivate_success)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -176,17 +176,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic)
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
 
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
@@ -216,17 +216,17 @@ TEST_F(AckermannSteeringControllerTest, test_update_logic_chained)
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[STATE_STEER_RIGHT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[STATE_STEER_LEFT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
 
   EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
   EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size());
@@ -266,17 +266,17 @@ TEST_F(AckermannSteeringControllerTest, receive_message_and_publish_updated_stat
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_STEER_RIGHT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_value(), 1.4179821977774734,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_STEER_LEFT_WHEEL].get_optional().value(),
+    1.4179821977774734, COMMON_THRESHOLD);
 
   subscribe_and_get_messages(msg);
 
diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp
index b4c0586473..deb228c0c8 100644
--- a/admittance_controller/src/admittance_controller.cpp
+++ b/admittance_controller/src/admittance_controller.cpp
@@ -526,21 +526,37 @@ void AdmittanceController::read_state_from_hardware(
   {
     if (has_position_state_interface_)
     {
-      state_current.positions[joint_ind] =
-        state_interfaces_[pos_ind * num_joints_ + joint_ind].get_value();
-      nan_position |= std::isnan(state_current.positions[joint_ind]);
+      auto state_current_position_op =
+        state_interfaces_[pos_ind * num_joints_ + joint_ind].get_optional();
+      nan_position |=
+        !state_current_position_op.has_value() || std::isnan(state_current_position_op.has_value());
+      if (state_current_position_op.has_value())
+      {
+        state_current.positions[joint_ind] = state_current_position_op.value();
+      }
     }
     if (has_velocity_state_interface_)
     {
-      state_current.velocities[joint_ind] =
-        state_interfaces_[vel_ind * num_joints_ + joint_ind].get_value();
-      nan_velocity |= std::isnan(state_current.velocities[joint_ind]);
+      auto state_current_velocity_op =
+        state_interfaces_[vel_ind * num_joints_ + joint_ind].get_optional();
+      nan_velocity |=
+        !state_current_velocity_op.has_value() || std::isnan(state_current_velocity_op.value());
+
+      if (state_current_velocity_op.has_value())
+      {
+        state_current.velocities[joint_ind] = state_current_velocity_op.value();
+      }
     }
     if (has_acceleration_state_interface_)
     {
-      state_current.accelerations[joint_ind] =
-        state_interfaces_[acc_ind * num_joints_ + joint_ind].get_value();
-      nan_acceleration |= std::isnan(state_current.accelerations[joint_ind]);
+      auto state_current_acceleration_op =
+        state_interfaces_[acc_ind * num_joints_ + joint_ind].get_optional();
+      nan_acceleration |= !state_current_acceleration_op.has_value() ||
+                          std::isnan(state_current_acceleration_op.has_value());
+      if (state_current_acceleration_op.has_value())
+      {
+        state_current.accelerations[joint_ind] = state_current_acceleration_op.value();
+      }
     }
   }
 
@@ -576,23 +592,31 @@ void AdmittanceController::write_state_to_hardware(
   size_t vel_ind =
     (has_position_command_interface_) ? pos_ind + has_velocity_command_interface_ : pos_ind;
   size_t acc_ind = vel_ind + has_acceleration_command_interface_;
+
+  auto logger = get_node()->get_logger();
+
   for (size_t joint_ind = 0; joint_ind < num_joints_; ++joint_ind)
   {
+    bool success = true;
     if (has_position_command_interface_)
     {
-      command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
+      success &= command_interfaces_[pos_ind * num_joints_ + joint_ind].set_value(
         state_commanded.positions[joint_ind]);
     }
     if (has_velocity_command_interface_)
     {
-      command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
+      success &= command_interfaces_[vel_ind * num_joints_ + joint_ind].set_value(
         state_commanded.velocities[joint_ind]);
     }
     if (has_acceleration_command_interface_)
     {
-      command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
+      success &= command_interfaces_[acc_ind * num_joints_ + joint_ind].set_value(
         state_commanded.accelerations[joint_ind]);
     }
+    if (!success)
+    {
+      RCLCPP_WARN(logger, "Error while setting command for joint %zu.", joint_ind);
+    }
   }
   last_commanded_ = state_commanded;
 }
diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
index 95eaf1965c..db8fa2f76f 100644
--- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp
@@ -54,14 +54,27 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet
 
 bool BicycleSteeringController::update_odometry(const rclcpp::Duration & period)
 {
+  auto logger = get_node()->get_logger();
+
   if (params_.open_loop)
   {
     odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds());
   }
   else
   {
-    const double traction_wheel_value = state_interfaces_[STATE_TRACTION_WHEEL].get_value();
-    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    const auto traction_wheel_value_op = state_interfaces_[STATE_TRACTION_WHEEL].get_optional();
+    const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional();
+
+    if (!traction_wheel_value_op.has_value() || !steering_position_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        logger, "Unable to retrieve the data from the traction wheel or steering position!");
+      return true;
+    }
+
+    const double traction_wheel_value = traction_wheel_value_op.value();
+    const double steering_position = steering_position_op.value();
+
     if (std::isfinite(traction_wheel_value) && std::isfinite(steering_position))
     {
       if (params_.position_feedback)
diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
index 9c41f5fd47..11a86ec01e 100644
--- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
+++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp
@@ -127,9 +127,9 @@ TEST_F(BicycleSteeringControllerTest, reactivate_success)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -159,9 +159,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
+    COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
@@ -191,9 +192,10 @@ TEST_F(BicycleSteeringControllerTest, test_update_logic_chained)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
+    COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
@@ -246,9 +248,10 @@ TEST_F(BicycleSteeringControllerTest, receive_message_and_publish_updated_status
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_value(), 0.1 / 0.45, COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_WHEEL].get_optional().value(), 0.1 / 0.45,
+    COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   subscribe_and_get_messages(msg);
diff --git a/effort_controllers/src/joint_group_effort_controller.cpp b/effort_controllers/src/joint_group_effort_controller.cpp
index e7d0f274a5..b0e2890ec1 100644
--- a/effort_controllers/src/joint_group_effort_controller.cpp
+++ b/effort_controllers/src/joint_group_effort_controller.cpp
@@ -55,11 +55,14 @@ controller_interface::CallbackReturn JointGroupEffortController::on_deactivate(
   const rclcpp_lifecycle::State & previous_state)
 {
   auto ret = ForwardCommandController::on_deactivate(previous_state);
-
   // stop all joints
   for (auto & command_interface : command_interfaces_)
   {
-    command_interface.set_value(0.0);
+    if (!command_interface.set_value(0.0))
+    {
+      RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface value to 0.0");
+    }
+    return controller_interface::CallbackReturn::SUCCESS;
   }
 
   return ret;
diff --git a/effort_controllers/test/test_joint_group_effort_controller.cpp b/effort_controllers/test/test_joint_group_effort_controller.cpp
index 1ffa757478..0378f64223 100644
--- a/effort_controllers/test/test_joint_group_effort_controller.cpp
+++ b/effort_controllers/test/test_joint_group_effort_controller.cpp
@@ -106,9 +106,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   // send command
   auto command_ptr = std::make_shared<forward_command_controller::CmdType>();
@@ -121,9 +121,9 @@ TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands have been modified
-  ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
@@ -143,9 +143,9 @@ TEST_F(JointGroupEffortControllerTest, WrongCommandCheckTest)
     controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
@@ -160,9 +160,9 @@ TEST_F(JointGroupEffortControllerTest, NoCommandCheckTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
@@ -171,9 +171,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
   controller_->get_node()->set_parameter({"joints", joint_names_});
 
   // default values
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   auto node_state = controller_->get_node()->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -204,9 +204,9 @@ TEST_F(JointGroupEffortControllerTest, CommandCallbackTest)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
@@ -218,15 +218,15 @@ TEST_F(JointGroupEffortControllerTest, StopJointsOnDeactivateTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   // stop the controller
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // check joint commands are now zero
-  ASSERT_EQ(joint_1_cmd_.get_value(), 0.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 0.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0);
 }
diff --git a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
index 46c396f7b1..89de8cd3f1 100644
--- a/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
+++ b/force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp
@@ -335,7 +335,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
     ASSERT_EQ(
       exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_);
     ASSERT_EQ(
-      exported_state_interfaces[i]->get_value(),
+      exported_state_interfaces[i]->get_optional().value(),
       sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
   }
 }
@@ -373,8 +373,8 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name);
   ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x");
   ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z");
-  ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]);
-  ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]);
+  ASSERT_EQ(exported_state_interfaces[0]->get_optional().value(), sensor_values_[0]);
+  ASSERT_EQ(exported_state_interfaces[1]->get_optional().value(), sensor_values_[5]);
 }
 
 TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
@@ -423,7 +423,7 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   for (size_t i = 0; i < 6; ++i)
   {
     ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name);
-    ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]);
+    ASSERT_EQ(exported_state_interfaces[i]->get_optional().value(), sensor_values_[i]);
   }
 }
 
diff --git a/forward_command_controller/src/forward_controllers_base.cpp b/forward_command_controller/src/forward_controllers_base.cpp
index 331d3f9eea..1522e767f6 100644
--- a/forward_command_controller/src/forward_controllers_base.cpp
+++ b/forward_command_controller/src/forward_controllers_base.cpp
@@ -118,6 +118,8 @@ controller_interface::CallbackReturn ForwardControllersBase::on_deactivate(
 controller_interface::return_type ForwardControllersBase::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
+  auto logger = get_node()->get_logger();
+
   auto joint_commands = rt_command_ptr_.readFromRT();
 
   // no command received yet
@@ -129,15 +131,25 @@ controller_interface::return_type ForwardControllersBase::update(
   if ((*joint_commands)->data.size() != command_interfaces_.size())
   {
     RCLCPP_ERROR_THROTTLE(
-      get_node()->get_logger(), *(get_node()->get_clock()), 1000,
+      logger, *(get_node()->get_clock()), 1000,
       "command size (%zu) does not match number of interfaces (%zu)",
       (*joint_commands)->data.size(), command_interfaces_.size());
     return controller_interface::return_type::ERROR;
   }
 
+  const auto & data = (*joint_commands)->data;
+
   for (auto index = 0ul; index < command_interfaces_.size(); ++index)
   {
-    command_interfaces_[index].set_value((*joint_commands)->data[index]);
+    if (!command_interfaces_[index].set_value(data[index]))
+    {
+      RCLCPP_WARN(
+        logger, "Error while setting command interface value at index %zu: value = %f", index,
+        data[index]);
+      return controller_interface::return_type::OK;
+    }
+
+    return controller_interface::return_type::OK;
   }
 
   return controller_interface::return_type::OK;
diff --git a/forward_command_controller/test/test_forward_command_controller.cpp b/forward_command_controller/test/test_forward_command_controller.cpp
index fcfa65ee1c..3c1886bcd2 100644
--- a/forward_command_controller/test/test_forward_command_controller.cpp
+++ b/forward_command_controller/test/test_forward_command_controller.cpp
@@ -212,9 +212,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1);
 
   // send command
   auto command_ptr = std::make_shared<forward_command_controller::CmdType>();
@@ -227,9 +227,9 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands have been modified
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest)
@@ -255,9 +255,9 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest)
     controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(ForwardCommandControllerTest, NoCommandCheckTest)
@@ -277,9 +277,9 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
@@ -290,9 +290,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
   controller_->get_node()->set_parameter({"interface_name", "position"});
 
   // default values
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1);
 
   auto node_state = controller_->get_node()->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -323,9 +323,9 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
@@ -336,9 +336,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
   controller_->get_node()->set_parameter({"interface_name", "position"});
 
   // default values
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 3.1);
 
   auto node_state = controller_->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -373,9 +373,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30);
 
   node_state = controller_->get_node()->deactivate();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -425,9 +425,9 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
     controller_interface::return_type::OK);
 
   // values should not change
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 20);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 30);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 20);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 30);
 
   // set commands again
   controller_->rt_command_ptr_.writeFromNonRT(command_msg);
@@ -438,7 +438,7 @@ TEST_F(ForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5);
-  ASSERT_EQ(joint_2_pos_cmd_.get_value(), 6.6);
-  ASSERT_EQ(joint_3_pos_cmd_.get_value(), 7.7);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5);
+  ASSERT_EQ(joint_2_pos_cmd_.get_optional().value(), 6.6);
+  ASSERT_EQ(joint_3_pos_cmd_.get_optional().value(), 7.7);
 }
diff --git a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp
index c52f4f3ab6..7dcf1f35c5 100644
--- a/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp
+++ b/forward_command_controller/test/test_multi_interface_forward_command_controller.cpp
@@ -189,9 +189,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateSuccess)
   SetUpController(true);
 
   // check joint commands are the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest)
@@ -209,9 +209,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest)
@@ -224,9 +224,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, NoCommandCheckTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest)
@@ -244,9 +244,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, WrongCommandCheckTest)
     controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest)
@@ -276,9 +276,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, CommandCallbackTest)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsResetSuccess)
@@ -304,9 +304,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0);
 
   auto node_state = controller_->get_node()->deactivate();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -355,9 +355,9 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes
     controller_interface::return_type::OK);
 
   // values should not change
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 30.0);
 
   // set commands again
   controller_->rt_command_ptr_.writeFromNonRT(command_msg);
@@ -368,7 +368,7 @@ TEST_F(MultiInterfaceForwardCommandControllerTest, ActivateDeactivateCommandsRes
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_pos_cmd_.get_value(), 5.5);
-  ASSERT_EQ(joint_1_vel_cmd_.get_value(), 6.6);
-  ASSERT_EQ(joint_1_eff_cmd_.get_value(), 7.7);
+  ASSERT_EQ(joint_1_pos_cmd_.get_optional().value(), 5.5);
+  ASSERT_EQ(joint_1_vel_cmd_.get_optional().value(), 6.6);
+  ASSERT_EQ(joint_1_eff_cmd_.get_optional().value(), 7.7);
 }
diff --git a/gpio_controllers/src/gpio_command_controller.cpp b/gpio_controllers/src/gpio_command_controller.cpp
index fdecf71c30..f7d6353578 100644
--- a/gpio_controllers/src/gpio_command_controller.cpp
+++ b/gpio_controllers/src/gpio_command_controller.cpp
@@ -350,11 +350,18 @@ void GpioCommandController::apply_command(
   const auto full_command_interface_name =
     gpio_commands.interface_groups[gpio_index] + '/' +
     gpio_commands.interface_values[gpio_index].interface_names[command_interface_index];
+
+  const auto & command_value =
+    gpio_commands.interface_values[gpio_index].values[command_interface_index];
+
   try
   {
-    command_interfaces_map_.at(full_command_interface_name)
-      .get()
-      .set_value(gpio_commands.interface_values[gpio_index].values[command_interface_index]);
+    if (!command_interfaces_map_.at(full_command_interface_name).get().set_value(command_value))
+    {
+      RCLCPP_WARN(
+        get_node()->get_logger(), "Error while setting command for interface '%s' with value '%f'.",
+        full_command_interface_name.c_str(), command_value);
+    }
   }
   catch (const std::exception & e)
   {
@@ -394,8 +401,18 @@ void GpioCommandController::apply_state_value(
     state_msg.interface_values[gpio_index].interface_names[interface_index];
   try
   {
+    auto state_msg_interface_value_op =
+      state_interfaces_map_.at(interface_name).get().get_optional();
+
+    if (!state_msg_interface_value_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        get_node()->get_logger(), "Unable to retrieve the data from state: %s \n",
+        interface_name.c_str());
+    }
+
     state_msg.interface_values[gpio_index].values[interface_index] =
-      state_interfaces_map_.at(interface_name).get().get_value();
+      state_msg_interface_value_op.value();
   }
   catch (const std::exception & e)
   {
diff --git a/gpio_controllers/test/test_gpio_command_controller.cpp b/gpio_controllers/test/test_gpio_command_controller.cpp
index ec0e0f1228..885ee42a46 100644
--- a/gpio_controllers/test/test_gpio_command_controller.cpp
+++ b/gpio_controllers/test/test_gpio_command_controller.cpp
@@ -127,12 +127,12 @@ class GpioCommandControllerTestSuite : public ::testing::Test
 
   void assert_default_command_and_state_values()
   {
-    ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0));
-    ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1));
-    ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2));
-    ASSERT_EQ(gpio_1_1_dig_state.get_value(), gpio_states.at(0));
-    ASSERT_EQ(gpio_1_2_dig_state.get_value(), gpio_states.at(1));
-    ASSERT_EQ(gpio_2_ana_state.get_value(), gpio_states.at(2));
+    ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0));
+    ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1));
+    ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2));
+    ASSERT_EQ(gpio_1_1_dig_state.get_optional().value(), gpio_states.at(0));
+    ASSERT_EQ(gpio_1_2_dig_state.get_optional().value(), gpio_states.at(1));
+    ASSERT_EQ(gpio_2_ana_state.get_optional().value(), gpio_states.at(2));
   }
 
   void update_controller_loop()
@@ -482,9 +482,9 @@ TEST_F(
   controller.rt_command_ptr_.writeFromNonRT(std::make_shared<CmdType>(command));
   update_controller_loop();
 
-  ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0);
-  ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0);
-  ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0);
+  ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0);
+  ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0);
+  ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0);
 }
 
 TEST_F(
@@ -505,9 +505,9 @@ TEST_F(
   controller.rt_command_ptr_.writeFromNonRT(std::make_shared<CmdType>(command));
   update_controller_loop();
 
-  ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0);
-  ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0);
-  ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0);
+  ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0);
+  ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0);
+  ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0);
 }
 
 TEST_F(
@@ -528,9 +528,9 @@ TEST_F(
   controller.rt_command_ptr_.writeFromNonRT(std::make_shared<CmdType>(command));
   update_controller_loop();
 
-  ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0);
-  ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0);
-  ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands[2]);
+  ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0);
+  ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0);
+  ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands[2]);
 }
 
 TEST_F(
@@ -552,9 +552,9 @@ TEST_F(
   controller.rt_command_ptr_.writeFromNonRT(std::make_shared<CmdType>(command));
   update_controller_loop();
 
-  ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), gpio_commands.at(0));
-  ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), gpio_commands.at(1));
-  ASSERT_EQ(gpio_2_ana_cmd.get_value(), gpio_commands.at(2));
+  ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), gpio_commands.at(0));
+  ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), gpio_commands.at(1));
+  ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), gpio_commands.at(2));
 }
 
 TEST_F(
@@ -578,9 +578,9 @@ TEST_F(
   wait_one_miliseconds_for_timeout();
   update_controller_loop();
 
-  ASSERT_EQ(gpio_1_1_dig_cmd.get_value(), 0.0);
-  ASSERT_EQ(gpio_1_2_dig_cmd.get_value(), 1.0);
-  ASSERT_EQ(gpio_2_ana_cmd.get_value(), 30.0);
+  ASSERT_EQ(gpio_1_1_dig_cmd.get_optional().value(), 0.0);
+  ASSERT_EQ(gpio_1_2_dig_cmd.get_optional().value(), 1.0);
+  ASSERT_EQ(gpio_2_ana_cmd.get_optional().value(), 30.0);
 }
 
 TEST_F(GpioCommandControllerTestSuite, ControllerShouldPublishGpioStatesWithCurrentValues)
diff --git a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp
index 034f7e43dd..e40b1cdfce 100644
--- a/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp
+++ b/gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp
@@ -65,8 +65,19 @@ controller_interface::return_type GripperActionController<HardwareInterface>::up
 {
   command_struct_rt_ = *(command_.readFromRT());
 
-  const double current_position = joint_position_state_interface_->get().get_value();
-  const double current_velocity = joint_velocity_state_interface_->get().get_value();
+  const auto current_position_op = joint_position_state_interface_->get().get_optional();
+  const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional();
+
+  if (!current_position_op.has_value() || !current_velocity_op.has_value())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Unable to retrieve the data from current position or current velocity");
+    return controller_interface::return_type::OK;
+  }
+
+  const double current_position = current_position_op.value();
+  const double current_velocity = current_velocity_op.value();
 
   const double error_position = command_struct_rt_.position_ - current_position;
   const double error_velocity = -current_velocity;
@@ -147,7 +158,12 @@ rclcpp_action::CancelResponse GripperActionController<HardwareInterface>::cancel
 template <const char * HardwareInterface>
 void GripperActionController<HardwareInterface>::set_hold_position()
 {
-  command_struct_.position_ = joint_position_state_interface_->get().get_value();
+  const auto command_struct_position_op = joint_position_state_interface_->get().get_optional();
+  if (!command_struct_position_op.has_value())
+  {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position");
+  }
+  command_struct_.position_ = command_struct_position_op.value();
   command_struct_.max_effort_ = params_.max_effort;
   command_.writeFromNonRT(command_struct_);
 }
@@ -287,7 +303,14 @@ controller_interface::CallbackReturn GripperActionController<HardwareInterface>:
   hw_iface_adapter_.init(joint_command_interface_, get_node());
 
   // Command - non RT version
-  command_struct_.position_ = joint_position_state_interface_->get().get_value();
+  auto command_struct_position_op = joint_position_state_interface_->get().get_optional();
+  if (!command_struct_position_op.has_value())
+  {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve the data about joint position");
+    return controller_interface::CallbackReturn::SUCCESS;
+  }
+
+  command_struct_.position_ = command_struct_position_op.value();
   command_struct_.max_effort_ = params_.max_effort;
   command_.initRT(command_struct_);
 
diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp
index 66da3d869a..2ae1ba911a 100644
--- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp
+++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp
@@ -69,9 +69,10 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
 public:
   bool init(
     std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle,
-    const rclcpp_lifecycle::LifecycleNode::SharedPtr & /* node */)
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   {
     joint_handle_ = joint_handle;
+    node_ = node;
     return true;
   }
 
@@ -83,12 +84,18 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_POSITION>
     double /* error_velocity */, double max_allowed_effort)
   {
     // Forward desired position to command
-    joint_handle_->get().set_value(desired_position);
+    if (!joint_handle_->get().set_value(desired_position))
+    {
+      RCLCPP_WARN(
+        node_->get_logger(), "Error while setting desired position to value = %lf",
+        desired_position);
+    }
     return max_allowed_effort;
   }
 
 private:
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 };
 
 /**
@@ -129,6 +136,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node)
   {
     joint_handle_ = joint_handle;
+    node_ = node;
     // Init PID gains from ROS parameter server
     const std::string prefix = "gains." + joint_handle_->get().get_prefix_name();
     const auto k_p = auto_declare<double>(node, prefix + ".p", 0.0);
@@ -148,7 +156,10 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     }
     // Reset PIDs, zero effort commands
     pid_->reset();
-    joint_handle_->get().set_value(0.0);
+    if (!joint_handle_->get().set_value(0.0))
+    {
+      RCLCPP_WARN(node_->get_logger(), "Error while setting joint handle to value 0.0");
+    }
   }
 
   void stopping(const rclcpp::Time & /* time */) {}
@@ -168,7 +179,10 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     double command = pid_->compute_command(error_position, error_velocity, period);
     command = std::min<double>(
       fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
-    joint_handle_->get().set_value(command);
+    if (!joint_handle_->get().set_value(command))
+    {
+      RCLCPP_WARN(node_->get_logger(), "Error while setting command value = %lf", command);
+    }
     last_update_time_ = std::chrono::steady_clock::now();
     return command;
   }
@@ -178,6 +192,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
   PidPtr pid_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> joint_handle_;
   std::chrono::steady_clock::time_point last_update_time_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 };
 
 #endif  // GRIPPER_CONTROLLERS__HARDWARE_INTERFACE_ADAPTER_HPP_
diff --git a/joint_state_broadcaster/src/joint_state_broadcaster.cpp b/joint_state_broadcaster/src/joint_state_broadcaster.cpp
index 5a9d1caeef..993385a2e3 100644
--- a/joint_state_broadcaster/src/joint_state_broadcaster.cpp
+++ b/joint_state_broadcaster/src/joint_state_broadcaster.cpp
@@ -352,11 +352,18 @@ controller_interface::return_type JointStateBroadcaster::update(
     {
       interface_name = map_interface_to_joint_state_[interface_name];
     }
+    const auto state_interface_op = state_interface.get_optional();
+    if (!state_interface_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Unable to retrieve data from state interface %s: ", state_interface.get_name().c_str());
+    }
     name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] =
-      state_interface.get_value();
+      state_interface_op.value();
     RCLCPP_DEBUG(
       get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(),
-      state_interface.get_value());
+      state_interface_op.value());
   }
 
   if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock())
diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp
index 904bcf08d1..4036c61333 100644
--- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp
+++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp
@@ -381,10 +381,26 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   // FORWARD KINEMATICS (odometry).
-  const double wheel_front_left_state_vel = state_interfaces_[FRONT_LEFT].get_value();
-  const double wheel_front_right_state_vel = state_interfaces_[FRONT_RIGHT].get_value();
-  const double wheel_rear_right_state_vel = state_interfaces_[REAR_RIGHT].get_value();
-  const double wheel_rear_left_state_vel = state_interfaces_[REAR_LEFT].get_value();
+  const auto wheel_front_left_state_vel_op = state_interfaces_[FRONT_LEFT].get_optional();
+  const auto wheel_front_right_state_vel_op = state_interfaces_[FRONT_RIGHT].get_optional();
+  const auto wheel_rear_right_state_vel_op = state_interfaces_[REAR_RIGHT].get_optional();
+  const auto wheel_rear_left_state_vel_op = state_interfaces_[REAR_LEFT].get_optional();
+
+  if (
+    !wheel_front_left_state_vel_op.has_value() || !wheel_front_right_state_vel_op.has_value() ||
+    !wheel_rear_right_state_vel_op.has_value() || !wheel_front_left_state_vel_op.has_value())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Unable to retrieve data from front left wheel or front right wheel or rear left wheel or "
+      "rear right wheel");
+    return controller_interface::return_type::OK;
+  }
+
+  const double wheel_front_left_state_vel = wheel_front_left_state_vel_op.value();
+  const double wheel_front_right_state_vel = wheel_front_right_state_vel_op.value();
+  const double wheel_rear_right_state_vel = wheel_rear_right_state_vel_op.value();
+  const double wheel_rear_left_state_vel = wheel_rear_left_state_vel_op.value();
 
   if (
     !std::isnan(wheel_front_left_state_vel) && !std::isnan(wheel_rear_left_state_vel) &&
@@ -503,14 +519,27 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma
   if (controller_state_publisher_->trylock())
   {
     controller_state_publisher_->msg_.header.stamp = get_node()->now();
-    controller_state_publisher_->msg_.front_left_wheel_velocity =
-      state_interfaces_[FRONT_LEFT].get_value();
-    controller_state_publisher_->msg_.front_right_wheel_velocity =
-      state_interfaces_[FRONT_RIGHT].get_value();
-    controller_state_publisher_->msg_.back_right_wheel_velocity =
-      state_interfaces_[REAR_RIGHT].get_value();
-    controller_state_publisher_->msg_.back_left_wheel_velocity =
-      state_interfaces_[REAR_LEFT].get_value();
+    const auto front_left_op = state_interfaces_[FRONT_LEFT].get_optional();
+    const auto front_right_op = state_interfaces_[FRONT_RIGHT].get_optional();
+    const auto rear_right_op = state_interfaces_[REAR_RIGHT].get_optional();
+    const auto rear_left_op = state_interfaces_[REAR_LEFT].get_optional();
+
+    if (
+      !front_left_op.has_value() || !front_right_op.has_value() || !rear_right_op.has_value() ||
+      !rear_left_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Unable to retrieve the data of state interface of front left or front right or rear left "
+        "or rear right");
+      return controller_interface::return_type::OK;
+    }
+
+    controller_state_publisher_->msg_.front_left_wheel_velocity = front_left_op.value();
+    controller_state_publisher_->msg_.front_right_wheel_velocity = front_right_op.value();
+    controller_state_publisher_->msg_.back_right_wheel_velocity = rear_right_op.value();
+    controller_state_publisher_->msg_.back_left_wheel_velocity = rear_left_op.value();
+
     controller_state_publisher_->msg_.reference_velocity.linear.x = reference_interfaces_[0];
     controller_state_publisher_->msg_.reference_velocity.linear.y = reference_interfaces_[1];
     controller_state_publisher_->msg_.reference_velocity.angular.z = reference_interfaces_[2];
diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
index 00c1f5a74b..6cb0364b28 100644
--- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
+++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp
@@ -161,11 +161,11 @@ TEST_F(
 
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value(), 101.101);
+  ASSERT_EQ(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value(), 101.101);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value()));
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[NR_CMD_ITFS - 4].get_optional().value()));
 
   ASSERT_EQ(
     controller_->update(controller_->get_node()->now(), rclcpp::Duration::from_seconds(0.01)),
@@ -383,7 +383,7 @@ TEST_F(
   }
   for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i)
   {
-    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 0.0);
+    EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 0.0);
   }
 
   std::shared_ptr<ControllerReferenceMsg> msg_2 = std::make_shared<ControllerReferenceMsg>();
@@ -422,7 +422,7 @@ TEST_F(
 
   for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i)
   {
-    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 3.0);
+    EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 3.0);
   }
 }
 
@@ -481,7 +481,7 @@ TEST_F(
   }
   for (size_t i = 0; i < controller_->command_interfaces_.size(); ++i)
   {
-    EXPECT_EQ(controller_->command_interfaces_[i].get_value(), 6.0);
+    EXPECT_EQ(controller_->command_interfaces_[i].get_optional().value(), 6.0);
   }
 }
 
@@ -632,10 +632,14 @@ TEST_F(MecanumDriveControllerTest, SideToSideAndRotationOdometryTest)
     size_t fr_index = controller_->get_front_right_wheel_index();
     size_t rl_index = controller_->get_rear_left_wheel_index();
     size_t rr_index = controller_->get_rear_right_wheel_index();
-    joint_state_values_[fl_index] = controller_->command_interfaces_[fl_index].get_value();
-    joint_state_values_[fr_index] = controller_->command_interfaces_[fr_index].get_value();
-    joint_state_values_[rl_index] = controller_->command_interfaces_[rl_index].get_value();
-    joint_state_values_[rr_index] = controller_->command_interfaces_[rr_index].get_value();
+    joint_state_values_[fl_index] =
+      controller_->command_interfaces_[fl_index].get_optional().value();
+    joint_state_values_[fr_index] =
+      controller_->command_interfaces_[fr_index].get_optional().value();
+    joint_state_values_[rl_index] =
+      controller_->command_interfaces_[rl_index].get_optional().value();
+    joint_state_values_[rr_index] =
+      controller_->command_interfaces_[rr_index].get_optional().value();
   }
 
   RCLCPP_INFO(
diff --git a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp
index 5b7e760ca2..8380798031 100644
--- a/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp
+++ b/parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp
@@ -58,21 +58,47 @@ controller_interface::return_type GripperActionController::update(
   const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
 {
   command_struct_rt_ = *(command_.readFromRT());
+  auto logger = get_node()->get_logger();
+
+  const auto current_position_op = joint_position_state_interface_->get().get_optional();
+  const auto current_velocity_op = joint_velocity_state_interface_->get().get_optional();
+
+  if (!current_position_op.has_value() || !current_velocity_op.has_value())
+  {
+    RCLCPP_DEBUG(logger, "Unable to retrieve data of current position or current velocity");
+    return controller_interface::return_type::OK;
+  }
+
+  const double current_position = current_position_op.value();
+  const double current_velocity = current_velocity_op.value();
 
-  const double current_position = joint_position_state_interface_->get().get_value();
-  const double current_velocity = joint_velocity_state_interface_->get().get_value();
   const double error_position = command_struct_rt_.position_cmd_ - current_position;
 
   check_for_success(get_node()->now(), error_position, current_position, current_velocity);
 
-  joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_);
-  if (speed_interface_.has_value())
+  if (!joint_command_interface_->get().set_value(command_struct_rt_.position_cmd_))
+  {
+    RCLCPP_WARN(
+      logger, "Error while setting joint position command to: %f",
+      command_struct_rt_.position_cmd_);
+    return controller_interface::return_type::OK;
+  }
+  if (
+    speed_interface_.has_value() &&
+    !speed_interface_->get().set_value(command_struct_rt_.max_velocity_))
   {
-    speed_interface_->get().set_value(command_struct_rt_.max_velocity_);
+    RCLCPP_WARN(
+      logger, "Error while setting speed command to: %f", command_struct_rt_.max_velocity_);
+
+    return controller_interface::return_type::OK;
   }
-  if (effort_interface_.has_value())
+  if (
+    effort_interface_.has_value() &&
+    !effort_interface_->get().set_value(command_struct_rt_.max_effort_))
   {
-    effort_interface_->get().set_value(command_struct_rt_.max_effort_);
+    RCLCPP_WARN(
+      logger, "Error while setting effort command to: %f", command_struct_rt_.max_effort_);
+    return controller_interface::return_type::OK;
   }
 
   return controller_interface::return_type::OK;
@@ -168,7 +194,12 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback(
 
 void GripperActionController::set_hold_position()
 {
-  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
+  const auto position_op = joint_position_state_interface_->get().get_optional();
+  if (!position_op.has_value())
+  {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position");
+  }
+  command_struct_.position_cmd_ = position_op.value();
   command_struct_.max_effort_ = params_.max_effort;
   command_struct_.max_velocity_ = params_.max_velocity;
   command_.writeFromNonRT(command_struct_);
@@ -318,7 +349,12 @@ controller_interface::CallbackReturn GripperActionController::on_activate(
   }
 
   // Command - non RT version
-  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
+  const auto position_op = joint_position_state_interface_->get().get_optional();
+  if (!position_op.has_value())
+  {
+    RCLCPP_DEBUG(get_node()->get_logger(), "Unable to retrieve data of joint position");
+  }
+  command_struct_.position_cmd_ = position_op.value();
   command_struct_.max_effort_ = params_.max_effort;
   command_struct_.max_velocity_ = params_.max_velocity;
   command_.initRT(command_struct_);
diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp
index 782d39dfdc..0199b6b7e2 100644
--- a/pid_controller/src/pid_controller.cpp
+++ b/pid_controller/src/pid_controller.cpp
@@ -494,7 +494,15 @@ controller_interface::return_type PidController::update_and_write_commands(
   {
     for (size_t i = 0; i < measured_state_values_.size(); ++i)
     {
-      measured_state_values_[i] = state_interfaces_[i].get_value();
+      const auto state_interface_value_op = state_interfaces_[i].get_optional();
+      if (!state_interface_value_op.has_value())
+      {
+        RCLCPP_DEBUG(
+          get_node()->get_logger(), "Unable to retrieve the state interface value for %s",
+          state_interfaces_[i].get_name().c_str());
+        return controller_interface::return_type::OK;
+      }
+      measured_state_values_[i] = state_interface_value_op.value();
     }
   }
 
@@ -599,7 +607,17 @@ controller_interface::return_type PidController::update_and_write_commands(
       state_publisher_->msg_.dof_states[i].time_step = period.seconds();
       // Command can store the old calculated values. This should be obvious because at least one
       // another value is NaN.
-      state_publisher_->msg_.dof_states[i].output = command_interfaces_[i].get_value();
+      const auto command_interface_value_op = command_interfaces_[i].get_optional();
+
+      if (!command_interface_value_op.has_value())
+      {
+        RCLCPP_DEBUG(
+          get_node()->get_logger(), "Unable to retrieve the command interface value for %s",
+          command_interfaces_[i].get_name().c_str());
+        return controller_interface::return_type::OK;
+      }
+
+      state_publisher_->msg_.dof_states[i].output = command_interface_value_op.value();
     }
     state_publisher_->unlockAndPublish();
   }
diff --git a/pid_controller/test/test_pid_controller.cpp b/pid_controller/test/test_pid_controller.cpp
index 1c0494a002..14db82f0e3 100644
--- a/pid_controller/test/test_pid_controller.cpp
+++ b/pid_controller/test/test_pid_controller.cpp
@@ -181,15 +181,15 @@ TEST_F(PidControllerTest, reactivate_success)
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_TRUE(std::isnan(controller_->reference_interfaces_[0]));
   ASSERT_TRUE(std::isnan(controller_->measured_state_values_[0]));
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), 101.101);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), 101.101);
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
@@ -272,7 +272,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_off)
   // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
   const double expected_command_value = 30102.30102;
 
-  double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  double actual_value =
+    std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
 }
 
@@ -331,7 +332,8 @@ TEST_F(PidControllerTest, test_update_logic_feedforward_on_with_zero_feedforward
     // -> cmd = p_term + i_term + d_term + feedforward_gain * ref = 30102.3 + 0 * 101.101 = 30102.3
     const double expected_command_value = 30102.301020;
 
-    double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+    double actual_value =
+      std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
     EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
   }
 }
@@ -388,7 +390,7 @@ TEST_F(PidControllerTest, test_update_logic_chainable_not_use_subscriber_update)
   // feedforward OFF -> cmd = p_term + i_term + d_term = 3.9 + 0.078 + 1170.0 = 1173.978
   const double expected_command_value = 1173.978;
 
-  EXPECT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+  EXPECT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
 }
 
 /**
@@ -415,7 +417,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_off)
 
   // check the result of the commands - the values are not wrapped
   const double expected_command_value = 2679.078;
-  EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
+  EXPECT_NEAR(
+    controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5);
 }
 
 /**
@@ -446,7 +449,8 @@ TEST_F(PidControllerTest, test_update_logic_angle_wraparound_on)
 
   // Check the command value
   const double expected_command_value = 787.713559;
-  EXPECT_NEAR(controller_->command_interfaces_[0].get_value(), expected_command_value, 1e-5);
+  EXPECT_NEAR(
+    controller_->command_interfaces_[0].get_optional().value(), expected_command_value, 1e-5);
 }
 
 TEST_F(PidControllerTest, subscribe_and_get_messages_success)
@@ -583,7 +587,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_with_gain)
     controller_interface::return_type::OK);
 
   // check on result from update
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
 }
 
 /**
@@ -641,7 +645,7 @@ TEST_F(PidControllerTest, test_update_chained_feedforward_off_with_gain)
     controller_interface::return_type::OK);
 
   // check on result from update
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), expected_command_value);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), expected_command_value);
 }
 
 /**
@@ -677,7 +681,8 @@ TEST_F(PidControllerTest, test_save_i_term_off)
   // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
   const double expected_command_value = 30102.30102;
 
-  double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  double actual_value =
+    std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
 
   // deactivate the controller and set command=state
@@ -690,7 +695,7 @@ TEST_F(PidControllerTest, test_save_i_term_off)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-  actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, 0.0, 1e-5);
 }
 
@@ -727,7 +732,8 @@ TEST_F(PidControllerTest, test_save_i_term_on)
   // feedforward OFF -> cmd = p_term + i_term + d_term = 30102.3
   const double expected_command_value = 30102.30102;
 
-  double actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  double actual_value =
+    std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, expected_command_value, 1e-5);
 
   // deactivate the controller and set command=state
@@ -740,7 +746,7 @@ TEST_F(PidControllerTest, test_save_i_term_on)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-  actual_value = std::round(controller_->command_interfaces_[0].get_value() * 1e5) / 1e5;
+  actual_value = std::round(controller_->command_interfaces_[0].get_optional().value() * 1e5) / 1e5;
   EXPECT_NEAR(actual_value, 2.00002, 1e-5);  // i_term from above
 }
 
diff --git a/pid_controller/test/test_pid_controller_dual_interface.cpp b/pid_controller/test/test_pid_controller_dual_interface.cpp
index e006986473..f5b1442a57 100644
--- a/pid_controller/test/test_pid_controller_dual_interface.cpp
+++ b/pid_controller/test/test_pid_controller_dual_interface.cpp
@@ -96,8 +96,8 @@ TEST_F(PidControllerDualInterfaceTest, test_chained_feedforward_with_gain_dual_i
   // check the commands
   const double joint1_expected_cmd = 8.915;
   const double joint2_expected_cmd = 9.915;
-  ASSERT_EQ(controller_->command_interfaces_[0].get_value(), joint1_expected_cmd);
-  ASSERT_EQ(controller_->command_interfaces_[1].get_value(), joint2_expected_cmd);
+  ASSERT_EQ(controller_->command_interfaces_[0].get_optional().value(), joint1_expected_cmd);
+  ASSERT_EQ(controller_->command_interfaces_[1].get_optional().value(), joint2_expected_cmd);
 }
 
 int main(int argc, char ** argv)
diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp
index 02f5449b4c..88a62f35a8 100644
--- a/tricycle_controller/src/tricycle_controller.cpp
+++ b/tricycle_controller/src/tricycle_controller.cpp
@@ -110,8 +110,19 @@ controller_interface::return_type TricycleController::update(
   TwistStamped command = *last_command_msg_;
   double & linear_command = command.twist.linear.x;
   double & angular_command = command.twist.angular.z;
-  double Ws_read = traction_joint_[0].velocity_state.get().get_value();     // in radians/s
-  double alpha_read = steering_joint_[0].position_state.get().get_value();  // in radians
+  auto Ws_read_op = traction_joint_[0].velocity_state.get().get_optional();
+  auto alpha_read_op = steering_joint_[0].position_state.get().get_optional();
+
+  if (!Ws_read_op.has_value() || !alpha_read_op.has_value())
+  {
+    RCLCPP_DEBUG(
+      get_node()->get_logger(),
+      "Unable to retrieve the data of traction joint velocity or steering joint position");
+    return controller_interface::return_type::OK;
+  }
+
+  double Ws_read = Ws_read_op.value();        // in radians/s
+  double alpha_read = alpha_read_op.value();  // in radians
 
   if (params_.open_loop)
   {
@@ -211,8 +222,18 @@ controller_interface::return_type TricycleController::update(
     realtime_ackermann_command_publisher_->unlockAndPublish();
   }
 
-  traction_joint_[0].velocity_command.get().set_value(Ws_write);
-  steering_joint_[0].position_command.get().set_value(alpha_write);
+  if (!traction_joint_[0].velocity_command.get().set_value(Ws_write))
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Error while setting velocity command for traction joint to value: '%f'.", Ws_write);
+  }
+  if (!steering_joint_[0].position_command.get().set_value(alpha_write))
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Error while setting position command for steering joint to value: '%f'.", alpha_write);
+  }
   return controller_interface::return_type::OK;
 }
 
@@ -433,8 +454,18 @@ bool TricycleController::reset()
 
 void TricycleController::halt()
 {
-  traction_joint_[0].velocity_command.get().set_value(0.0);
-  steering_joint_[0].position_command.get().set_value(0.0);
+  if (!traction_joint_[0].velocity_command.get().set_value(0.0))
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Error while setting velocity command for traction joint to value 0.0");
+  }
+  if (!steering_joint_[0].position_command.get().set_value(0.0))
+  {
+    RCLCPP_WARN(
+      get_node()->get_logger(),
+      "Error while setting position command for steering joint to value 0.0");
+  }
 }
 
 CallbackReturn TricycleController::get_traction(
diff --git a/tricycle_controller/test/test_tricycle_controller.cpp b/tricycle_controller/test/test_tricycle_controller.cpp
index d8d4f2cf63..f084da12a5 100644
--- a/tricycle_controller/test/test_tricycle_controller.cpp
+++ b/tricycle_controller/test/test_tricycle_controller.cpp
@@ -284,15 +284,15 @@ TEST_F(TestTricycleController, cleanup)
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
 
   // should be stopped
-  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value());
-  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value());
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value());
+  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value());
 
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
 
   // should be stopped
-  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value());
-  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value());
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value());
+  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value());
 
   executor.cancel();
 }
@@ -312,8 +312,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters)
   assignResources();
 
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  EXPECT_EQ(position_, steering_joint_pos_cmd_.get_value());
-  EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_value());
+  EXPECT_EQ(position_, steering_joint_pos_cmd_.get_optional().value());
+  EXPECT_EQ(velocity_, traction_joint_vel_cmd_.get_optional().value());
 
   state = controller_->get_node()->activate();
   ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
@@ -328,8 +328,8 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters)
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
-  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value());
-  EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_value());
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value());
+  EXPECT_EQ(1.0, traction_joint_vel_cmd_.get_optional().value());
 
   // deactivated
   // wait so controller process the second point when deactivated
@@ -340,14 +340,16 @@ TEST_F(TestTricycleController, correct_initialization_using_parameters)
   state = controller_->get_node()->deactivate();
   ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
 
-  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value()) << "Wheels are halted on deactivate()";
-  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value()) << "Wheels are halted on deactivate()";
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value())
+    << "Wheels are halted on deactivate()";
+  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value())
+    << "Wheels are halted on deactivate()";
 
   // cleanup
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
-  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_value());
-  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_value());
+  EXPECT_EQ(0.0, steering_joint_pos_cmd_.get_optional().value());
+  EXPECT_EQ(0.0, traction_joint_vel_cmd_.get_optional().value());
 
   state = controller_->get_node()->configure();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
index 03be6b88f0..a8adc8a13d 100644
--- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp
@@ -60,11 +60,24 @@ bool TricycleSteeringController::update_odometry(const rclcpp::Duration & period
   }
   else
   {
-    const double traction_right_wheel_value =
-      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_value();
-    const double traction_left_wheel_value =
-      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_value();
-    const double steering_position = state_interfaces_[STATE_STEER_AXIS].get_value();
+    const auto traction_right_wheel_value_op =
+      state_interfaces_[STATE_TRACTION_RIGHT_WHEEL].get_optional();
+    const auto traction_left_wheel_value_op =
+      state_interfaces_[STATE_TRACTION_LEFT_WHEEL].get_optional();
+    const auto steering_position_op = state_interfaces_[STATE_STEER_AXIS].get_optional();
+
+    if (
+      !traction_right_wheel_value_op.has_value() || !traction_left_wheel_value_op.has_value() ||
+      !steering_position_op.value())
+    {
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Unable to retrieve the data from right wheel or left wheel or steering position");
+      return true;
+    }
+    const double traction_right_wheel_value = traction_right_wheel_value_op.value();
+    const double traction_left_wheel_value = traction_left_wheel_value_op.value();
+    const double steering_position = steering_position_op.value();
     if (
       std::isfinite(traction_right_wheel_value) && std::isfinite(traction_left_wheel_value) &&
       std::isfinite(steering_position))
diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
index 93c92c865f..50b14d5dc4 100644
--- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
+++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp
@@ -135,9 +135,9 @@ TEST_F(TricycleSteeringControllerTest, reactivate_success)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
-  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value()));
+  ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_optional().value()));
 
   ASSERT_EQ(
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
@@ -167,13 +167,13 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic)
     controller_interface::return_type::OK);
 
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
@@ -204,13 +204,13 @@ TEST_F(TricycleSteeringControllerTest, test_update_logic_chained)
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x));
@@ -250,13 +250,13 @@ TEST_F(TricycleSteeringControllerTest, receive_message_and_publish_updated_statu
 
   // we test with open_loop=false, but steering angle was not updated (is zero) -> same commands
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_RIGHT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_value(), 0.22222222222222224,
-    COMMON_THRESHOLD);
+    controller_->command_interfaces_[CMD_TRACTION_LEFT_WHEEL].get_optional().value(),
+    0.22222222222222224, COMMON_THRESHOLD);
   EXPECT_NEAR(
-    controller_->command_interfaces_[CMD_STEER_WHEEL].get_value(), 1.4179821977774734,
+    controller_->command_interfaces_[CMD_STEER_WHEEL].get_optional().value(), 1.4179821977774734,
     COMMON_THRESHOLD);
 
   subscribe_and_get_messages(msg);
diff --git a/velocity_controllers/src/joint_group_velocity_controller.cpp b/velocity_controllers/src/joint_group_velocity_controller.cpp
index 97f6713d72..4e4bb18db4 100644
--- a/velocity_controllers/src/joint_group_velocity_controller.cpp
+++ b/velocity_controllers/src/joint_group_velocity_controller.cpp
@@ -59,7 +59,11 @@ controller_interface::CallbackReturn JointGroupVelocityController::on_deactivate
   // stop all joints
   for (auto & command_interface : command_interfaces_)
   {
-    command_interface.set_value(0.0);
+    if (!command_interface.set_value(0.0))
+    {
+      RCLCPP_WARN(get_node()->get_logger(), "Error while setting command interface to value 0.0");
+      return controller_interface::CallbackReturn::SUCCESS;
+    }
   }
 
   return ret;
diff --git a/velocity_controllers/test/test_joint_group_velocity_controller.cpp b/velocity_controllers/test/test_joint_group_velocity_controller.cpp
index 31ba67ec60..1346b3fe70 100644
--- a/velocity_controllers/test/test_joint_group_velocity_controller.cpp
+++ b/velocity_controllers/test/test_joint_group_velocity_controller.cpp
@@ -106,9 +106,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   // send command
   auto command_ptr = std::make_shared<forward_command_controller::CmdType>();
@@ -121,9 +121,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandSuccessTest)
     controller_interface::return_type::OK);
 
   // check joint commands have been modified
-  ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest)
@@ -143,9 +143,9 @@ TEST_F(JointGroupVelocityControllerTest, WrongCommandCheckTest)
     controller_interface::return_type::ERROR);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest)
@@ -160,9 +160,9 @@ TEST_F(JointGroupVelocityControllerTest, NoCommandCheckTest)
     controller_interface::return_type::OK);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 }
 
 TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest)
@@ -171,9 +171,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest)
   controller_->get_node()->set_parameter({"joints", joint_names_});
 
   // default values
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   auto node_state = controller_->get_node()->configure();
   ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
@@ -204,9 +204,9 @@ TEST_F(JointGroupVelocityControllerTest, CommandCallbackTest)
     controller_interface::return_type::OK);
 
   // check command in handle was set
-  ASSERT_EQ(joint_1_cmd_.get_value(), 10.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 20.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 30.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 10.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 20.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 30.0);
 }
 
 TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest)
@@ -218,15 +218,15 @@ TEST_F(JointGroupVelocityControllerTest, StopJointsOnDeactivateTest)
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // check joint commands are still the default ones
-  ASSERT_EQ(joint_1_cmd_.get_value(), 1.1);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 2.1);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 3.1);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 1.1);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 2.1);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 3.1);
 
   // stop the controller
   ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   // check joint commands are now zero
-  ASSERT_EQ(joint_1_cmd_.get_value(), 0.0);
-  ASSERT_EQ(joint_2_cmd_.get_value(), 0.0);
-  ASSERT_EQ(joint_3_cmd_.get_value(), 0.0);
+  ASSERT_EQ(joint_1_cmd_.get_optional().value(), 0.0);
+  ASSERT_EQ(joint_2_cmd_.get_optional().value(), 0.0);
+  ASSERT_EQ(joint_3_cmd_.get_optional().value(), 0.0);
 }