use get_value<double>() instead of get_value()

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());
@@ -133,7 +134,9 @@ TEST_F(ChainableControllerInterfaceTest, setting_chained_mode)
 
   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());
+    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

@@ -178,7 +178,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