Add new get_value API for Handles and Interfaces (#1976)

---------

Co-authored-by: Dr. Denis <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>

controller_interface/include/semantic_components/force_torque_sensor.hpp

@@ -96,7 +96,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     {
       if (existing_axes_[i])
       {
-        forces[i] = state_interfaces_[interface_counter].get().get_value();
+        forces[i] = state_interfaces_[interface_counter].get().get_value<double>().value();
         ++interface_counter;
       }
     }
@@ -123,7 +123,7 @@ class ForceTorqueSensor : public SemanticComponentInterface<geometry_msgs::msg::
     {
       if (existing_axes_[i + FORCES_SIZE])
       {
-        torques[i] = state_interfaces_[torque_interface_counter].get().get_value();
+        torques[i] = state_interfaces_[torque_interface_counter].get().get_value<double>().value();
         ++torque_interface_counter;
       }
     }

controller_interface/include/semantic_components/imu_sensor.hpp

@@ -52,7 +52,7 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     std::array<double, 4> orientation;
     for (auto i = 0u; i < orientation.size(); ++i)
     {
-      orientation[i] = state_interfaces_[i].get().get_value();
+      orientation[i] = state_interfaces_[i].get().get_value<double>().value();
     }
     return orientation;
   }
@@ -69,7 +69,8 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     const std::size_t interface_offset{4};
     for (auto i = 0u; i < angular_velocity.size(); ++i)
     {
-      angular_velocity[i] = state_interfaces_[interface_offset + i].get().get_value();
+      angular_velocity[i] =
+        state_interfaces_[interface_offset + i].get().get_value<double>().value();
     }
     return angular_velocity;
   }
@@ -86,7 +87,8 @@ class IMUSensor : public SemanticComponentInterface<sensor_msgs::msg::Imu>
     const std::size_t interface_offset{7};
     for (auto i = 0u; i < linear_acceleration.size(); ++i)
     {
-      linear_acceleration[i] = state_interfaces_[interface_offset + i].get().get_value();
+      linear_acceleration[i] =
+        state_interfaces_[interface_offset + i].get().get_value<double>().value();
     }
     return linear_acceleration;
   }

controller_interface/include/semantic_components/pose_sensor.hpp

@@ -50,7 +50,7 @@ class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
     std::array<double, 3> position;
     for (auto i = 0u; i < position.size(); ++i)
     {
-      position[i] = state_interfaces_[i].get().get_value();
+      position[i] = state_interfaces_[i].get().get_value<double>().value();
     }
     return position;
   }
@@ -67,7 +67,7 @@ class PoseSensor : public SemanticComponentInterface<geometry_msgs::msg::Pose>
     const std::size_t interface_offset{3};
     for (auto i = 0u; i < orientation.size(); ++i)
     {
-      orientation[i] = state_interfaces_[interface_offset + i].get().get_value();
+      orientation[i] = state_interfaces_[interface_offset + i].get().get_value<double>().value();
     }
     return orientation;
   }

controller_interface/include/semantic_components/range_sensor.hpp

@@ -35,7 +35,7 @@ class RangeSensor : public SemanticComponentInterface<sensor_msgs::msg::Range>
    *
    * \return value of the range in meters
    */
