Final variant support

example_10/controllers/gpio_controller.cpp

@@ -59,8 +59,8 @@ controller_interface::return_type GPIOController::update(
   {
     RCLCPP_DEBUG(
       get_node()->get_logger(), "%s: (%f)", state_interfaces_[i].get_name().c_str(),
-      state_interfaces_[i].get_value());
-    gpio_msg_.values.at(i) = static_cast<float>(state_interfaces_.at(i).get_value());
+      state_interfaces_[i].get_value<double>());
+    gpio_msg_.values.at(i) = static_cast<float>(state_interfaces_.at(i).get_value<double>());
   }
   gpio_publisher_->publish(gpio_msg_);
 
@@ -84,7 +84,7 @@ controller_interface::return_type GPIOController::update(
     command_interfaces_[i].set_value(output_cmd_ptr_->data[i]);
     RCLCPP_DEBUG(
       get_node()->get_logger(), "%s: (%f)", command_interfaces_[i].get_name().c_str(),
-      command_interfaces_[i].get_value());
+      command_interfaces_[i].get_value<double>());
   }
 
   return controller_interface::return_type::OK;

example_10/description/ros2_control/rrbot.ros2_control.xacro

@@ -21,28 +21,45 @@
 
       <joint name="${prefix}joint1">
         <command_interface name="position">
+          <param name="data_type">double</param>
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="data_type">double</param>
+        </state_interface>
       </joint>
       <joint name="${prefix}joint2">
         <command_interface name="position">
+          <param name="data_type">double</param>
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="data_type">double</param>
+        </state_interface>
       </joint>
       <!-- from https://github.com/ros-controls/ros2_control/blob/f58d4072dbd1c3fd4e1f64cd10a19a2b60f59049/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp#L344 -->
       <gpio name="flange_analog_IOs">
-        <command_interface name="analog_output1"/>
-        <state_interface name="analog_output1"/> <!-- Needed to know current state of the output -->
-        <state_interface name="analog_input1"/>
-        <state_interface name="analog_input2"/>
+        <command_interface name="analog_output1">
+          <param name="data_type">double</param>
+        </command_interface>
+        <state_interface name="analog_output1"> <!-- Needed to know current state of the output -->
+          <param name="data_type">double</param>
+        </state_interface>
+        <state_interface name="analog_input1">
+          <param name="data_type">double</param>
+        </state_interface>
+        <state_interface name="analog_input2">
+          <param name="data_type">double</param>
+        </state_interface>
       </gpio>
       <gpio name="flange_vacuum">
-        <command_interface name="vacuum"/>
+        <command_interface name="vacuum">
+          <param name="data_type">double</param>
+        </command_interface>
         <state_interface name="vacuum">
+          <param name="data_type">double</param>
           <param name="initial_value">1.0</param>
         </state_interface>
       </gpio>

example_10/hardware/include/ros2_control_demo_example_10/rrbot.hpp

@@ -68,11 +68,31 @@ class RRBotSystemWithGPIOHardware : public hardware_interface::SystemInterface
 private:
   // Parameters for the RRBot simulation
 
-  // Store the command and state interfaces for the simulated robot
-  std::vector<double> hw_commands_;
-  std::vector<double> hw_states_;
-  std::vector<double> hw_gpio_in_;
-  std::vector<double> hw_gpio_out_;
+  struct FlangeVacuum
+  {
+    explicit FlangeVacuum(const std::string & gpio_name) : name(gpio_name) {}
+    // delete move constructor since would throw because of const std::string members
+    // but we dont want to move this anyways so const for member is ok i guess
+    FlangeVacuum(FlangeVacuum && other) = delete;
+
+    const std::string name;
+    const std::string vacuum = name + "/vacuum";
+  };
+
+  struct FlangeIOs
+  {
+    explicit FlangeIOs(const std::string & gpio_name) : name(gpio_name) {}
+    // delete move constructor since would throw because of const std::string members
+    // but we dont want to move this anyways so const for member is ok i guess
+    FlangeIOs(FlangeIOs && other) = delete;
+
+    const std::string name;
+    const std::string out = name + "/analog_output1";
+    const std::string input_1 = name + "/analog_input1";
+    const std::string input_2 = name + "/analog_input2";
+  };
+  std::unique_ptr<FlangeVacuum> flange_vacuum_;
+  std::unique_ptr<FlangeIOs> flange_ios_;
 };
 
 }  // namespace ros2_control_demo_example_10

example_10/hardware/rrbot.cpp

@@ -35,9 +35,6 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
     return hardware_interface::CallbackReturn::ERROR;
   }
 
