adaptations of controllers and tests not finished

diff_drive_controller/src/diff_drive_controller.cpp

@@ -151,9 +151,10 @@ controller_interface::return_type DiffDriveController::update(
     double right_feedback_mean = 0.0;
     for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
     {
-      const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value();
+      const double left_feedback =
+        registered_left_wheel_handles_[index].feedback.get().get_value<double>();
       const double right_feedback =
-        registered_right_wheel_handles_[index].feedback.get().get_value();
+        registered_right_wheel_handles_[index].feedback.get().get_value<double>();
 
       if (std::isnan(left_feedback) || std::isnan(right_feedback))
       {

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.hpp

@@ -57,12 +57,24 @@ class ForceTorqueSensorBroadcasterTest : public ::testing::Test
   const std::string frame_id_ = "fts_sensor_frame";
   std::array<double, 6> sensor_values_ = {1.1, 2.2, 3.3, 4.4, 5.5, 6.6};
 
-  hardware_interface::StateInterface fts_force_x_{sensor_name_, "force.x", &sensor_values_[0]};
-  hardware_interface::StateInterface fts_force_y_{sensor_name_, "force.y", &sensor_values_[1]};
-  hardware_interface::StateInterface fts_force_z_{sensor_name_, "force.z", &sensor_values_[2]};
-  hardware_interface::StateInterface fts_torque_x_{sensor_name_, "torque.x", &sensor_values_[3]};
-  hardware_interface::StateInterface fts_torque_y_{sensor_name_, "torque.y", &sensor_values_[4]};
-  hardware_interface::StateInterface fts_torque_z_{sensor_name_, "torque.z", &sensor_values_[5]};
+  hardware_interface::StateInterface fts_force_x_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("force.x", "double", std::to_string(sensor_values_[0])))};
+  hardware_interface::StateInterface fts_force_y_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("force.y", "double", std::to_string(sensor_values_[1])))};
+  hardware_interface::StateInterface fts_force_z_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("force.z", "double", std::to_string(sensor_values_[2])))};
+  hardware_interface::StateInterface fts_torque_x_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("torque.x", "double", std::to_string(sensor_values_[3])))};
+  hardware_interface::StateInterface fts_torque_y_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("torque.y", "double", std::to_string(sensor_values_[4])))};
+  hardware_interface::StateInterface fts_torque_z_{hardware_interface::InterfaceDescription(
+    sensor_name_,
+    hardware_interface::InterfaceInfo("torque.z", "double", std::to_string(sensor_values_[5])))};
 
   std::unique_ptr<FriendForceTorqueSensorBroadcaster> fts_broadcaster_;
 

gripper_controllers/include/gripper_controllers/gripper_action_controller_impl.hpp

@@ -65,8 +65,8 @@ 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 double current_position = joint_position_state_interface_->get().get_value<double>();
+  const double current_velocity = joint_velocity_state_interface_->get().get_value<double>();
 
   const double error_position = command_struct_rt_.position_ - current_position;
   const double error_velocity = -current_velocity;
@@ -147,7 +147,7 @@ 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();
+  command_struct_.position_ = joint_position_state_interface_->get().get_value<double>();
   command_struct_.max_effort_ = params_.max_effort;
   command_.writeFromNonRT(command_struct_);
 }
@@ -292,7 +292,7 @@ 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();
+  command_struct_.position_ = joint_position_state_interface_->get().get_value<double>();
   command_struct_.max_effort_ = params_.max_effort;
   command_.initRT(command_struct_);
 

joint_state_broadcaster/src/joint_state_broadcaster.cpp

@@ -338,10 +338,10 @@ controller_interface::return_type JointStateBroadcaster::update(
       interface_name = map_interface_to_joint_state_[interface_name];
     }
     name_if_value_mapping_[state_interface.get_prefix_name()][interface_name] =
-      state_interface.get_value();
+      state_interface.get_value<double>();
     RCLCPP_DEBUG(
       get_node()->get_logger(), "%s: %f\n", state_interface.get_name().c_str(),
-      state_interface.get_value());
+      state_interface.get_value<double>());
   }
 
   if (realtime_joint_state_publisher_ && realtime_joint_state_publisher_->trylock())

