FRI 2.x support

README.md

@@ -26,24 +26,35 @@ ROS 2 packages for the KUKA LBR, including communication to the real robot via t
 Full documentation available [here](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/index.html).
 
 ## Quick Start
-Install [colcon](https://docs.ros.org/en/humble/Tutorials/Colcon-Tutorial.html#install-colcon), [rosdep](https://docs.ros.org/en/humble/Installation/Alternatives/Ubuntu-Install-Binary.html#installing-and-initializing-rosdep) and [vcstool](https://github.com/dirk-thomas/vcstool#how-to-install-vcstool). Build this repository
+1. Install ROS 2 development tools
+    ```shell
+    sudo apt install ros-dev-tools
+    ```
 
-```shell
-mkdir -p lbr-stack/src && cd lbr-stack
-wget https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml -P src
-vcs import src < src/repos.yaml
-rosdep install --from-paths src --ignore-src -r -y
-colcon build --symlink-install
-```
-Next, launch the simulation via
-```shell
-source install/setup.bash
-ros2 launch lbr_bringup bringup.launch.py \
-    model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
-    sim:=true # [true, false] \
-    rviz:=true # [true, false] \
-    moveit:=true # [true, false]
-```
+2. Create a workspace, clone, and install dependencies
+    ```shell
+    mkdir -p lbr-stack/src && cd lbr-stack
+    vcs import src --input https://raw.githubusercontent.com/lbr-stack/lbr_fri_ros2_stack/humble/lbr_fri_ros2_stack/repos.yaml
+    rosdep install --from-paths src --ignore-src -r -y
+    ```
+
+3. Build
+    ```shell
+    colcon build --symlink-install --cmake-args -DFRI_CLIENT_VERSION=1.15 --no-warn-unused-cli # replace by your FRI client version
+    ```
+
+> [!NOTE]
+> FRI client is added as external CMake project via [fri_vendor](https://github.com/lbr-stack/fri_vendor) and must be available as branch, refer [README](https://github.com/lbr-stack/fri?tab=readme-ov-file#contributing).
+
+4. Launch the simulation via
+    ```shell
+    source install/setup.bash
+    ros2 launch lbr_bringup bringup.launch.py \
+        model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
+        sim:=true # [true, false] \
+        rviz:=true # [true, false] \
+        moveit:=true # [true, false]
+    ```
 
 Now, run the [demos](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/lbr_fri_ros2_stack/lbr_demos/doc/lbr_demos.html). To get started with the real robot, checkout the [Documentation](https://lbr-fri-ros2-stack-doc.readthedocs.io/en/humble/index.html) above.
 

lbr_fri_ros2/include/lbr_fri_ros2/async_client.hpp

@@ -9,6 +9,7 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_ros2/command_interface.hpp"
 #include "lbr_fri_ros2/enum_maps.hpp"

lbr_fri_ros2/include/lbr_fri_ros2/command_guard.hpp

@@ -11,6 +11,7 @@
 
 #include "friLBRClient.h"
 #include "friLBRState.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 

lbr_fri_ros2/include/lbr_fri_ros2/command_interface.hpp

@@ -10,9 +10,11 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_ros2/command_guard.hpp"
+#include "lbr_fri_ros2/enum_maps.hpp"
 #include "lbr_fri_ros2/filters.hpp"
 
 namespace lbr_fri_ros2 {

lbr_fri_ros2/include/lbr_fri_ros2/enum_maps.hpp

@@ -4,6 +4,7 @@
 #include <string>
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 namespace lbr_fri_ros2 {
 struct EnumMaps {
@@ -43,8 +44,16 @@ struct EnumMaps {
     switch (client_command_mode) {
     case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE:
       return "NO_COMMAND_MODE";
+#if FRICLIENT_VERSION_MAJOR == 1
     case KUKA::FRI::EClientCommandMode::POSITION:
       return "POSITION";
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+      return "JOINT_POSITION";
+    case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:
+      return "CARTESIAN_POSE";
+#endif
     case KUKA::FRI::EClientCommandMode::TORQUE:
       return "TORQUE";
     case KUKA::FRI::EClientCommandMode::WRENCH:

lbr_fri_ros2/include/lbr_fri_ros2/state_interface.hpp

@@ -7,6 +7,7 @@
 #include "rclcpp/logging.hpp"
 
 #include "friLBRClient.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_ros2/filters.hpp"

lbr_fri_ros2/src/app_component.cpp

@@ -144,7 +144,12 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host,
 
     // subscriptions
     switch (async_client_ptr_->get_state_interface().get_state().client_command_mode) {
+#if FRICLIENT_VERSION_MAJOR == 1
     case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
       position_command_sub_ =
           app_node_ptr_->create_subscription<lbr_fri_msgs::msg::LBRPositionCommand>(
               "command/joint_position", 1,
@@ -169,9 +174,15 @@ void AppComponent::connect_(const int &port_id, const char *const remote_host,
 
 void AppComponent::on_position_command_(
     const lbr_fri_msgs::msg::LBRPositionCommand::SharedPtr lbr_position_command) {
-  if (!on_command_checks_(KUKA::FRI::EClientCommandMode::POSITION)) {
-    return;
-  }
+#if FRICLIENT_VERSION_MAJOR == 1
+  if (!on_command_checks_(KUKA::FRI::EClientCommandMode::POSITION))
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    if (!on_command_checks_(KUKA::FRI::EClientCommandMode::JOINT_POSITION))
+#endif
+    {
+      return;
+    }
 
   if (async_client_ptr_->get_state_interface().get_state().session_state ==
       KUKA::FRI::ESessionState::COMMANDING_ACTIVE) {

lbr_fri_ros2/src/app_component.hpp

@@ -8,6 +8,8 @@
 #include "rclcpp/rclcpp.hpp"
 #include "urdf/model.h"
 
+#include "friVersion.h"
+
 #include "lbr_fri_msgs/msg/lbr_position_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"
 #include "lbr_fri_msgs/msg/lbr_torque_command.hpp"

lbr_fri_ros2/src/async_client.cpp

@@ -49,20 +49,39 @@ void AsyncClient::command() {
   }
 
   switch (robotState().getClientCommandMode()) {
+#if FRICLIENT_VERSION_MAJOR == 1
   case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
+  {
     command_interface_.get_joint_position_command(robotCommand(), robotState());
     return;
-  case KUKA::FRI::EClientCommandMode::TORQUE:
+  }
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE: {
+    std::string err =
+        EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) +
+        " command mode not supported yet.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+#endif
+  case KUKA::FRI::EClientCommandMode::TORQUE: {
     command_interface_.get_torque_command(robotCommand(), robotState());
     return;
-  case KUKA::FRI::EClientCommandMode::WRENCH:
+  }
+  case KUKA::FRI::EClientCommandMode::WRENCH: {
     command_interface_.get_wrench_command(robotCommand(), robotState());
     return;
-  default:
+  }
+  default: {
     std::string err =
         "Unsupported command mode: " + std::to_string(robotState().getClientCommandMode()) + ".";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
+  }
 }
 } // end of namespace lbr_fri_ros2

lbr_fri_ros2/src/command_guard.cpp

@@ -9,14 +9,23 @@ bool CommandGuard::is_valid_command(const_idl_command_t_ref lbr_command,
   switch (lbr_state.getClientCommandMode()) {
   case KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE:
     return false;
+#if FRICLIENT_VERSION_MAJOR == 1
   case KUKA::FRI::EClientCommandMode::POSITION:
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::JOINT_POSITION:
+#endif
     if (!command_in_position_limits_(lbr_command, lbr_state)) {
       return false;
     }
     if (!command_in_velocity_limits_(lbr_command, lbr_state)) {
       return false;
     }
     return true;
+#if FRICLIENT_VERSION_MAJOR == 2
+  case KUKA::FRI::EClientCommandMode::CARTESIAN_POSE:
+    return false;
+#endif
   case KUKA::FRI::EClientCommandMode::WRENCH:
     if (!command_in_position_limits_(lbr_command, lbr_state)) {
       return false;

lbr_fri_ros2/src/command_interface.cpp

@@ -11,11 +11,25 @@ CommandInterface::CommandInterface(const PIDParameters &pid_parameters,
 
 void CommandInterface::get_joint_position_command(fri_command_t_ref command,
                                                   const_fri_state_t_ref state) {
+#if FRICLIENT_VERSION_MAJOR == 1
   if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::POSITION) {
-    std::string err = "Set joint position only allowed in position command mode.";
+    std::string err = "Set joint position only allowed in " +
+                      EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::POSITION) +
+                      " command mode.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
     throw std::runtime_error(err);
   }
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+  if (state.getClientCommandMode() != KUKA::FRI::EClientCommandMode::JOINT_POSITION) {
+    std::string err =
+        "Set joint position only allowed in " +
+        EnumMaps::client_command_mode_map(KUKA::FRI::EClientCommandMode::JOINT_POSITION) +
+        " command mode.";
+    RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());
+    throw std::runtime_error(err);
+  }
+#endif
   if (!command_guard_) {
     std::string err = "Uninitialized command guard.";
     RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str());

lbr_fri_ros2/src/state_interface.cpp

@@ -6,8 +6,10 @@ StateInterface::StateInterface(const StateInterfaceParameters &state_interface_p
 
 void StateInterface::set_state(const_fri_state_t_ref state) {
   state_.client_command_mode = state.getClientCommandMode();
+#if FRICLIENT_VERSION_MAJOR == 1
   std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
+#endif
   std::memcpy(state_.commanded_torque.data(), state.getCommandedTorque(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
   state_.connection_quality = state.getConnectionQuality();
@@ -41,8 +43,10 @@ void StateInterface::set_state(const_fri_state_t_ref state) {
 void StateInterface::set_state_open_loop(const_fri_state_t_ref state,
                                          const_idl_joint_pos_t_ref joint_position) {
   state_.client_command_mode = state.getClientCommandMode();
+#if FRICLIENT_VERSION_MAJOR == 1
   std::memcpy(state_.commanded_joint_position.data(), state.getCommandedJointPosition(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
+#endif
   std::memcpy(state_.commanded_torque.data(), state.getCommandedTorque(),
               sizeof(double) * fri_state_t::NUMBER_OF_JOINTS);
   state_.connection_quality = state.getConnectionQuality();

lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp

@@ -16,6 +16,7 @@
 #include "rclcpp_lifecycle/state.hpp"
 
 #include "friLBRState.h"
+#include "friVersion.h"
 
 #include "lbr_fri_msgs/msg/lbr_command.hpp"
 #include "lbr_fri_msgs/msg/lbr_state.hpp"

lbr_ros2_control/src/system_interface.cpp

@@ -272,14 +272,21 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
   if (hw_session_state_ != KUKA::FRI::COMMANDING_ACTIVE) {
     return hardware_interface::return_type::OK;
   }
-  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION) {
-    if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
-                    [](const double &v) { return std::isnan(v); })) {
+#if FRICLIENT_VERSION_MAJOR == 1
+  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::POSITION)
+#endif
+#if FRICLIENT_VERSION_MAJOR == 2
+    if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::JOINT_POSITION)
+#endif
+    {
+      if (std::any_of(hw_lbr_command_.joint_position.cbegin(),
+                      hw_lbr_command_.joint_position.cend(),
+                      [](const double &v) { return std::isnan(v); })) {
+        return hardware_interface::return_type::OK;
+      }
+      async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
       return hardware_interface::return_type::OK;
     }
-    async_client_ptr_->get_command_interface().set_command_target(hw_lbr_command_);
-    return hardware_interface::return_type::OK;
-  }
   if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::TORQUE) {
     if (std::any_of(hw_lbr_command_.joint_position.cbegin(), hw_lbr_command_.joint_position.cend(),
                     [](const double &v) { return std::isnan(v); }) ||
@@ -291,8 +298,17 @@ hardware_interface::return_type SystemInterface::write(const rclcpp::Time & /*ti
     return hardware_interface::return_type::OK;
   }
   if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::WRENCH) {
-    throw std::runtime_error("Wrench command mode currently not implemented.");
+    throw std::runtime_error(
+        lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) +
+        " command mode currently not implemented.");
+  }
+#if FRICLIENT_VERSION_MAJOR == 2
+  if (hw_client_command_mode_ == KUKA::FRI::EClientCommandMode::CARTESIAN_POSE) {
+    throw std::runtime_error(
+        lbr_fri_ros2::EnumMaps::client_command_mode_map(hw_client_command_mode_) +
+        " command mode currently not implemented.");
   }
+#endif
   return hardware_interface::return_type::ERROR;
 }