-  hw_states_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
-  hw_commands_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
-
   for (const hardware_interface::ComponentInfo & joint : info_.joints)
   {
     // RRBotSystemPositionOnly has exactly one state and command interface on each joint
@@ -77,6 +74,27 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
       return hardware_interface::CallbackReturn::ERROR;
     }
   }
+  // check if we have same command and state interfaces for joint this makes iterating easier
+  // first check if size is equal then we only need to iterate over one of them
+  if (joint_state_interfaces_.size() != joint_command_interfaces_.size())
+  {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("RRBotSystemWithSensorHardware"),
+      "Expect joint CommandInterface and joint StateInterfaces to be of equal size.");
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  for (const auto & [state_itf_name, state_itf_descr] : joint_state_interfaces_)
+  {
+    if (joint_command_interfaces_.find(state_itf_name) == joint_command_interfaces_.end())
+    {
+      RCLCPP_FATAL(
+        rclcpp::get_logger("RRBotSystemWithSensorHardware"),
+        "Expect joint CommandInterface and joint StateInterfaces to be equal but StateInterface "
+        "includes<%s> which is not included in CommandInterfaces.",
+        state_itf_name.c_str());
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+  }
 
   // RRBotSystemWithGPIOHardware has exactly two GPIO components
   if (info_.gpios.size() != 2)
@@ -116,6 +134,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_init(
       info_.gpios[0].state_interfaces.size(), 1);
     return hardware_interface::CallbackReturn::ERROR;
   }
+  // set gpios
+  flange_ios_ = std::make_unique<FlangeIOs>(info_.gpios[0].name);
+  flange_vacuum_ = std::make_unique<FlangeVacuum>(info_.gpios[1].name);
 
   return hardware_interface::CallbackReturn::SUCCESS;
 }
@@ -128,69 +149,23 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_configure(
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // reset values always when configuring hardware
-  std::fill(hw_states_.begin(), hw_states_.end(), 0);
-  std::fill(hw_commands_.begin(), hw_commands_.end(), 0);
-  std::fill(hw_gpio_in_.begin(), hw_gpio_in_.end(), 0);
-  std::fill(hw_gpio_out_.begin(), hw_gpio_out_.end(), 0);
-
-  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!");
-
-  return hardware_interface::CallbackReturn::SUCCESS;
-}
-
-std::vector<hardware_interface::StateInterface>
-RRBotSystemWithGPIOHardware::export_state_interfaces()
-{
-  std::vector<hardware_interface::StateInterface> state_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
-  {
-    state_interfaces.emplace_back(hardware_interface::StateInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_[i]));
-  }
-
-  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "State interfaces:");
-  hw_gpio_in_.resize(4);
-  size_t ct = 0;
-  for (size_t i = 0; i < info_.gpios.size(); i++)
+  for (const auto & [itf_name, itf_descr] : joint_state_interfaces_)
   {
-    for (auto state_if : info_.gpios.at(i).state_interfaces)
-    {
-      state_interfaces.emplace_back(hardware_interface::StateInterface(
-        info_.gpios.at(i).name, state_if.name, &hw_gpio_in_[ct++]));
-      RCLCPP_INFO(
-        rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s",
-        info_.gpios.at(i).name.c_str(), state_if.name.c_str());
-    }
+    set_state(itf_name, 0.0);
+    set_command(itf_name, 0.0);
   }
-
-  return state_interfaces;
-}
-
-std::vector<hardware_interface::CommandInterface>
-RRBotSystemWithGPIOHardware::export_command_interfaces()
-{
-  std::vector<hardware_interface::CommandInterface> command_interfaces;
-  for (uint i = 0; i < info_.joints.size(); i++)
+  for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_)
   {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i]));
+    set_state(gpio_state, 0.0);
   }
-  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Command interfaces:");
-  hw_gpio_out_.resize(2);
-  size_t ct = 0;
-  for (size_t i = 0; i < info_.gpios.size(); i++)
+  for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_)
   {
-    for (auto command_if : info_.gpios.at(i).command_interfaces)
-    {
-      command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        info_.gpios.at(i).name, command_if.name, &hw_gpio_out_[ct++]));
-      RCLCPP_INFO(
-        rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Added %s/%s",
-        info_.gpios.at(i).name.c_str(), command_if.name.c_str());
-    }
+    set_command(gpio_cmd, 0.0);
   }
 
