Merge pull request #186 from lbr-stack/dev-humble-state-interface-fix

closes #181
lbr-stack · May 31, 2024 · 77f6979 · 77f6979
2 parents 0f47943 + e9e8271
commit 77f6979
Show file tree

Hide file tree

Showing 10 changed files with 51 additions and 10 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -15,7 +15,7 @@ jobs:
     strategy:
       matrix:
         os: [ubuntu-latest]
-        fri_version: [1.11, 1.14, 1.15, 1.16, 2.5]
+        fri_version: [1.11, 1.14, 1.15, 1.16, 2.5, 2.7]
     steps:
       - uses: ros-tooling/[email protected]
       - uses: ros-tooling/[email protected]

diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/formatting.hpp
@@ -61,7 +61,7 @@ struct EnumMaps {
     case KUKA::FRI::EClientCommandMode::POSITION:
       return "POSITION";
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
     case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
       return "JOINT_POSITION";
     case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:

diff --git a/lbr_fri_ros2/src/async_client.cpp b/lbr_fri_ros2/src/async_client.cpp
@@ -21,7 +21,7 @@ AsyncClient::AsyncClient(const KUKA::FRI::EClientCommandMode &client_command_mod
 #if FRICLIENT_VERSION_MAJOR == 1
   case KUKA::FRI::EClientCommandMode::POSITION:
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
   case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
 #endif
   {

diff --git a/lbr_fri_ros2/src/interfaces/position_command.cpp b/lbr_fri_ros2/src/interfaces/position_command.cpp
@@ -19,7 +19,7 @@ void PositionCommandInterface::buffered_command_to_fri(fri_command_t_ref command
     throw std::runtime_error(err);
   }
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
   if (state.client_command_mode != KUKA::FRI::EClientCommandMode::JOINT_POSITION) {
     std::string err =
         "Expected robot in " +

diff --git a/lbr_fri_ros2/test/test_command_interfaces.cpp b/lbr_fri_ros2/test/test_command_interfaces.cpp
@@ -36,7 +36,7 @@ class TestCommandInterfaces : public ::testing::Test {
 #if FRICLIENT_VERSION_MAJOR == 1
     case KUKA::FRI::EClientCommandMode::POSITION:
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
     case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
 #endif
     {
@@ -146,7 +146,7 @@ TEST_F(TestCommandInterfaces, TestPositionCommandInterface) {
 #if FRICLIENT_VERSION_MAJOR == 1
   set_up(KUKA::FRI::EClientCommandMode::POSITION);
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
   set_up(KUKA::FRI::EClientCommandMode::JOINT_POSITION);
 #endif
   test_simple();

diff --git a/lbr_fri_ros2_stack/repos-fri-2.7.yaml b/lbr_fri_ros2_stack/repos-fri-2.7.yaml
@@ -0,0 +1,13 @@
+repositories:
+  fri:
+    type: git
+    url: https://github.com/lbr-stack/fri
+    version: fri-2.7
+  lbr_fri_idl:
+    type: git
+    url: https://github.com/lbr-stack/lbr_fri_idl
+    version: fri-2
+  lbr_fri_ros2_stack:
+    type: git
+    url: https://github.com/lbr-stack/lbr_fri_ros2_stack
+    version: humble
diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro
@@ -16,6 +16,8 @@
             <xacro:unless value="${sim}">
                 <hardware>
                     <plugin>lbr_ros2_control::SystemInterface</plugin>
+                    <param name="fri_client_sdk_major_version">${system_parameters['hardware']['fri_client_sdk']['major_version']}</param>
+                    <param name="fri_client_sdk_minor_version">${system_parameters['hardware']['fri_client_sdk']['minor_version']}</param>
                     <param name="client_command_mode">${system_parameters['hardware']['client_command_mode']}</param>
                     <param name="port_id">${system_parameters['hardware']['port_id']}</param>
                     <param name="remote_host">${system_parameters['hardware']['remote_host']}</param>
@@ -103,7 +105,9 @@
                     <state_interface name="velocity" />
                     <state_interface name="effort" />
                     <xacro:unless value="${sim}">
-                        <state_interface name="commanded_joint_position" />
+                        <xacro:if value="${system_parameters['hardware']['fri_client_sdk']['major_version'] == 1}">
+                            <state_interface name="commanded_joint_position" />
+                        </xacro:if>
                         <state_interface name="commanded_torque" />
                         <state_interface name="external_torque" />
                         <state_interface name="ipo_joint_position" />

diff --git a/lbr_ros2_control/config/lbr_system_parameters.yaml b/lbr_ros2_control/config/lbr_system_parameters.yaml
@@ -1,5 +1,8 @@
 # these parameters are read by the lbr_system_interface.xacro and configure the lbr_ros2_control::SystemInterface
 hardware:
+  fri_client_sdk: # the fri_client_sdk version is used to create the correct state interfaces lbr_system_interface.xacro
+    major_version: 1
+    minor_version: 15
   client_command_mode: position # the command mode specifies the user-sent commands. Available: [position, torque, wrench]
   port_id: 30200 # port id for the UDP communication. Useful in multi-robot setups
   remote_host: INADDR_ANY # the expected robot IP address. INADDR_ANY will accept any incoming connection
@@ -11,7 +14,7 @@ hardware:
   pid_i_min: 0.0 # min integral value for the joint position command
   pid_antiwindup: false # enable antiwindup for the joint position command
   command_guard_variant: default # if requested position / velocities beyond limits, CommandGuard will be triggered and shut the connection. Available: [default, safe_stop]
-  external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz] 
+  external_torque_cutoff_frequency: 10 # low-pass filter for the external joint torque measurements [Hz]
   measured_torque_cutoff_frequency: 10 # low-pass filter for the joint torque measurements [Hz]
   open_loop: true # KUKA works the best in open_loop control mode
 

diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp
@@ -31,10 +31,12 @@
 
 namespace lbr_ros2_control {
 struct SystemInterfaceParameters {
+  uint8_t fri_client_sdk_major_version{1};
+  uint8_t fri_client_sdk_minor_version{15};
 #if FRICLIENT_VERSION_MAJOR == 1
   KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::POSITION};
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
   KUKA::FRI::EClientCommandMode client_command_mode{KUKA::FRI::EClientCommandMode::JOINT_POSITION};
 #endif
   int32_t port_id{30200};
@@ -68,7 +70,12 @@ class SystemInterface : public hardware_interface::SystemInterface {
 protected:
   static constexpr char LOGGER_NAME[] = "lbr_ros2_control::SystemInterface";
 
+#if FRICLIENT_VERSION_MAJOR == 1
   static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 7;
+#endif
+#if FRICLIENT_VERSION_MAJOR >= 2
+  static constexpr uint8_t LBR_FRI_STATE_INTERFACE_SIZE = 6;
+#endif
   static constexpr uint8_t LBR_FRI_COMMAND_INTERFACE_SIZE = 2;
   static constexpr uint8_t LBR_FRI_SENSORS = 2;
   static constexpr uint8_t AUXILIARY_SENSOR_SIZE = 12;

diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp
@@ -320,12 +320,26 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
 
 bool SystemInterface::parse_parameters_(const hardware_interface::HardwareInfo &system_info) {
   try {
+    parameters_.fri_client_sdk_major_version =
+        std::stoul(system_info.hardware_parameters.at("fri_client_sdk_major_version"));
+    parameters_.fri_client_sdk_minor_version =
+        std::stoul(system_info.hardware_parameters.at("fri_client_sdk_minor_version"));
+    if (parameters_.fri_client_sdk_major_version != FRICLIENT_VERSION_MAJOR) {
+      RCLCPP_ERROR_STREAM(
+          rclcpp::get_logger(LOGGER_NAME),
+          lbr_fri_ros2::ColorScheme::ERROR
+              << "Expected FRI client SDK version '" << FRICLIENT_VERSION_MAJOR << "', got '"
+              << std::to_string(parameters_.fri_client_sdk_major_version)
+              << "'. Update lbr_system_parameters.yaml or compile against correct FRI version."
+              << lbr_fri_ros2::ColorScheme::ENDC);
+      return false;
+    }
     std::string client_command_mode = system_info.hardware_parameters.at("client_command_mode");
     if (client_command_mode == "position") {
 #if FRICLIENT_VERSION_MAJOR == 1
       parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::POSITION;
 #endif
-#if FRICLIENT_VERSION_MAJOR == 2
+#if FRICLIENT_VERSION_MAJOR >= 2
       parameters_.client_command_mode = KUKA::FRI::EClientCommandMode::JOINT_POSITION;
 #endif
     } else if (client_command_mode == "torque") {