joint_state_broadcaster/test/test_joint_state_broadcaster.hpp

@@ -23,6 +23,7 @@
 
 #include "joint_state_broadcaster/joint_state_broadcaster.hpp"
 
+#include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
 using hardware_interface::HW_IF_EFFORT;
@@ -82,27 +83,37 @@ class JointStateBroadcasterTest : public ::testing::Test
   std::vector<double> joint_values_ = {1.1, 2.1, 3.1};
   double custom_joint_value_ = 3.5;
 
-  hardware_interface::StateInterface joint_1_pos_state_{
-    joint_names_[0], interface_names_[0], &joint_values_[0]};
-  hardware_interface::StateInterface joint_2_pos_state_{
-    joint_names_[1], interface_names_[0], &joint_values_[1]};
-  hardware_interface::StateInterface joint_3_pos_state_{
-    joint_names_[2], interface_names_[0], &joint_values_[2]};
-  hardware_interface::StateInterface joint_1_vel_state_{
-    joint_names_[0], interface_names_[1], &joint_values_[0]};
-  hardware_interface::StateInterface joint_2_vel_state_{
-    joint_names_[1], interface_names_[1], &joint_values_[1]};
-  hardware_interface::StateInterface joint_3_vel_state_{
-    joint_names_[2], interface_names_[1], &joint_values_[2]};
-  hardware_interface::StateInterface joint_1_eff_state_{
-    joint_names_[0], interface_names_[2], &joint_values_[0]};
-  hardware_interface::StateInterface joint_2_eff_state_{
-    joint_names_[1], interface_names_[2], &joint_values_[1]};
-  hardware_interface::StateInterface joint_3_eff_state_{
-    joint_names_[2], interface_names_[2], &joint_values_[2]};
-
-  hardware_interface::StateInterface joint_X_custom_state{
-    joint_names_[0], custom_interface_name_, &custom_joint_value_};
+  hardware_interface::StateInterface joint_1_pos_state_{hardware_interface::InterfaceDescription(
+    joint_names_[0], hardware_interface::InterfaceInfo(
+                       interface_names_[0], "double", std::to_string(joint_values_[0])))};
+  hardware_interface::StateInterface joint_2_pos_state_{hardware_interface::InterfaceDescription(
+    joint_names_[1], hardware_interface::InterfaceInfo(
+                       interface_names_[0], "double", std::to_string(joint_values_[1])))};
+  hardware_interface::StateInterface joint_3_pos_state_{hardware_interface::InterfaceDescription(
+    joint_names_[2], hardware_interface::InterfaceInfo(
+                       interface_names_[0], "double", std::to_string(joint_values_[2])))};
+  hardware_interface::StateInterface joint_1_vel_state_{hardware_interface::InterfaceDescription(
+    joint_names_[0], hardware_interface::InterfaceInfo(
+                       interface_names_[1], "double", std::to_string(joint_values_[0])))};
+  hardware_interface::StateInterface joint_2_vel_state_{hardware_interface::InterfaceDescription(
+    joint_names_[1], hardware_interface::InterfaceInfo(
+                       interface_names_[1], "double", std::to_string(joint_values_[1])))};
+  hardware_interface::StateInterface joint_3_vel_state_{hardware_interface::InterfaceDescription(
+    joint_names_[2], hardware_interface::InterfaceInfo(
+                       interface_names_[1], "double", std::to_string(joint_values_[2])))};
+  hardware_interface::StateInterface joint_1_eff_state_{hardware_interface::InterfaceDescription(
+    joint_names_[0], hardware_interface::InterfaceInfo(
+                       interface_names_[2], "double", std::to_string(joint_values_[0])))};
+  hardware_interface::StateInterface joint_2_eff_state_{hardware_interface::InterfaceDescription(
+    joint_names_[1], hardware_interface::InterfaceInfo(
+                       interface_names_[2], "double", std::to_string(joint_values_[1])))};
+  hardware_interface::StateInterface joint_3_eff_state_{hardware_interface::InterfaceDescription(
+    joint_names_[2], hardware_interface::InterfaceInfo(
+                       interface_names_[2], "double", std::to_string(joint_values_[2])))};
+
+  hardware_interface::StateInterface joint_X_custom_state{hardware_interface::InterfaceDescription(
+    joint_names_[0], hardware_interface::InterfaceInfo(
+                       custom_interface_name_, "double", std::to_string(custom_joint_value_)))};
 
   std::unique_ptr<FriendJointStateBroadcaster> state_broadcaster_;
 };

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -414,7 +414,7 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
   {
     for (size_t index = 0; index < dof_; ++index)
     {
-      trajectory_point_interface[index] = joint_interface[index].get().get_value();
+      trajectory_point_interface[index] = joint_interface[index].get().get_value<double><double>();
     }
   };
 
@@ -453,15 +453,15 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
   {
     for (size_t index = 0; index < dof_; ++index)
     {
-      trajectory_point_interface[index] = joint_interface[index].get().get_value();
+      trajectory_point_interface[index] = joint_interface[index].get().get_value<double>();
     }
   };
 
   auto interface_has_values = [](const auto & joint_interface)
   {
     return std::find_if(
              joint_interface.begin(), joint_interface.end(), [](const auto & interface)
-             { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
+             { return std::isnan(interface.get().get_value<double>()); }) == joint_interface.end();
   };
 
   // Assign values from the command interfaces as state. Therefore needs check for both.
@@ -523,15 +523,15 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
   {
     for (size_t index = 0; index < dof_; ++index)
     {
-      trajectory_point_interface[index] = joint_interface[index].get().get_value();
+      trajectory_point_interface[index] = joint_interface[index].get().get_value<double>();
     }
   };
 
   auto interface_has_values = [](const auto & joint_interface)
   {
     return std::find_if(
              joint_interface.begin(), joint_interface.end(), [](const auto & interface)
-             { return std::isnan(interface.get().get_value()); }) == joint_interface.end();
+             { return std::isnan(interface.get().get_value<double>()); }) == joint_interface.end();
   };
 
   // Assign values from the command interfaces as command.
@@ -983,7 +983,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
     if (has_position_command_interface_)
     {
       joint_command_interface_[0][index].get().set_value(
-        joint_command_interface_[0][index].get().get_value());
+        joint_command_interface_[0][index].get().get_value<double>());
     }
 
     if (has_velocity_command_interface_)
@@ -1248,15 +1248,15 @@ void JointTrajectoryController::fill_partial_goal(
         {
           if (
             has_position_command_interface_ &&
-            !std::isnan(joint_command_interface_[0][index].get().get_value()))
+            !std::isnan(joint_command_interface_[0][index].get().get_value<double>()))
           {
             // copy last command if cmd interface exists
-            it.positions.push_back(joint_command_interface_[0][index].get().get_value());
+            it.positions.push_back(joint_command_interface_[0][index].get().get_value<double>());
           }
           else if (has_position_state_interface_)
           {
             // copy current state if state interface exists
-            it.positions.push_back(joint_state_interface_[0][index].get().get_value());
+            it.positions.push_back(joint_state_interface_[0][index].get().get_value<double>());
           }
         }
         if (!it.velocities.empty())

parallel_gripper_controller/include/parallel_gripper_controller/parallel_gripper_action_controller_impl.hpp

@@ -59,8 +59,8 @@ controller_interface::return_type GripperActionController::update(
 {
   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 double current_position = joint_position_state_interface_->get().get_value<double>();
+  const double current_velocity = joint_velocity_state_interface_->get().get_value<double>();
   const double error_position = command_struct_rt_.position_cmd_ - current_position;
 
   check_for_success(get_node()->now(), error_position, current_position, current_velocity);
@@ -168,7 +168,7 @@ rclcpp_action::CancelResponse GripperActionController::cancel_callback(
 
 void GripperActionController::set_hold_position()
 {
-  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
+  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value<double>();
   command_struct_.max_effort_ = params_.max_effort;
   command_struct_.max_velocity_ = params_.max_velocity;
   command_.writeFromNonRT(command_struct_);
@@ -323,7 +323,7 @@ controller_interface::CallbackReturn GripperActionController::on_activate(
   }
 
   // Command - non RT version
-  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value();
+  command_struct_.position_cmd_ = joint_position_state_interface_->get().get_value<double>();
   command_struct_.max_effort_ = params_.max_effort;
   command_struct_.max_velocity_ = params_.max_velocity;
   command_.initRT(command_struct_);

parallel_gripper_controller/test/test_parallel_gripper_controller.hpp

@@ -51,15 +51,16 @@ class GripperControllerTest : public ::testing::Test
 
   // dummy joint state values used for tests
   const std::string joint_name_ = "joint1";
-  std::vector<double> joint_states_ = {1.1, 2.1};
-  std::vector<double> joint_commands_ = {3.1};
 
-  hardware_interface::StateInterface joint_1_pos_state_{
-    joint_name_, hardware_interface::HW_IF_POSITION, &joint_states_[0]};
-  hardware_interface::StateInterface joint_1_vel_state_{
-    joint_name_, hardware_interface::HW_IF_VELOCITY, &joint_states_[1]};
-  hardware_interface::CommandInterface joint_1_cmd_{
-    joint_name_, hardware_interface::HW_IF_POSITION, &joint_commands_[0]};
+  hardware_interface::StateInterface joint_1_pos_state_{hardware_interface::InterfaceDescription(
+    joint_name_,
+    hardware_interface::InterfaceInfo(hardware_interface::HW_IF_POSITION, "double", "1.1"))};
+  hardware_interface::StateInterface joint_1_vel_state_{hardware_interface::InterfaceDescription(
+    joint_name_,
+    hardware_interface::InterfaceInfo(hardware_interface::HW_IF_VELOCITY, "double", "2.1"))};
+  hardware_interface::CommandInterface joint_1_cmd_{hardware_interface::InterfaceDescription(
+    joint_name_,
+    hardware_interface::InterfaceInfo(hardware_interface::HW_IF_POSITION, "double", "3.1"))};
 };
 
 }  // anonymous namespace

pid_controller/include/pid_controller/pid_controller.hpp

@@ -114,8 +114,12 @@ class PidController : public controller_interface::ChainableControllerInterface
   rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
   std::unique_ptr<ControllerStatePublisher> state_publisher_;
 
+  std::vector<hardware_interface::InterfaceDescription> export_state_interface_descriptions()
+    override;
+
   // override methods from ChainableControllerInterface
-  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+  std::vector<hardware_interface::InterfaceDescription> export_reference_interface_descriptions()
+    override;
 
   bool on_set_chained_mode(bool chained_mode) override;
 

pid_controller/src/pid_controller.cpp

@@ -346,24 +346,31 @@ controller_interface::InterfaceConfiguration PidController::state_interface_conf
   return state_interfaces_config;
 }
 
-std::vector<hardware_interface::CommandInterface> PidController::on_export_reference_interfaces()
+std::vector<hardware_interface::InterfaceDescription>
+PidController::export_state_interface_descriptions()
 {
-  reference_interfaces_.resize(
-    dof_ * params_.reference_and_state_interfaces.size(), std::numeric_limits<double>::quiet_NaN());
+  // does not export any StateInterfaces
+  return {};
+}
+
+std::vector<hardware_interface::InterfaceDescription>
+PidController::export_reference_interface_descriptions()
+{
+  reference_interfaces_.reserve(dof_ * params_.reference_and_state_interfaces.size());
 
-  std::vector<hardware_interface::CommandInterface> reference_interfaces;
-  reference_interfaces.reserve(reference_interfaces_.size());
+  std::vector<hardware_interface::InterfaceDescription> reference_interfaces_descr;
+  reference_interfaces_descr.reserve(reference_interfaces_.size());
 
   for (const auto & interface : params_.reference_and_state_interfaces)
   {
     for (const auto & dof_name : reference_and_state_dof_names_)
     {
-      reference_interfaces.push_back(hardware_interface::CommandInterface(InterfaceDescription(
-        get_node()->get_name(), InterfaceInfo(dof_name + "/" + interface, "double"))));
+      reference_interfaces_descr.emplace_back(
+        (get_node()->get_name(), InterfaceInfo(dof_name + "/" + interface, "double")));
     }
   }
 
-  return reference_interfaces;
+  return reference_interfaces_descr;
 }
 
 bool PidController::on_set_chained_mode(bool chained_mode)