-  float get_range() const { return state_interfaces_[0].get().get_value(); }
+  float get_range() const { return state_interfaces_[0].get().get_value<double>().value(); }
 
   /// Return Range message with range in meters
   /**

controller_interface/include/semantic_components/semantic_component_interface.hpp

@@ -93,7 +93,7 @@ class SemanticComponentInterface
     // insert all the values
     for (auto i = 0u; i < state_interfaces_.size(); ++i)
     {
-      values.emplace_back(state_interfaces_[i].get().get_value());
+      values.emplace_back(state_interfaces_[i].get().get_value<double>().value());
     }
     return true;
   }

controller_interface/test/test_chainable_controller_interface.cpp

@@ -52,7 +52,8 @@ TEST_F(ChainableControllerInterfaceTest, export_state_interfaces)
   EXPECT_EQ(exported_state_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
   EXPECT_EQ(exported_state_interfaces[0]->get_interface_name(), "test_state");
 
-  EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE);
+  EXPECT_EQ(
+    exported_state_interfaces[0]->get_value<double>().value(), EXPORTED_STATE_INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
@@ -72,7 +73,7 @@ TEST_F(ChainableControllerInterfaceTest, export_reference_interfaces)
   EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), TEST_CONTROLLER_NAME);
   EXPECT_EQ(reference_interfaces[0]->get_interface_name(), "test_itf");
 
-  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value<double>().value(), INTERFACE_VALUE);
 }
 
 TEST_F(ChainableControllerInterfaceTest, interfaces_prefix_is_not_node_name)
@@ -120,7 +121,7 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // Fail setting chained mode
-  EXPECT_EQ(reference_interfaces[0]->get_value(), INTERFACE_VALUE);
+  EXPECT_EQ(reference_interfaces[0]->get_value<double>().value(), INTERFACE_VALUE);
 
   EXPECT_FALSE(controller.set_chained_mode(true));
   EXPECT_FALSE(controller.is_in_chained_mode());
@@ -129,11 +130,13 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
   EXPECT_FALSE(controller.is_in_chained_mode());
 
   // Success setting chained mode
-  reference_interfaces[0]->set_value(0.0);
+  (void)reference_interfaces[0]->set_value(0.0);
 
   EXPECT_TRUE(controller.set_chained_mode(true));
   EXPECT_TRUE(controller.is_in_chained_mode());
-  EXPECT_EQ(exported_state_interfaces[0]->get_value(), EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
+  EXPECT_EQ(
+    exported_state_interfaces[0]->get_value<double>().value(),
+    EXPORTED_STATE_INTERFACE_VALUE_IN_CHAINMODE);
 
   controller.configure();
   EXPECT_TRUE(controller.set_chained_mode(false));

controller_manager/test/test_chainable_controller/test_chainable_controller.cpp

@@ -101,21 +101,23 @@ controller_interface::return_type TestChainableController::update_and_write_comm
 
   for (size_t i = 0; i < command_interfaces_.size(); ++i)
   {
-    command_interfaces_[i].set_value(reference_interfaces_[i] - state_interfaces_[i].get_value());
+    (void)command_interfaces_[i].set_value(
+      reference_interfaces_[i] - state_interfaces_[i].get_value<double>().value());
   }
   // If there is a command interface then integrate and set it to the exported state interface data
   for (size_t i = 0; i < exported_state_interface_names_.size() && i < command_interfaces_.size();
        ++i)
   {
-    state_interfaces_values_[i] = command_interfaces_[i].get_value() * CONTROLLER_DT;
+    state_interfaces_values_[i] =
+      command_interfaces_[i].get_value<double>().value() * CONTROLLER_DT;
   }
   // If there is no command interface and if there is a state interface then just forward the same
   // value as in the state interface
   for (size_t i = 0; i < exported_state_interface_names_.size() && i < state_interfaces_.size() &&
                      command_interfaces_.empty();
        ++i)
   {
-    state_interfaces_values_[i] = state_interfaces_[i].get_value();
+    state_interfaces_values_[i] = state_interfaces_[i].get_value<double>().value();
   }
 
   return controller_interface::return_type::OK;
@@ -240,7 +242,7 @@ std::vector<double> TestChainableController::get_state_interface_data() const
   std::vector<double> state_intr_data;
   for (const auto & interface : state_interfaces_)
   {
-    state_intr_data.push_back(interface.get_value());
+    state_intr_data.push_back(interface.get_value<double>().value());
   }
   return state_intr_data;
 }

controller_manager/test/test_controller/test_controller.cpp

@@ -71,7 +71,7 @@ controller_interface::return_type TestController::update(
   // set value to hardware to produce and test different behaviors there
   if (!std::isnan(set_first_command_interface_value_to))
   {
-    command_interfaces_[0].set_value(set_first_command_interface_value_to);
+    (void)command_interfaces_[0].set_value(set_first_command_interface_value_to);
     // reset to be easier to test
     set_first_command_interface_value_to = std::numeric_limits<double>::quiet_NaN();
   }
@@ -90,7 +90,7 @@ controller_interface::return_type TestController::update(
       RCLCPP_DEBUG(
         get_node()->get_logger(), "Setting value of command interface '%s' to %f",
         command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]);
-      command_interfaces_[i].set_value(external_commands_for_testing_[i]);
+      (void)command_interfaces_[i].set_value(external_commands_for_testing_[i]);
     }
   }
 
@@ -187,7 +187,7 @@ std::vector<double> TestController::get_state_interface_data() const
   std::vector<double> state_intr_data;
   for (const auto & interface : state_interfaces_)
   {
-    state_intr_data.push_back(interface.get_value());
+    state_intr_data.push_back(interface.get_value<double>().value());
   }
   return state_intr_data;
 }

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -520,8 +520,12 @@ class TestControllerChainingWithControllerManager
     // Command of DiffDrive controller are references of PID controllers
     EXP_LEFT_WHEEL_REF = chained_ctrl_calculation(reference[0], EXP_LEFT_WHEEL_HW_STATE);
     EXP_RIGHT_WHEEL_REF = chained_ctrl_calculation(reference[1], EXP_RIGHT_WHEEL_HW_STATE);
-    ASSERT_EQ(diff_drive_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_REF);
-    ASSERT_EQ(diff_drive_controller->command_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_REF);
+    ASSERT_EQ(
+      diff_drive_controller->command_interfaces_[0].get_value<double>().value(),
+      EXP_LEFT_WHEEL_REF);
+    ASSERT_EQ(
+      diff_drive_controller->command_interfaces_[1].get_value<double>().value(),
+      EXP_RIGHT_WHEEL_REF);
     ASSERT_EQ(pid_left_wheel_controller->reference_interfaces_[0], EXP_LEFT_WHEEL_REF);
     ASSERT_EQ(pid_right_wheel_controller->reference_interfaces_[0], EXP_RIGHT_WHEEL_REF);
 
@@ -537,21 +541,32 @@ class TestControllerChainingWithControllerManager
 
     EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
     EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
-    ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
-    ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+    ASSERT_EQ(
+      pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+      EXP_LEFT_WHEEL_CMD);
+    ASSERT_EQ(
+      pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+      EXP_LEFT_WHEEL_HW_STATE);
     // DiffDrive uses the same state
-    ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+    ASSERT_EQ(
+      diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
+      EXP_LEFT_WHEEL_HW_STATE);
     // The state doesn't change wrt to any data from the hardware calculation
     ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
     ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
 
     EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
     EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
-    ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
     ASSERT_EQ(
-      pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+      pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+      EXP_RIGHT_WHEEL_CMD);
+    ASSERT_EQ(
+      pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+      EXP_RIGHT_WHEEL_HW_STATE);
     // DiffDrive uses the same state
-    ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+    ASSERT_EQ(
+      diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
+      EXP_RIGHT_WHEEL_HW_STATE);
     // The state doesn't change wrt to any data from the hardware calculation
     ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
     ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
@@ -811,10 +826,16 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers)
   EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
   // 32 / 2
   EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
-  ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
-  ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_CMD);
+  ASSERT_EQ(
+    pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_HW_STATE);
   // DiffDrive uses the same state
-  ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_HW_STATE);
   // The state doesn't change wrt to any data from the hardware calculation
   ASSERT_EQ(robot_localization_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
   ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[0], EXP_STATE_ODOM_X);
@@ -823,13 +844,19 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers)
   EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
   // 128 / 2
   EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
-  ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
-  ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_CMD);
+  ASSERT_EQ(
+    pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_HW_STATE);
   ASSERT_EQ(odom_publisher_controller->internal_counter, 2u);
   ASSERT_EQ(sensor_fusion_controller->internal_counter, 6u);
   ASSERT_EQ(robot_localization_controller->internal_counter, 4u);
   // DiffDrive uses the same state
-  ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_HW_STATE);
   // The state doesn't change wrt to any data from the hardware calculation
   ASSERT_EQ(robot_localization_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
   ASSERT_EQ(odom_publisher_controller->get_state_interface_data()[1], EXP_STATE_ODOM_Y);
@@ -1986,19 +2013,31 @@ TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_add
   EXP_LEFT_WHEEL_CMD = chained_ctrl_calculation(EXP_LEFT_WHEEL_REF, EXP_LEFT_WHEEL_HW_STATE);
   // 32 / 2
   EXP_LEFT_WHEEL_HW_STATE = hardware_calculation(EXP_LEFT_WHEEL_CMD);
-  ASSERT_EQ(pid_left_wheel_controller->command_interfaces_[0].get_value(), EXP_LEFT_WHEEL_CMD);
-  ASSERT_EQ(pid_left_wheel_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    pid_left_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_CMD);
+  ASSERT_EQ(
+    pid_left_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_HW_STATE);
   // DiffDrive uses the same state
-  ASSERT_EQ(diff_drive_controller->state_interfaces_[0].get_value(), EXP_LEFT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    diff_drive_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_LEFT_WHEEL_HW_STATE);
 
   // 128 - 0
   EXP_RIGHT_WHEEL_CMD = chained_ctrl_calculation(EXP_RIGHT_WHEEL_REF, EXP_RIGHT_WHEEL_HW_STATE);
   // 128 / 2
   EXP_RIGHT_WHEEL_HW_STATE = hardware_calculation(EXP_RIGHT_WHEEL_CMD);
-  ASSERT_EQ(pid_right_wheel_controller->command_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_CMD);
-  ASSERT_EQ(pid_right_wheel_controller->state_interfaces_[0].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    pid_right_wheel_controller->command_interfaces_[0].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_CMD);
+  ASSERT_EQ(
+    pid_right_wheel_controller->state_interfaces_[0].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_HW_STATE);
   // DiffDrive uses the same state
-  ASSERT_EQ(diff_drive_controller->state_interfaces_[1].get_value(), EXP_RIGHT_WHEEL_HW_STATE);
+  ASSERT_EQ(
+    diff_drive_controller->state_interfaces_[1].get_value<double>().value(),
+    EXP_RIGHT_WHEEL_HW_STATE);
 
   // update all controllers at once and see that all have expected values --> also checks the order
   // of controller execution

doc/migration.rst

@@ -75,12 +75,36 @@ hardware_interface
 ******************
 * ``test_components`` was moved to its own package. Update the dependencies if you are using them. (`#1325 <https://github.com/ros-controls/ros2_control/pull/1325>`_)
 * With (`#1683 <https://github.com/ros-controls/ros2_control/pull/1683>`_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 <https://github.com/ros-controls/ros2_control/pull/1240>`_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles.
+* A new ``get_value`` that returns a ``std::optional`` was added to the ``CommandInterface`` and ``StateInterface``. This can be used to check if the value is available or not. (`#1976 <https://github.com/ros-controls/ros2_control/pull/1976>`_)
 
 Adaption of Command-/StateInterfaces
 ***************************************
 
+* The handles for ``Command-/StateInterfaces`` have new set/get methods to access the values.
 * ``Command-/StateInterfaces`` are now created and exported automatically by the framework via the ``on_export_command_interfaces()`` or ``on_export_state_interfaces()`` methods based on the interfaces defined in the ``ros2_control`` XML-tag, which get parsed and the ``InterfaceDescription`` is created accordingly (check the `hardware_info.hpp <https://github.com/ros-controls/ros2_control/tree/{REPOS_FILE_BRANCH}/hardware_interface/include/hardware_interface/hardware_info.hpp>`__). The memory is now allocated in the handle itself.
 
+Access to Command-/StateInterfaces
+----------------------------------
+
+Earlier code will issue compile-time warnings like:
+
+.. code::
+
+  warning: ‘double hardware_interface::Handle::get_value() const’ is deprecated: Use std::optional<T> get_value() or bool get_value(double & value) instead to retrieve the value. [-Wdeprecated-declarations]
+  warning: ignoring return value of ‘bool hardware_interface::Handle::set_value(const T&) [with T = double]’ [-Wunused-result]
+
+The old methods are deprecated and will be removed in the future. The new methods are:
+
+  * ``std::optional<T> get_value()`` or ``bool get_value(T & value)`` for getting the value.
+  * ``bool set_value(const T & value)`` for setting the value.
+
+The return value ``bool`` or ``std::optional<T>`` with ``get_value`` can be used to check if the value is available or not. Similarly, the ``set_value`` method returns a ``bool`` to check if the value was set or not.
+The ``get_value`` method will return an empty ``std::nullopt`` or ``false`` if the value is not available. The ``set_value`` method will return ``false`` if the value was not set.
+
+.. note::
+   Checking the result of these operations is important as the value might not be available or the value might not be set.
+   This is usually the case when the ros2_control framework has some asynchronous operations due to asynchronous controllers or asynchronous hardware components where different threads are involved to access the same data.
+
 Migration of Command-/StateInterfaces
 -------------------------------------
 To adapt to the new way of creating and exporting ``Command-/StateInterfaces`` follow those steps: