Merge branch 'humble' into dev-humble-launch

lbr-stack · Jun 9, 2024 · 14365a6 · 14365a6
2 parents 674c5d5 + 18db6ff
commit 14365a6
Show file tree

Hide file tree

Showing 17 changed files with 70 additions and 22 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -4,6 +4,7 @@ on:
   pull_request:
     branches:
       - humble
+  workflow_dispatch:
   schedule:
     - cron: "0 0 * * 0"
 
@@ -14,12 +15,12 @@ jobs:
     runs-on: ${{ matrix.os }}
     strategy:
       matrix:
-        os: [ubuntu-latest]
-        fri_version: [1.11, 1.14, 1.15, 1.16, 2.5]
+        os: [ubuntu-22.04]
+        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]
         with:
           package-name: lbr_fri_ros2_stack
           target-ros2-distro: humble
-          vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/dev-humble-launch/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml
+          vcs-repo-file-url: https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos-fri-${{ matrix.fri_version }}.yaml
diff --git a/README.md b/README.md
@@ -85,10 +85,16 @@ If you enjoyed using this repository for your work, we would really appreciate
 
 ## Acknowledgements
 ### Open Source Contributors
-We would like to acknowledge all open source contributors 🚀
+We would like to acknowledge all contributors 🚀
+
+**lbr_fri_ros2_stack**
 
 [![lbr_fri_ros2_stack contributors](https://contrib.rocks/image?repo=lbr-stack/lbr_fri_ros2_stack&max=20)](https://github.com/lbr-stack/lbr_fri_ros2_stack/graphs/contributors)
 
+**fri**
+
+[![fri contributors](https://contrib.rocks/image?repo=lbr-stack/fri&max=20)](https://github.com/lbr-stack/fri/graphs/contributors)
+
 ### Organizations and Grants
 We would further like to acknowledge following supporters:
 

diff --git a/lbr_demos/lbr_demos_advanced_cpp/package.xml b/lbr_demos/lbr_demos_advanced_cpp/package.xml
@@ -11,7 +11,7 @@
 
   <exec_depend>lbr_description</exec_depend>
 
-  <depend>FRIClient</depend>
+  <depend>fri_client_sdk</depend>
   <depend>geometry_msgs</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_idl</depend>

diff --git a/lbr_demos/lbr_demos_cpp/package.xml b/lbr_demos/lbr_demos_cpp/package.xml
@@ -10,7 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>control_msgs</depend>
-  <depend>FRIClient</depend>
+  <depend>fri_client_sdk</depend>
   <depend>lbr_fri_idl</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>rclcpp</depend>

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/package.xml b/lbr_fri_ros2/package.xml
@@ -14,7 +14,7 @@
   <build_depend>eigen</build_depend>
 
   <depend>control_toolbox</depend>
-  <depend>FRIClient</depend>
+  <depend>fri_client_sdk</depend>
   <depend>kdl_parser</depend>
   <depend>lbr_fri_idl</depend>
   <depend>orocos_kdl_vendor</depend>

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/command_guard.cpp b/lbr_fri_ros2/src/command_guard.cpp
@@ -2,7 +2,7 @@
 
 namespace lbr_fri_ros2 {
 CommandGuard::CommandGuard(const CommandGuardParameters &command_guard_parameters)
-    : parameters_(command_guard_parameters){};
+    : parameters_(command_guard_parameters), prev_measured_joint_position_init_(false){};
 
 bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command,
                                     const_idl_state_t_ref lbr_state) {

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/package.xml b/lbr_fri_ros2_stack/package.xml
@@ -8,13 +8,13 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>lbr_fri_idl</depend>
+  <depend>lbr_fri_ros2</depend>
+  <depend>lbr_ros2_control</depend>
 
   <exec_depend>lbr_bringup</exec_depend>
-  <exec_depend>lbr_demos</exec_depend>
   <exec_depend>lbr_description</exec_depend>
-  <exec_depend>lbr_fri_idl</exec_depend>
-  <exec_depend>lbr_fri_ros2</exec_depend>
-  <exec_depend>lbr_ros2_control</exec_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

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/package.xml b/lbr_ros2_control/package.xml
@@ -9,7 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>FRIClient</depend>
+  <depend>fri_client_sdk</depend>
   <depend>lbr_fri_idl</depend>
   <depend>lbr_fri_ros2</depend>
   <depend>pluginlib</depend>

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") {