-  return command_interfaces;
+  RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully configured!");
+
+  return hardware_interface::CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate(
@@ -201,9 +176,9 @@ hardware_interface::CallbackReturn RRBotSystemWithGPIOHardware::on_activate(
   // END: This part here is for exemplary purposes - Please do not copy to your production code
 
   // command and state should be equal when starting
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (const auto & [itf_name, itf_descr] : joint_state_interfaces_)
   {
-    hw_commands_[i] = hw_states_[i];
+    set_command(itf_name, get_state(itf_name));
   }
 
   RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Successfully activated!");
@@ -227,26 +202,26 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::read(
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Reading...");
 
-  for (uint i = 0; i < hw_states_.size(); i++)
+  for (const auto & [itf_name, itf_descr] : joint_state_interfaces_)
   {
     // Simulate RRBot's movement
-    hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]);
+    set_state(itf_name, get_state(itf_name) + (get_command(itf_name) - get_state(itf_name)));
   }
 
   // mirror GPIOs back
-  hw_gpio_in_[0] = hw_gpio_out_[0];
-  hw_gpio_in_[3] = hw_gpio_out_[1];
+  set_state(flange_ios_->out, get_command(flange_ios_->out));
+  set_state(flange_vacuum_->vacuum, get_command(flange_vacuum_->vacuum));
   // random inputs
   unsigned int seed = time(NULL) + 1;
-  hw_gpio_in_[1] = static_cast<float>(rand_r(&seed));
+  set_state(flange_ios_->input_1, static_cast<float>(rand_r(&seed)));
   seed = time(NULL) + 2;
-  hw_gpio_in_[2] = static_cast<float>(rand_r(&seed));
+  set_state(flange_ios_->input_2, static_cast<float>(rand_r(&seed)));
 
-  for (uint i = 0; i < hw_gpio_in_.size(); i++)
+  for (const auto & [gpio_state, gpio_state_desc] : gpio_state_interfaces_)
   {
     RCLCPP_INFO(
-      rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %d!",
-      hw_gpio_in_[i], i);
+      rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Read %.1f from GP input %s!",
+      get_state(gpio_state), gpio_state.c_str());
   }
   RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully read!");
   // END: This part here is for exemplary purposes - Please do not copy to your production code
@@ -260,11 +235,11 @@ hardware_interface::return_type RRBotSystemWithGPIOHardware::write(
   // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code
   RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Writing...");
 
-  for (uint i = 0; i < hw_gpio_out_.size(); i++)
+  for (const auto & [gpio_cmd, gpio_cmd_desc] : gpio_command_interfaces_)
   {
     RCLCPP_INFO(
-      rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %d!",
-      hw_gpio_out_[i], i);
+      rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "Got command %.1f for GP output %s!",
+      get_command(gpio_cmd), gpio_cmd.c_str());
   }
   RCLCPP_INFO(rclcpp::get_logger("RRBotSystemWithGPIOHardware"), "GPIOs successfully written!");
   // END: This part here is for exemplary purposes - Please do not copy to your production code

example_14/description/ros2_control/rrbot_modular_actuators_without_feedback_sensors_for_position_feedback.ros2_control.xacro

@@ -17,7 +17,9 @@
         <param name="example_param_socket_port">23286</param>
       </hardware>
       <joint name="joint1">
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="data_type">double</param>
+        </state_interface>
       </joint>
     </ros2_control>
     <ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
@@ -29,7 +31,9 @@
         <param name="example_param_socket_port">23287</param>
       </hardware>
       <joint name="joint2">
-        <state_interface name="position"/>
+        <state_interface name="position">
+          <param name="data_type">double</param>
+        </state_interface>
       </joint>
       </ros2_control>
 
@@ -42,6 +46,7 @@
       </hardware>
       <joint name="joint1">
         <command_interface name="velocity">
+          <param name="data_type">double</param>
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>
@@ -56,6 +61,7 @@
       </hardware>
       <joint name="joint2">
         <command_interface name="velocity">
+          <param name="data_type">double</param>
           <param name="min">-1</param>
           <param name="max">1</param>
         </command_interface>

example_14/hardware/include/ros2_control_demo_example_14/rrbot_actuator_without_feedback.hpp

@@ -74,9 +74,7 @@ class RRBotActuatorWithoutFeedback : public hardware_interface::ActuatorInterfac
   // Parameters for the RRBot simulation
   double hw_start_sec_;
   double hw_stop_sec_;
-
-  // Store the command for the simulated robot
-  double hw_joint_command_;
+  std::string vel_itf_;
 
   // Fake "mechanical connection" between actuator and sensor using sockets
   struct sockaddr_in address_;

example_14/hardware/include/ros2_control_demo_example_14/rrbot_sensor_for_position_feedback.hpp

@@ -80,7 +80,7 @@ class RRBotSensorPositionFeedback : public hardware_interface::SensorInterface
   // Store the command for the simulated robot
   double measured_velocity;  // Local variable, but avoid initialization on each read
   double last_measured_velocity_;
-  double hw_joint_state_;
+  std::string pos_itf_;
 
   // Timestamps to calculate position for velocity
   rclcpp::Clock clock_;